因此,我认为我已经知道如何将cv::KalmanFilter
类用作扩展卡尔曼滤波器。
将kf.statePost
保存在一个临时变量中:temp = kf.statePost
在kf.transitionMatrix
中放置转移函数的Jacobian矩阵
进行KF的预测步骤
使用转移函数更改kf.statePre
的正确值:kf.statePre = f(temp, control)
在kf.measurementMatrix
中放置测量(或校正)函数的Jacobian矩阵
做KF的纠正步骤
使用这个公式更改kf.temp5
矩阵: kf.temp5 = measurement - h(statePre)
其中h()
是测量(或校正)函数
使用这个公式更改kf.statePost
矩阵的正确值:kf.statePost = kf.statePre + kf.gain * kf.temp5
最后你可以在kf.statePost
中获得系统的估计状态!
kf.statePost = kf.statePre + kf.gain * kf.temp5
? - user456814