13,446,392 members (35,221 online)

#### Stats

46.5K views
39 bookmarked
Posted 2 Oct 2012

# Kalman Filtering: Part 2

, 9 Oct 2012
A simple example of Kalman filtering and merging data.
 ```﻿/////////////////////////////////////////////////////////////////////////////// // // Kalman3D.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 3D. /// public class Kalman3D { #region Protected data. /// /// State. /// Matrix m_x = new Matrix(1, 3); /// /// Covariance. /// Matrix m_p = new Matrix(3, 3); /// /// Minimal covariance. /// Matrix m_q = new Matrix(3, 3); /// /// 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.Data[0]; } set { m_x.Data[0] = value; } } /// /// How fast the value is changing. /// public double Velocity { get { return m_x.Data[1]; } } /// /// Get the second derivative of the value. /// public double Acceleration { get { return m_x.Data[2]; } } /// /// The last kalman gain used, useful for debug. /// public double LastGain { get; protected set; } /// /// Last updated positional variance. /// /// public double Variance() { return m_p.Data[0]; } /// /// Predict the value forward from last measurement time by dt. /// X = F*X + H*U /// /// /// public double Predicition(double dt) { return m_x.Data[0] + (dt * m_x.Data[1]) + (dt * dt * m_x.Data[2]); } /// /// Get the estimated covariance of position predicted /// forward from last measurement time by dt. /// P = F*P*F^T + Q. /// /// /// public double Variance(double dt) { Matrix f = new Matrix(3, 3) { Data = new double[] { 1, dt, dt * dt, 0, 1, dt, 0, 0, 1} }; Matrix pPredicted = Matrix.MultiplyABAT(f, m_p); pPredicted.Add(m_q); return pPredicted.Data[0]; } /// /// Reset the filter. /// /// Measurement to position state minimal variance. /// Measurement to velocity 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 qa, double r, double pd, double ix) { m_q.Data[0] = qx * qx; m_q.Data[1] = qv * qx; m_q.Data[2] = qa * qx; m_q.Data[3] = qx * qv; m_q.Data[4] = qv * qv; m_q.Data[5] = qa * qv; m_q.Data[6] = qx * qa; m_q.Data[7] = qv * qa; m_q.Data[8] = qa * qa; m_r = r; // Diagonal to pd, others to zero. for (int i = 0; i < m_p.Data.Length; i++) { m_p.Data[i] = 0; } m_p.Data[0] = m_p.Data[4] = m_p.Data[8] = pd; // Initial position, velocity and acceleration. m_x.Data[0] = ix; m_x.Data[1] = 0; m_x.Data[2] = 0; } /// /// Update the state by measurement m at dt time from last measurement. /// Requires only X position, sythesizes velocity and acceleration /// as internal states. /// /// Measured position. /// Time delta. /// Updated position. public double Update(double mx, double dt) { double mv = dt != 0 ? (mx - m_x.Data[0]) / dt : 0; double ma = dt != 0 ? (mv - m_x.Data[1]) / dt : 0; return Update(mx, mv, ma, dt); } /// /// Update the state by measurement m at dt time from last measurement. /// Requires only X position and velocity, sythesizes acceleration /// as internal state. /// /// Measured position. /// Measured velocity. /// Time delta. /// Updated position. public double Update(double mx, double mv, double dt) { double ma = dt != 0 ? (mv - m_x.Data[1]) / dt : 0; return Update(mx, mv, ma, dt); } /// /// Update the state by measurement m at dt time from last measurement. /// /// Measured position. /// Measured velocity. /// Measured acceleration. /// Time delta. /// Updated position. public double Update(double mx, double mv, double ma, double dt) { // Predict to now, then update. // Predict: // X = F*X + H*U // P = F*P*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. // // Same as 1D but mv is used instead of delta m_x[0], and H = [1,1]. // X = F*X + H*U Matrix f = new Matrix(3, 3) { Data = new double[] { 1, dt, dt * dt, 0, 1, dt, 0, 0, 1} }; Matrix h = Matrix.MakeIdentity(3); Matrix ht = Matrix.MakeIdentity(3); // h is identity, so ht is also identity. // U = {0,0}; m_x = Matrix.Multiply(f, m_x); // P = F*P*F^T + Q m_p = Matrix.MultiplyABAT(f, m_p); m_p.Add(m_q); // Y = M – H*X Matrix y = new Matrix(1, 3) { Data = new double[] { mx - m_x.Data[0], mv - m_x.Data[1], ma - m_x.Data[2]} }; // S = H*P*H^T + R Matrix s = Matrix.MultiplyABAT(h, m_p); // R is added across the diagnal, reducing by 10% for velocity and accel., // which is often good for motion, and allows us // to work off of a single m_r value. s.Data[0] += m_r; s.Data[4] += m_r; s.Data[8] += m_r; // K = P * H^T *S^-1 Matrix tmp = Matrix.Multiply(m_p, ht); Matrix sinv = Matrix.Invert(s); Matrix k = new Matrix(3, 3); // inited to zero. if (sinv != null) { k = Matrix.Multiply(tmp, sinv); } // Measurements are unstable so make the gain always be between 0 and 1. for (int i = 0; i < k.Data.Length; i++) { double gain = k.Data[i]; gain = gain < 0 ? 0 : gain > 0.25 ? 0.25 : gain; k.Data[i] = gain; } LastGain = k.Determinant; // X = X + K*Y m_x.Add(Matrix.Multiply(k, y)); // P = (I – K * H) * P Matrix kh = Matrix.Multiply(k, h); Matrix id = Matrix.MakeIdentity(3); kh.Multiply(-1); id.Add(kh); id.Multiply(m_p); m_p.Set(id); // Return latest estimate. return m_x.Data[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.