Unstable correction step in robot_localization ekf causes robot position to jump randomly

asked 2022-12-14 20:48:45 -0500

igor-markovic gravatar image

I have been using this package for localization and noticed the (x, y) position estimate jumping sometimes, seemingly randomly. I enabled the debug logs and have narrowed it down to certain pose measurement correction steps.

Below is one example of such a correction step, where the state for x is 2.0513, the incoming measurement for x is 2.0302, yet the corrected state for x ends up at 2.306: Note that the following may be easier to look at if copied/pasted into a text editor, as ROS Answers doesn't handle long lines very well.

------ FilterBase::processMeasurement (odom1_pose) ------
Filter is already initialized. Carrying out predict/correct loop...
Measurement time is 1670886696.8463904858, last measurement time is 1670886696.8467752934, delta is -0.000384807586669921875
---------------------- Ekf::correct ----------------------
State is:
[2.0513      0.47695     1.5555e-37  -6.7904e-41 4.1097e-39  -2.9497     0.3457      -0.00012196 -1.0111e-39 5.7466e-41  -1.9881e-37 -0.19097    0.10424     -4.1764e-05 -1.0859e-37 ]

Topic is:
odom1_pose
Measurement is:
[2.0302      0.46678     0           0           0           0           0           0           0           0           0           0           0           0           0           ]

Measurement topic name is:
odom1_pose

Measurement covariance is:
[5.6886e-05  3.7788e-05  -6.1751e-05 0           0           0           0           0           0           0           0           0           0           0           0           
 3.7788e-05  5.3547e-05  -5.0133e-05 0           0           0           0           0           0           0           0           0           0           0           0           
 -6.1751e-05 -5.0133e-05 1e-06       0           0           0           0           0           0           0           0           0           0           0           0           
 0           0           0           1e-06       0           0           0           0           0           0           0           0           0           0           0           
 0           0           0           0           1e-06       0           0           0           0           0           0           0           0           0           0           
 0           0           0           0           0           0           0           0           0           0           0           0           0           0           0           
 0           0           0           0           0           0           0           0           0           0           0           0           0           0           0           
 0           0           0           0           0           0           0           0           0           0           0           0           0           0           0           
 0           0           0           0           0           0           0           0           1e-06       0           0           0           0           0           0           
 0           0           0           0           0           0           0           0           0           1e-06       0           0           0           0           0           
 0           0           0           0           0           0           0           0           0           0           1e-06       0           0           0           0           
 0           0           0           0           0           0           0           0           0           0           0           0           0           0           0           
 0           0           0           0           0           0           0           0           0           0           0           0           0           0           0           
 0           0           0           0           0           0           0           0           0           0           0           0           0           0           0           
 0           0           0           0           0           0           0           0           0           0           0           0           0           0           1e-06       ]

Update indices are:
[0           1           2           3           4           8           9           10          14          ]

Current state subset is:
[2.0513      0.47695     1.5555e-37  -6.7904e-41 4.1097e-39  -1.0111e-39 5.7466e-41  -1.9881e-37 -1.0859e-37 ]

Measurement subset is:
[2.0302      0.46678     0           0           0           0           0           0           0           ]

Measurement covariance subset is:
[5.6886e-05  3.7788e-05  -6.1751e-05 0           0           0           0           0           0           
 3.7788e-05  5.3547e-05  -5.0133e-05 0           0           0           0           0           0           
 -6.1751e-05 ...
(more)
edit retag flag offensive close merge delete

Comments

Please post your EKF config and a sample sensor message for every sensor input. Also please describe your sensor frequency. You covariance matrix is nearly singular (condition number is 273438.46 after correction), so my guess is that you have either a mis-configured EKF config or sensor data with bad covariance values.

Tom Moore gravatar image Tom Moore  ( 2023-04-27 03:56:34 -0500 )edit