OpenCV中的Kalman滤波例程学习

kalman原理可参见：http://blog.csdn.net/lanbing510/article/details/40936343 

                  http://blog.csdn.net/crazyquhezheng/article/details/48659959

Kalman滤波的5个转移函数

状态转移：X'(k)=A*x(k-1)+B*u(k) //最终结果为预测值，A为状态转移矩阵，B为控制矩阵

Z(k) =H*x(k)                              //最终结果为观测值，H为观测矩阵

噪音转移：P'(k)=A*P(k-1)*At + Q)                 // P'(k)为预测噪音, Q为预测协方差

增益：K(k)=P'(k)*H^T*(H*P'(k)*H^T+R)^-1           //R为观测协方差

状态修正：x(k)=x'(k)+K(k)*(z(k)-H*x'(k))        //当k偏向1时，最终结果偏向观测值；k偏向0时，最终结果偏向预测值

噪音修正：P(k)=(I-K(k)*H)*P'(k)

KalmanFilter成员变量说明

/**********************************************************************
MatstatePre;           //!< predictedstate (x'(k)): x’(k)=A*x(k-1)+B*u(k)
MatstatePost;          //!< correctedstate (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
MattransitionMatrix;   //!< statetransition matrix (A)
MatcontrolMatrix;      //!< controlmatrix (B) (not used if there is no control)
MatmeasurementMatrix;  //!< measurementmatrix (H)
MatprocessNoiseCov;    //!< process noisecovariance matrix (Q)
MatmeasurementNoiseCov;//!< measurement noise covariance matrix (R)
MaterrorCovPre;         //!< priori error estimate covariancematrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
Matgain;              //!<Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
MaterrorCovPost;     //!<posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
*************************************************************************/

kalman.cpp范例

这个例程描述了对一个绕圆弧做近似匀速运动的点的跟踪。首先建立匀速运动模型，设定状态变量x = 。，则运动模型为

θ’(k+1) = θ(k)+ ω(k)*t

ω’(k+1)=ω(k)

因此。

观测量为角度Z=θ，观测模型为，因此。

此例中：令t =1

KalmanFilterKF(2, 1, 0);   //即状态量有2个，观测量有1个，控制量为0个
//此时状态量大小为2*1，过程噪音量大小为2*2，观测量大小为1。
//初始状态随机赋值，服从μ=0，σ=0.1的正态分布
randn(state,Scalar::all(0), Scalar::all(0.1));
//状态转移转移矩阵A
KF.transitionMatrix= *(Mat_<float>(2, 2) << 1, 1, 0, 1);
//PrintMat(KF.transitionMatrix);
//观测矩阵H对角线赋1
setIdentity(KF.measurementMatrix);
//预测噪音协方差Q对角线赋初值1e-5，其大小为2*2，不可赋值为0，否则会认为其已经是最优解
setIdentity(KF.processNoiseCov,Scalar::all(1e-5));
//观测噪音协方差R对角线赋初值1e-1，其大小为1*1，不可赋值为0，否则会认为其已经是最优解
setIdentity(KF.measurementNoiseCov,Scalar::all(1e-1));
//真实噪音P对角线赋初值为1，其大小为2*2
setIdentity(KF.errorCovPost,Scalar::all(1));
//state随机赋初值
randn(KF.statePost,Scalar::all(0), Scalar::all(0.1));
 /******************************************************/
Matprediction = KF.predict();        
//开始预测，得到X’(k+1)，并更新P’(k)，即实现前两个方程（状态转移和噪音转移）
/******************************************************/        
//测量误差
randn(measurement,Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));
measurement+= KF.measurementMatrix*state;          //依据X(k)得到Z(k)测量值
/******************************************************/      
KF.correct(measurement)              
//依据测量值返回校正后的X(k+1)，并更新噪音P(k+1)、K，即实现后三个方程（增益、状态修正和噪音修正方程）
 /******************************************************/     
randn(processNoise,Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
state =KF.transitionMatrix*state + processNoise;
//点的真实值，近似匀速的圆周运动。

综上：Kalman filter中需要初始化的量有：

transitionMatrix/*A*/；

controlMatrix/*B，若不存在控制量，则不需要*/;

measurementMatrix/*H*/；

processNoiseCov/*预测标准差，不能为0*/；

measurementNoiseCov/*测量标准差，不能为0*/；

errorCovPost/*初始噪音，初值可为I*/；

statePost/*初始状态，随机，服从正态分布N(μ,σ)即可*/

题外：可以将画图的代码段更换为：

         printf_s("实际:%.2f,  预测:%.2f,  测量:%.2f\n",stateAngle, predictAngle, measAngle);

         并在state =KF.transitionMatrix*state + processNoise;后加上

         printf_s("实际:%.2f,  预测:%.2f\n",state.at<float>(0), KF.statePost.at<float>(0));

最终输出结果为：