Click here to Skip to main content
Click here to Skip to main content
Add your own
alternative version
Go to top

Kalman Filtering: Part 2

, 9 Oct 2012
A simple example of Kalman filtering and merging data.
///////////////////////////////////////////////////////////////////////////////
//
//  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
{
    /// <summary>
    /// Kalman 1D w/ velocity estimation.
    /// </summary>
    public class Kalman1D
    {
        #region Protected data.
        /// <summary>
        /// State.
        /// </summary>
        double [] m_x = new double[2];
        /// <summary>
        /// Covariance.
        /// </summary>
        double [] m_p = new double[4];

        /// <summary>
        /// Minimal covariance.
        /// </summary>
        double [] m_q = new double[4];

        /// <summary>
        /// Minimal innovative covariance, keeps filter from locking in to a solution.
        /// </summary>
        double m_r;
        #endregion

        /// <summary>
        /// The last updated value, can also be set if filter gets
        /// sudden absolute measurement data for the latest update.
        /// </summary>
        public double Value 
        {
            get { return m_x[0]; }
            set { m_x[0] = value; }
        }

        /// <summary>
        /// How fast the value is changing.
        /// </summary>
        public double Velocity
        {
            get { return m_x[1]; }
        }

        /// <summary>
        /// The last kalman gain used, useful for debug.
        /// </summary>
        public double LastGain { get; protected set; }

        /// <summary>
        /// Last updated positional variance.
        /// </summary>
        /// <returns></returns>
        public double Variance () { return m_p[0]; }   

        /// <summary>
        /// Predict the value forward from last measurement time by dt.
        /// X = F*X + H*U        
        /// </summary>
        /// <param name="dt"></param>
        /// <returns></returns>
        public double Predicition(double dt)
        {
            return m_x[0] + (dt * m_x[1]);
        }

        /// <summary>
        /// Get the estimated covariance of position predicted 
        /// forward from last measurement time by dt.
        /// P = F*X*F^T + Q.
        /// </summary>
        /// <param name="dt"></param>
        /// <returns></returns>
        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];
        }

        /// <summary>
        /// Reset the filter.
        /// </summary>
        /// <param name="qx">Measurement to position state minimal variance.</param>
        /// <param name="qv">Measurement to velocity state minimal variance.</param>
        /// <param name="r">Measurement covariance (sets minimal gain).</param>
        /// <param name="pd">Initial variance.</param>
        /// <param name="ix">Initial position.</param>
	    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;
 	    }

        /// <summary>
        /// Update the state by measurement m at dt time from last measurement.
        /// </summary>
        /// <param name="m"></param>
        /// <param name="dt"></param>
        /// <returns></returns>
        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];
        }
    }
}

By viewing downloads associated with this article you agree to the Terms of Service and the article's licence.

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.

License

This article, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)

Share

About the Author

HoshiKata
Software Developer (Senior) KMC Systems
United States United States
Phil is a Principal Software developer focusing on weird yet practical algorithms that run the gamut of embedded and desktop (PID loops, Kalman filters, FFTs, client-server SOAP bindings, ASIC design, communication protocols, game engines, robotics).
 
In his personal life he is a part time mad scientist, full time dad, and studies small circle jujitsu, plays guitar and piano.

| Advertise | Privacy | Mobile
Web03 | 2.8.140905.1 | Last Updated 9 Oct 2012
Article Copyright 2012 by HoshiKata
Everything else Copyright © CodeProject, 1999-2014
Terms of Service
Layout: fixed | fluid