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