Kalman Filter(卡尔曼滤波)的个人笔记以及程序实现 - 小胖蛋
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 <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滤波