OpenCV how to use the KalmanFilter class as ExtendedKF

Question

As written in the docs, it is possible to use the cv::KalmanFilter class as an Extended-Kalman-Filter (EKF). Can someone explain me how?

All the matrices involved are declared as public so I can edit all of them. The source code is for the normal (linear) Kalman-Filter.

I think that I should edit the transitionMatrix with my non linear system, namely G. This matrix is the one of my non linear system with as input variables both the statePost and the control; and ControlMatrix should be all 0. Right?

But where should I put the Jacobian of G?

I have the same doubt for the update process, I have my non linear system H for the measurementMatrix.

Maybe I'm a bit confused, can someone help me please?

Answer 1

So, I think that I figured out how to use the cv::KalmanFilter class as an EKF. This is how I made it:

1. save in a temp variable the kf.statePost: temp = kf.statePost

2. put in the kf.transitionMatrix the Jacobian of the transition function

3. do the prediction step of the KF

4. change the kf.statePre with the correct value using the transition function: kf.statePre = f(temp, control)

5. put in the kf.measurementMatrix the Jacobian of the measurement (or correction) function

6. do the correction step of the KF

7. change the kf.temp5 matrix with the correct value: kf.temp5 = measurement - h(statePre) where h() is the measurement (or correction) function

8. change 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!