java 二维卡尔曼滤波_Kalman Filter(卡尔曼滤波)的个人笔记以及程序实现

简单的介绍一下卡尔曼滤波器的关键的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

4 #include

5 #include

6 #include

7

8 using namespacecv;9 using namespacestd;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 intmain()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<

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(0),prediction.at(1));71

72 //3.update measurement

73

74 measurement.at(0) = (float)move_mouse.x;75 measurement.at(1) = (float)move_mouse.y;76

77 //4.update

78 kalman.correct(measurement);79

80

81 //5.draw82

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滤波