search

cra-ros-pkg / robot_localization

robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc. Please ask questions on answers.ros.org.
http://www.cra.com
Other
1.39k stars 895 forks source link

UKF transfer function does not capture nonlinearity correctly #621

Open jola6897 opened 3 years ago

jola6897 commented 3 years ago

Bug report

Required Info:

Steps to reproduce issue

Check the predict function inside ukf.cpp.

Expected behavior

The non-linear state transfer function is evaluated on each sigma point individually.

Actual behavior

In ukf.cpp inside the predict function the sigma points are generated and the transferFunction_ is applied to them. So basically we try to estimate the a priori state. Using sigma points allows us to capture also non-linear functions quite easy as in theory transferFunction_ could also be non-linear. And indeed the function that is used here is non-linear.

BUT: The problem is that in the implementation transferFunction_ is a matrix which is the same for all sigma points. It is obtained by evaluating the non-linear function at the a posteriori state (state_). This means that the transferFunction_ is only correct for the a posteriori state (state_) which happens to be also the first sigma point. All other sigma points have by construction a slightly different orientation therefore roll, pitch, yaw are different for each sigma point. This means that the actual behavior is not captured correctly and that the a priori state and covariance are not correct either. If the variance of the orientation is small then the error will be also really small because the sampled sigma points in terms of orientation are quite close together, but this is not always the case.

Additional information

My feedback is based on "A New Extension of the Kalman Filter to Nonlinear Systems" that is also referenced in "A Generalized Extended Kalman Filter Implementation for the Robot Operating System".

I do not know if this behavior was intended to speed up computation or maybe there is also a proof that this is a sufficient approximation that I am unaware of. If so could you please share the reasons for your decision to implement it like that, as I would like to understand it.

Thanks a lot for your effort. Looking forward to your feedback.

ayrton04 commented 3 years ago

Nope, definite oversight on that front. The transfer function matrix formulation should really go away. Care to submit a PR that constructs the transfer function matrix for every sigma point? The computation expense will go up, but the behaviour will at least be correct.

jola6897 commented 3 years ago

Hi,

I added a pull request that should resolve this bug (#628). If you accept this pull request or implement something similar you can close this issue :).

jola6897 commented 3 years ago

I had a closer look again, and I think that there is even a larger issue with the UKF. Since the euler angles that are used in the state vector belong to SO(3), this means that the sampling and the mean computation need to be adapted as they behave differently than the SE(3) state variables like position. I can recommend:

Y. Cheon and J. Kim, "Unscented Filtering in a Unit Quaternion Space for Spacecraft Attitude Estimation," 2007 
IEEE International Symposium on Industrial Electronics, Vigo, Spain, 2007, pp. 66-71, doi: 10.1109/ISIE.2007.4374575.

They are mainly concerned about quaternions in the state vector but the principle is the same. I do not know if this warrants a new issue, but I think both should be fixed in the same pull request.

ayrton04 commented 3 years ago

Thanks for this. I'll read up on that and make those adjustments.