基于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'(