简单的介绍一下卡尔曼滤波器的关键的5个公式。

引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：

　　X(k)=A X(k-1)+B U(k)+W(k)

　　再加上系统的测量值：

　　Z(k)=H X(k)+V(k)

上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声，他们的协方差（covariance ）分别是Q，R（这里我们假设他们不随系统状态变化而变化）

首先利用系统的过程模型来预测系统下一状态，设在k时刻的系统状态为x(k),则可以根据系统模型，由上一状态预测出现在状态：

x(k|k-1) = A x(k-1|k-1) + B u(k).....(1)

其中x(k|k-1)是上一时刻的状态对现在时刻状态的预测，x(k-1|k-1)是上一时刻状态的最优结果， u(k)为现在时刻状态的控制量。

系统的状态已经更新，现在需要更新系统的误差估计协方差矩阵，用p(k|k-1)表示误差估计协方差矩阵：

p(k|k-1) = A p(k-1|k-1)A' + Q........(2)

其中p(k|k-1)是在k时刻由上一状态对此状态的预测， p(k-1|k-1)是x(k-1|k-1)对应的误差估计协方差矩阵，A' 是A 的转置。Q 表示系统过程噪声的covariance。

现在我们得到了预测结果，然后我们根据得到的现在状态的测量值进行更新（修正）（预测得到的）现在的状态 x(k|k):

x(k|k) = x(k|k-1) + Kg(k) (Z(k)-Hx(k|k-1))....(3)

(3)式中 Kg(k)未知，则需要对其就行求解，就引出(4)式：

 Kg(k) = p(k|k-1) H' /(H p(k|k-1)H' + R)...(4)

到现在，我们以及得出的k 时刻的系统状态的最优值x(k|k),为了让卡尔曼滤波器不断地进行下去，我们需要更新 x(k|k) 对应的p(k|k) (应用于式（2）)：

p(k|k) = (I-Kg(k) H) p(k|k-1).....(5)

 

卡尔曼滤波实现的小程序：opencv+ C++

  1 #include "opencv2/video/tracking.hpp"
  2 #include "opencv2/highgui/highgui.hpp"
  3 #include <cmath>  
  4 #include <vector>
  5 #include <stdio.h>
  6 #include <iostream>
  7 
  8 using namespace cv;
  9 using namespace std;
 10 const int winHeight=600;  
 11 const int winWidth=800;
 12 
 13 Point move_mouse = Point(winWidth>>1,winHeight>>1);
 14 //mouse event callback  
 15 
 16 void mouseEvent(int event, int x, int y, int flags, void *param )
 17 {
 18     if (event==CV_EVENT_MOUSEMOVE)
 19     {  
 20         move_mouse=Point(x,y);  
 21     } 
 22 }
 23 
 24 
 25 int main()
 26 {
 27     
 28     ////1.kalman filter setup
 29 
 30     const int stateNum=4;  
 31     const int measureNum=2;
 32 
 33     KalmanFilter kalman(stateNum,measureNum,0);
 34     Mat processNoise(stateNum,1,CV_32F);
 35     Mat measureNoise(stateNum,1,CV_32F);
 36     Mat measurement (measureNum,1,CV_32F);
 37     float A[stateNum][stateNum] ={//transition matrix  
 38         1,0,1,0,  
 39         0,1,0,1,  
 40         0,0,1,0,  
 41         0,0,0,1  
 42     };  
 43 
 48     memcpy( kalman.transitionMatrix.data,A,sizeof(A));
 49     cout<<kalman.transitionMatrix<<endl;
 50     setIdentity(kalman.measurementMatrix);
 51     setIdentity(kalman.processNoiseCov,Scalar::all(1e-5));
 52     setIdentity(kalman.measurementNoiseCov,Scalar::all(1e-1));
 53     setIdentity(kalman.errorCovPost,Scalar::all(1));
 54 
 55     //initialize post state of kalman filter at random 
 56     randn(kalman.statePost,Scalar::all(0), Scalar::all(0.1));
 57     CvFont font;  
 58     cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);  
 59 
 60    
 62     namedWindow("kalman",1);
 63     setMouseCallback("kalman",mouseEvent);
 64     
 65     Mat img(500, 500, CV_8UC3);
 66     while (1)
 67     {
 68         //2.kalman prediction
 69         Mat prediction = kalman.predict();
 70         Point predict_pt = Point(prediction.at<float>(0),prediction.at<float>(1));
 71  
 72        //3.update measurement 
 73 
 74         measurement.at<float>(0) = (float)move_mouse.x;
 75         measurement.at<float>(1) = (float)move_mouse.y;
 76 
 77         //4.update
 78         kalman.correct(measurement);
 79 
 80 
 81         //5.draw
 82 
 84         img = Scalar::all(0);
 85         circle(img,predict_pt,4,CV_RGB(0,255,0),2);//predicted point with green 
 86         circle(img,move_mouse,4,CV_RGB(255,0,0),2);//current position with red
 87      
 93     imshow("kalman", img);  
 94         int key=cvWaitKey(3);  
 95         if (key==27)
 96         {
 97             break;    
 98         }
 99     }
101     system("pause");
102     return 0;
103 }

程序说明：

本程序由http://blog.sina.com.cn/s/blog_9db9f81901015wuo.html 稍作修改适应新版本的opencv。

程序主要由以下几个部分：

1.实例化一个 KalmanFilter 的对象kalman，声明并初始化矩阵processNoise，measureNoise， measurement ;

注意：要设置的变量主要是KalmanFilter的成员，否则会出现跟踪效果不好或者出错

Mat statePre;    //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
Mat statePost;    //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
Mat transitionMatrix;    //!< state transition matrix (A)
Mat controlMatrix;    //!< control matrix (B) (not used if there is no control) 
Mat measurementMatrix;    //!< measurement matrix (H)
Mat processNoiseCov;   //!< process noise covariance matrix (Q)
Mat measurementNoiseCov;  //!< measurement noise covariance matrix (R)
Mat errorCovPre;   //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
Mat gain;  //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
Mat errorCovPost;    //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

2.预测KalmanPredict，只需要调用函数 predict() ，然后读出自己需要的值

3.根据采集到的数据更新观测数据

4.用观测数据来更新预测状态值。

 

 

参考资料：http://blog.sina.com.cn/s/blog_4c2b25550100b8yq.html

http://blog.csdn.net/yangtrees/article/details/8075911#comments 学习OpenCV——Kalman滤波