alternative version

# Kalman Filtering: Part 2

, 9 Oct 2012 CPOL
A simple example of Kalman filtering and merging data.
 KalmanDemoCode.zip KalmanDemoCode Kalman filtering2.docx KalmanNew.jpg Properties Settings.settings ```﻿/////////////////////////////////////////////////////////////////////////////// // // Kalman1D.cs // // By Philip R. Braica (HoshiKata@aol.com, VeryMadSci@gmail.com) // // Distributed under the The Code Project Open License (CPOL) // http://www.codeproject.com/info/cpol10.aspx /////////////////////////////////////////////////////////////////////////////// // Using. using System; using System.Collections.Generic; using System.Linq; using System.Text; // Namespace. namespace KalmanDemo { /// /// Kalman 1D w/ velocity estimation. /// public class Kalman1D { #region Protected data. /// /// State. /// double [] m_x = new double[2]; /// /// Covariance. /// double [] m_p = new double[4]; /// /// Minimal covariance. /// double [] m_q = new double[4]; /// /// Minimal innovative covariance, keeps filter from locking in to a solution. /// double m_r; #endregion /// /// The last updated value, can also be set if filter gets /// sudden absolute measurement data for the latest update. /// public double Value { get { return m_x[0]; } set { m_x[0] = value; } } /// /// How fast the value is changing. /// public double Velocity { get { return m_x[1]; } } /// /// The last kalman gain used, useful for debug. /// public double LastGain { get; protected set; } /// /// Last updated positional variance. /// /// public double Variance () { return m_p[0]; } /// /// Predict the value forward from last measurement time by dt. /// X = F*X + H*U /// /// /// public double Predicition(double dt) { return m_x[0] + (dt * m_x[1]); } /// /// Get the estimated covariance of position predicted /// forward from last measurement time by dt. /// P = F*X*F^T + Q. /// /// /// public double Variance(double dt) { return m_p[0] + dt * (m_p[2] + m_p[1]) + dt * dt * m_p[3] + m_q[0]; // Not needed. // m_p[1] = m_p[1] + dt * m_p[3] + m_q[1]; // m_p[2] = m_p[2] + dt * m_p[3] + m_q[2]; // m_p[3] = m_p[3] + m_q[3]; } /// /// Reset the filter. /// /// Measurement to position state minimal variance. /// Measurement to velocity state minimal variance. /// Measurement covariance (sets minimal gain). /// Initial variance. /// Initial position. public void Reset(double qx, double qv, double r, double pd, double ix) { m_q[0] = qx; m_q[1] = qv; m_r = r; m_p[0] = m_p[3] = pd; m_p[1] = m_p[2] = 0; m_x[0] = ix; m_x[1] = 0; } /// /// Update the state by measurement m at dt time from last measurement. /// /// /// /// public double Update(double m, double dt) { // Predict to now, then update. // Predict: // X = F*X + H*U // P = F*X*F^T + Q. // Update: // Y = M – H*X Called the innovation = measurement – state transformed by H. // S = H*P*H^T + R S= Residual covariance = covariane transformed by H + R // K = P * H^T *S^-1 K = Kalman gain = variance / residual covariance. // X = X + K*Y Update with gain the new measurement // P = (I – K * H) * P Update covariance to this time. // X = F*X + H*U double oldX = m_x[0]; m_x[0] = m_x[0] + (dt * m_x[1]); // P = F*X*F^T + Q m_p[0] = m_p[0] + dt * (m_p[2] + m_p[1]) + dt * dt * m_p[3] + m_q[0]; m_p[1] = m_p[1] + dt * m_p[3] + m_q[1]; m_p[2] = m_p[2] + dt * m_p[3] + m_q[2]; m_p[3] = m_p[3] + m_q[3]; // Y = M – H*X double y0 = m - m_x[0]; double y1 = ((m - oldX) / dt) - m_x[1]; // S = H*P*H^T + R // Because H = [1, 0] this is easy, and s is a single value not a matrix to invert. double s = m_p[0] + m_r; // K = P * H^T *S^-1 double k = m_p[0] / s; LastGain = k; // X = X + K*Y m_x[0] += y0 * k; m_x[1] += y1 * k; // P = (I – K * H) * P for (int i = 0; i < 4; i++) m_p[i] = m_p[i] - k * m_p[i]; // Return latest estimate. return m_x[0]; } } } ```

If a file you wish to view isn't highlighted, and is a text file (not binary), please let us know and we'll add colourisation support for it.