您的位置：首页 > 运维架构

OpenCV中的Kalman滤波例程学习

2016-11-29 19:53 399 查看

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)*HT*(H*P'(k)*HT+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;//!<statetransitionmatrix(A)
MatcontrolMatrix;//!<controlmatrix(B)(notusedifthereisnocontrol)
MatmeasurementMatrix;//!<measurementmatrix(H)
MatprocessNoiseCov;//!<processnoisecovariancematrix(Q)
MatmeasurementNoiseCov;//!<measurementnoisecovariancematrix(R)
MaterrorCovPre;//!<priorierrorestimatecovariancematrix(P'(k)):P'(k)=A*P(k-1)*At+Q)*/
Matgain;//!<Kalmangainmatrix(K(k)):K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
MaterrorCovPost;//!<posteriorierrorestimatecovariancematrix(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;
//点的真实值，近似匀速的圆周运动。

综上：Kalmanfilter中需要初始化的量有：
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));
最终输出结果为：

内容来自用户分享和网络整理，不保证内容的准确性，如有侵权内容，可联系管理员处理

标签： opencv kalman

相关文章推荐

新的分享

章节导航