So, I think that I figured out how to use the cv::KalmanFilter
class as an EKF.
This is how I made it:
save in a temp variable the
kf.statePost
:temp = kf.statePost
put in the
kf.transitionMatrix
the Jacobian of the transition functiondo the prediction step of the KF
change the
kf.statePre
with the correct value using the transition function:kf.statePre = f(temp, control)
put in the
kf.measurementMatrix
the Jacobian of the measurement (or correction) functiondo the correction step of the KF
change the
kf.temp5
matrix with the correct value:kf.temp5 = measurement - h(statePre)
whereh()
is the measurement (or correction) functionchange the
kf.statePost
with the correct value:kf.statePost = kf.statePre + kf.gain * kf.temp5
And finally you have the estimated state of the system in kf.statePost
!