基于C#的Kalman滤波器

最近项目用到了kalman滤波器，由于是.NET平台的，原来的OpenCV的那套东西不能用了，还好灵光乍现，开源就是好啊！

花了一个小时将CvKalman用C#进行实现，解决了！

其中的Matrix运算采用了CSDN下载频道的“C#矩阵库”。

using System;
using System.Collections.Generic;
using System.Text;

namespace SimTransfer
{
    public class KalmanFacade
    {
        #region inner class
        class KalmanFilter
        {
            int MP;                     /* number of measurement vector dimensions */
            int DP;                     /* number of state vector dimensions */
            int CP;                     /* number of control vector dimensions */

            public Matrix state_pre;           /* predicted state (x'(k)):
                                        x(k)=A*x(k-1)+B*u(k) */
            public Matrix state_post;          /* corrected state (x(k)):
                                        x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
            public Matrix transition_matrix;   /* state transition matrix (A) */
            public Matrix control_matrix;      /* control matrix (B)
                                       (it is not used if there is no control)*/
            public Matrix measurement_matrix;  /* measurement matrix (H) */
            public Matrix process_noise_cov;   /* process noise covariance matrix (Q) */
            public Matrix measurement_noise_cov; /* measurement noise covariance matrix (R) */
            public Matrix error_cov_pre;       /* priori error estimate covariance matrix (P'(k)):
                                        P'(k)=A*P(k-1)*At + Q)*/
            Matrix gain;                /* Kalman gain matrix (K(k)):
                                        K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
            Matrix error_cov_post;      /* posteriori error estimate covariance matrix (P(k)):
                                        P(k)=(I-K(k)*H)*P'(k) */
            Matrix temp1;               /* temporary matrices */
            Matrix temp2;
            Matrix temp3;
            Matrix temp4;
            Matrix temp5;

            public KalmanFilter()
            {
                MP = 1;
                DP = 2;
                CP = 0;
                state_pre = new Matrix(DP, 1);
                state_pre.Zero();
                state_post = new Matrix(DP, 1);
                state_post.Zero();
                transition_matrix = new Matrix(DP, DP);
                transition_matrix.SetIdentity(1.0);
                transition_matrix[0, 1] = 1;
                process_noise_cov = new Matrix(DP, DP);
                process_noise_cov.SetIdentity(1.0);
                measurement_matrix = new Matrix(MP, DP);
                measurement_matrix.SetIdentity(1.0);
                measurement_noise_cov = new Matrix(MP, MP);
                measurement_noise_cov.SetIdentity(1.0);
                error_cov_pre = new Matrix(DP, DP);
                error_cov_post = new Matrix(DP, DP);
                error_cov_post.SetIdentity(1);
                gain = new Matrix(DP, MP);
                if (CP > 0)
                {
                    control_matrix = new Matrix(DP, CP);
                    control_matrix.Zero();
                }
                //
                temp1 = new Matrix(DP, DP);
                temp2 = new Matrix(MP, DP);
                temp3 = new Matrix(MP, MP);
                temp4 = new Matrix(MP, DP);
                temp5 = new Matrix(MP, 1);
            }

            public Matrix Predict()
            {
                state_pre = transition_matrix.Multiply(state_post);
                //if (CP>0)
                //{
                //    control_matrix
                //}
                temp1 = transition_matrix.Multiply(error_cov_post);
                Matrix at = transition_matrix.Transpose();
                error_cov_pre = temp1.Multiply(at).Add(process_noise_cov);

                Matrix result = new Matrix(state_pre);
                return result;
            }

            public Matrix Correct(Matrix measurement)
            {
                temp2 = measurement_matrix.Multiply(error_cov_pre);
                Matrix ht = measurement_matrix.Transpose();
                temp3 = temp2.Multiply(ht).Add(measurement_noise_cov);
                temp3.InvertSsgj();
                temp4 = temp3.Multiply(temp2);
                gain = temp4.Transpose();

                temp5 = measurement.Subtract(measurement_matrix.Multiply(state_pre));
                state_post = gain.Multiply(temp5).Add(state_pre);

                error_cov_post = error_cov_pre.Subtract(gain.Multiply(temp2));

                Matrix result = new Matrix(state_post);
                return result;
            }

            public Matrix AutoPredict(Matrix measurement)
            {
                Matrix result = Predict();

                Correct(measurement);

                return result;
            }
        }
        #endregion

        public KalmanFacade(int valueItem)
        {
            if (valueItem<=0)
            {
                throw new Exception("not enough value items!");
            }
            kmfilter = new KalmanFilter[valueItem];
            Random rand = new Random(1001);
            for (int i = 0; i < valueItem; i++ )
            {
                kmfilter[i] = new KalmanFilter();
                kmfilter[i].state_post[0, 0] = rand.Next(10);
                kmfilter[i].state_post[1, 0] = rand.Next(10);
                //
                kmfilter[i].process_noise_cov.SetIdentity(1e-5);
                kmfilter[i].measurement_noise_cov.SetIdentity(1e-1);
            }
        }

        private KalmanFilter[] kmfilter = null;
       
        public bool GetValue(double[] inValue, ref double[] outValue)
        {
            if (inValue.Length != kmfilter.Length || outValue.Length != kmfilter.Length)
            {
                return false;
            }

            Matrix[] measures = new Matrix[kmfilter.Length];
           
            for (int i = 0; i < kmfilter.Length; i++ )
            {
                measures[i] = new Matrix();
                measures[i][0, 0] = inValue[i];
                outValue[i] = kmfilter[i].AutoPredict(measures[i])[0, 0];
            }

            return true;
        }
    }

}

//==========test=============

            SimTransfer.KalmanFacade kalman = new SimTransfer.KalmanFacade(1);
            Random rand = new Random(1001);
            System.IO.StreamWriter dataFile = new System.IO.StreamWriter("D:\\test.csv");
           
            for (int x = 0; x < 2000; x++ )
            {
                double y = 100 * Math.Sin((2.0 * Math.PI / (float)200) * x);
               
                double noise = 20 * Math.Sin((40.0 * Math.PI / (float)200) * x) + 40 * (rand.NextDouble() - 0.5);

                double[] z_k = new double[1];
                z_k[0] = y + noise;
                double[] y_k = new double[1];
                kalman.GetValue(z_k, ref y_k);

                dataFile.WriteLine(y.ToString()
                    + "," + z_k[0].ToString()
                    + "," + y_k[0].ToString());
            }

            dataFile.Close();
            MessageBox.Show("OK!");

本文来自CSDN博客，转载请标明出处：http://blog.csdn.net/csdnbao/archive/2009/09/24/4590519.aspx