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.
///////////////////////////////////////////////////////////////////////////////
//
//  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
{
    /// <summary>
    /// Kalman 3D.
    /// </summary>
    public class Kalman3D
    {
        #region Protected data.
        /// <summary>
        /// State.
        /// </summary>
        Matrix m_x = new Matrix(1, 3);

        /// <summary>
        /// Covariance.
        /// </summary>
        Matrix m_p = new Matrix(3, 3);

        /// <summary>
        /// Minimal covariance.
        /// </summary>
        Matrix m_q = new Matrix(3, 3);

        /// <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.Data[0]; }
            set { m_x.Data[0] = value; }
        }

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

        /// <summary>
        /// Get the second derivative of the value.
        /// </summary>
        public double Acceleration
        {
            get { return m_x.Data[2]; }
        }

        /// <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.Data[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.Data[0] + (dt * m_x.Data[1]) + (dt * dt * m_x.Data[2]);
        }

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

        /// <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="qa">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 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;
        }

        /// <summary>
        /// Update the state by measurement m at dt time from last measurement.
        /// Requires only X position, sythesizes velocity and acceleration
        /// as internal states. 
        /// </summary>
        /// <param name="mx">Measured position.</param>
        /// <param name="dt">Time delta.</param>
        /// <returns>Updated position.</returns>
        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);
        }


        /// <summary>
        /// Update the state by measurement m at dt time from last measurement.
        /// Requires only X position and velocity, sythesizes acceleration
        /// as internal state. 
        /// </summary>
        /// <param name="mx">Measured position.</param>
        /// <param name="mv">Measured velocity.</param>
        /// <param name="dt">Time delta.</param>
        /// <returns>Updated position.</returns>
        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);
        }


        /// <summary>
        /// Update the state by measurement m at dt time from last measurement.
        /// </summary>
        /// <param name="mx">Measured position.</param>
        /// <param name="mv">Measured velocity.</param>
        /// <param name="ma">Measured acceleration.</param>
        /// <param name="dt">Time delta.</param>
        /// <returns>Updated position.</returns>
        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];
        }


    }
}

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
Web01 | 2.8.140916.1 | Last Updated 9 Oct 2012
Article Copyright 2012 by HoshiKata
Everything else Copyright © CodeProject, 1999-2014
Terms of Service
Layout: fixed | fluid