Unstable correction step in robot_localization ekf causes robot position to jump randomly
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 -5.0133e-05 1e-06 0 0 0 0 0 0
0 0 0 1e-06 0 0 0 0 0
0 0 0 0 1e-06 0 0 0 0
0 0 0 0 0 1e-06 0 0 0
0 0 0 0 0 0 1e-06 0 0
0 0 0 0 0 0 0 1e-06 0
0 0 0 0 0 0 0 0 1e-06 ]
State-to-measurement subset is:
[1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 ]
Kalman gain subset is:
[-8.4401 -7.5758 -675.96 1.0895e-09 -2.5491e-06 5.5728e-06 -1.0481e-13 2.2242e-10 2.9098e-08
-7.6408 -5.1376 -547.23 8.82e-10 -2.0637e-06 4.5115e-06 -8.4853e-14 1.8006e-10 2.3556e-08
-0.051842 -0.041612 -3.7171 7.6029e-12 -1.7789e-08 3.8889e-08 -7.3144e-16 1.5521e-12 2.0306e-10
-2.5101e-13 -2.0148e-13 -1.7998e-11 0.2495 1.3464e-17 -2.9433e-17 4.6375e-08 -1.5382e-14 -1.5368e-19
5.873e-10 4.7141e-10 4.211e-08 1.3464e-17 0.2495 6.8862e-14 1.5382e-14 4.6375e-08 3.5956e-16
0.14554 0.097184 10.398 -1.6759e-11 3.9212e-08 -8.5723e-08 1.6123e-15 -3.4213e-12 -4.476e-10
0.073866 0.062716 5.781 -9.3176e-12 2.1801e-08 -4.766e-08 8.9641e-16 -1.9022e-12 -2.4886e-10
0.38888 0.26095 27.831 -4.4857e-11 1.0496e-07 -2.2945e-07 4.3155e-15 -9.1575e-12 -1.198e-09
-1.2839e-09 -1.0306e-09 -9.2059e-08 -2.9433e-17 6.8862e-14 0.24962 2.8312e-21 -6.0105e-18 2.3852e-08
2.4148e-17 1.9383e-17 1.7315e-15 4.6375e-08 1.5382e-14 2.8312e-21 0.24852 1.1302e-25 1.4783e-23
-5.1242e-14 -4.1131e-14 -3.6742e-12 -1.5382e-14 4.6375e-08 -6.0105e-18 1.1302e-25 0.24852 -3.1385e-20
0.00080086 0.00046309 0.054521 -8.7875e-14 2.0561e-10 -4.4948e-10 8.454e-18 -1.7939e-14 -2.347e-12
0.062068 0.052747 4.8595 -7.8323e-12 1.8326e-08 -4.0063e-08 7.5351e-16 -1.599e-12 -2.0919e-10
0.23545 0.15861 16.874 -2.7197e-11 6.3634e-08 -1.3911e-07 2.6165e-15 -5.5521e-12 -7.2637e-10
-6.7039e-12 -5.3811e-12 -4.8068e-10 -1.5368e-19 3.5956e-16 2.3852e-08 1.4783e-23 -3.1385e-20 0.249 ]
Innovation is:
[-0.021045 -0.010175 -1.5555e-37 6.7904e-41 -4.1097e-39 1.0111e-39 -5.7466e-41 1.9881e-37 1.0859e-37 ]
Corrected full state is:
[2.306 0.69003 0.0015144 7.3325e-15 -1.7156e-11 -2.9537 0.34351 -0.010961 3.7506e-11 -7.0542e-19 1.4969e-15 -0.19099 0.10239 -0.0066107 1.9583e-13 ]
Corrected full estimate error covariance is:
[0.040975 0.033163 0.00022502 1.0895e-15 -2.5491e-12 -0.00063014 -0.00035041 -0.0016866 5.5728e-12 -1.0481e-19 2.2242e-16 -3.3037e-06 -0.00029456 -0.0010226 2.9098e-14
0.033163 0.02687 0.00018216 8.82e-16 -2.0637e-12 -0.00051058 -0.00028367 -0.0013666 4.5115e-12 -8.4853e-20 1.8006e-16 -2.6782e-06 -0.00023845 -0.00082854 2.3556e-14
0.00022502 0.00018216 1.5703e-06 7.6029e-18 -1.7789e-14 -3.4613e-06 -1.9244e-06 -9.2646e-06 3.8889e-14 -7.3144e-22 1.5521e-18 -1.8149e-08 -1.6176e-06 -5.617e-06 2.0306e-16
1.0895e-15 8.82e-16 7.6029e-18 2.495e-07 1.3464e-23 -1.6759e-17 -9.3176e-18 -4.4857e-17 -2.9433e-23 4.6375e-14 -1.5382e-20 -8.7875e-20 -7.8323e-18 -2.7197e-17 -1.5368e-25
-2.5491e-12 -2.0637e-12 -1.7789e-14 1.3464e-23 2.495e-07 3.9212e-14 2.1801e-14 1.0496e-13 6.8862e-20 1.5382e-20 4.6375e-14 2.0561e-16 1.8326e-14 6.3634e-14 3.5956e-22
-0.00063014 -0.00051058 -3.4613e-06 -1.6759e-17 3.9212e-14 0.0035133 5.271e-06 2.0456e-05 -8.5723e-14 1.6123e-21 -3.4213e-18 9.419e-05 4.4462e-06 1.251e-05 -4.476e-16
-0.00035041 -0.00028367 -1.9244e-06 -9.3176e-18 2.1801e-14 5.271e-06 0.00077541 1.4101e-05 -4.766e-14 8.9641e-22 -1.9022e-18 2.8042e-08 0.00047684 8.5418e-06 -2.4886e-16
-0.0016866 -0.0013666 -9.2646e-06 -4.4857e-17 1.0496e-13 2.0456e-05 1.4101e-05 0.0033481 -2.2945e-13 4.3155e-21 -9.1575e-18 1.3314e-07 1.1898e-05 0.0019499 -1.198e-15
5.5728e-12 4.5115e-12 3.8889e-14 -2.9433e-23 6.8862e-20 -8.5723e-14 -4.766e-14 -2.2945e-13 2.4962e-07 2.8312e-27 -6.0105e-24 -4.4948e-16 -4.0063e-14 -1.3911e-13 2.3852e-14
-1.0481e-19 -8.4853e-20 -7.3144e-22 4.6375e-14 1.5382e-20 1.6123e-21 8.9641e-22 4.3155e-21 2.8312e-27 2.4852e-07 1.1302e-31 8.454e-24 7.5351e-22 2.6165e-21 1.4783e-29
2.2242e-16 1.8006e-16 1.5521e-18 -1.5382e-20 4.6375e-14 -3.4213e-18 -1.9022e-18 -9.1575e-18 -6.0105e-24 1.1302e-31 2.4852e-07 -1.7939e-20 -1.599e-18 -5.5521e-18 -3.1385e-26
-3.3037e-06 -2.6782e-06 -1.8149e-08 -8.7875e-20 2.0561e-16 9.419e-05 2.8042e-08 1.3314e-07 -4.4948e-16 8.454e-24 -1.7939e-20 0.0030532 2.3492e-08 7.5081e-08 -2.347e-18
-0.00029456 -0.00023845 -1.6176e-06 -7.8323e-18 1.8326e-14 4.4462e-06 0.00047684 1.1898e-05 -4.0063e-14 7.5351e-22 -1.599e-18 2.3492e-08 0.016166 7.2085e-06 -2.0919e-16
-0.0010226 -0.00082854 -5.617e-06 -2.7197e-17 6.3634e-14 1.251e-05 8.5418e-06 0.0019499 -1.3911e-13 2.6165e-21 -5.5521e-18 7.5081e-08 7.2085e-06 0.017053 -7.2637e-16
2.9098e-14 2.3556e-14 2.0306e-16 -1.5368e-25 3.5956e-22 -4.476e-16 -2.4886e-16 -1.198e-15 2.3852e-14 1.4783e-29 -3.1385e-26 -2.347e-18 -2.0919e-16 -7.2637e-16 2.49e-07 ]
---------------------- /Ekf::correct ----------------------
------ /FilterBase::processMeasurement (odom1_pose) ------
To Reproduce
I have made a python script that performs the same matrix calculations, with adjustable default covariances (1e-6) for the automatically added 3-d variables (since I have 2-d mode set to True). You can run this script and by changing default_var value to something higher (1e-3), or even lower (1e-9), the instability goes away.
#!/usr/bin/env python3
import numpy as np
np.set_printoptions(linewidth=300)
default_var = 1e-6 # change this value to see instability be gone
print(f"default variance for 3d state variables: {default_var}")
state = np.array([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])
print("Starting state:")
print(state)
measurement_subset = np.array([2.0302, 0.46678, 0, 0, 0, 0, 0, 0, 0])
print("Measurement subset:")
print(measurement_subset)
measurement_cov_subset = np.array([
[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, -5.0133e-05, default_var, 0, 0, 0, 0, 0, 0],
[0, 0, 0, default_var, 0, 0, 0, 0, 0],
[0, 0, 0, 0, default_var, 0, 0, 0, 0],
[0, 0, 0, 0, 0, default_var, 0, 0, 0],
[0, 0, 0, 0, 0, 0, default_var, 0, 0],
[0, 0, 0, 0, 0, 0, 0, default_var, 0],
[0, 0, 0, 0, 0, 0, 0, 0, default_var]
])
# # try removing z covariances
# measurement_cov_subset = np.array([
# [5.6886e-05, 3.7788e-05, 0, 0, 0, 0, 0, 0, 0],
# [3.7788e-05, 5.3547e-05, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, default_var, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, default_var, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, default_var, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, default_var, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, default_var, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, default_var, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, default_var]
# ])
state_subset = np.array([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])
estimate_error_cov = np.array([
[0.0046429, -0.00037679, -7.6716e-39, 3.3466e-42, -2.1622e-40, 8.9713e-06, -3.1089e-05, 2.0563e-05, 4.9867e-41, -2.8393e-42, 1.1491e-39, 2.4016e-07, -2.6252e-05, 1.0834e-05, 5.3553e-39],
[-0.00037679, 0.004847, -6.1225e-39, 2.6701e-42, -1.7259e-40, -9.4357e-05, -7.5143e-06, -0.00024826, 3.9798e-41, -2.2654e-42, 8.9855e-40, -7.3535e-07, -6.1687e-06, -0.00014848, 4.274e-39],
[-7.6716e-39, -6.1225e-39, 3.3288e-07, 2.1476e-18, -5.0248e-15, -1.4834e-40, -3.337e-41, -4.3627e-40, 1.0987e-14, -2.0621e-22, 4.3754e-19, 1.3241e-42, -2.3023e-41, -2.819e-40, 5.732e-17],
[3.3466e-42, 2.6701e-42, 2.1476e-18, 3.3244e-07, 2.4025e-23, 9.4647e-44, 1.4551e-44, 1.9032e-43, -5.2527e-23, 8.2226e-14, -2.7274e-20, 7.5072e-43, 1.0039e-44, 1.2297e-43, -2.7405e-25],
[-2.1622e-40, -1.7259e-40, -5.0248e-15, 2.4025e-23, 3.3244e-07, -1.2422e-42, -9.4018e-43, -1.2295e-41, 1.2289e-19, 2.7274e-20, 8.2226e-14, 9.174e-41, -6.4867e-43, -7.9449e-42, 6.4117e-22],
[8.9713e-06, -9.4357e-05, -1.4834e-40, 9.4647e-44, -1.2422e-42, 0.0035054, 1.598e-08, -6.7766e-07, 9.6428e-43, -6.7786e-44, 1.9015e-39, 9.4154e-05, 2.5956e-08, -3.4296e-07, 1.0356e-40],
[-3.1089e-05, -7.5143e-06, -3.337e-41, 1.4551e-44, -9.4018e-43, 1.598e-08, 0.00077264, 5.0288e-08, 2.1683e-43, -1.2345e-44, 4.969e-42, -3.3644e-10, 0.00047451, 2.9798e-08, 2.3285e-41],
[2.0563e-05, -0.00024826, -4.3627e-40, 1.9032e-43, -1.2295e-41, -6.7766e-07, 5.0288e-08, 0.0032913, 2.8359e-42, -1.6146e-43, 6.5769e-41, 3.4641e-08, 7.9249e-08, 0.0019153, 3.0455e-40],
[4.9867e-41, 3.9798e-41, 1.0987e-14, -5.2527e-23, 1.2289e-19, 9.6428e-43, 2.1683e-43, 2.8359e-42, 3.3266e-07, 5.0429e-27, -1.0705e-23, -8.6578e-45, 1.496e-43, 1.8324e-42, 4.2325e-14],
[-2.8393e-42, -2.2654e-42, -2.0621e-22, 8.2226e-14, 2.7274e-20, -6.7786e-44, -1.2345e-44, -1.6146e-43, 5.0429e-27, 3.3071e-07, 2.0088e-31, -2.647e-43, -8.5173e-45, -1.0433e-43, 2.631e-29],
[1.1491e-39, 8.9855e-40, 4.3754e-19, -2.7274e-20, 8.2226e-14, 1.9015e-39, 4.969e-42, 6.5769e-41, -1.0705e-23, 2.0088e-31, 3.3071e-07, 5.8643e-38, 3.4252e-42, 4.2349e-41, -5.5852e-26],
[2.4016e-07, -7.3535e-07, 1.3241e-42, 7.5072e-43, 9.174e-41, 9.4154e-05, -3.3644e-10, 3.4641e-08, -8.6578e-45, -2.647e-43, 5.8643e-38, 0.0030532, -3.8947e-10, 1.4996e-08, -9.298e-43],
[-2.6252e-05, -6.1687e-06, -2.3023e-41, 1.0039e-44, -6.4867e-43, 2.5956e-08, 0.00047451, 7.9249e-08, 1.496e-43, -8.5173e-45, 3.4252e-42, -3.8947e-10, 0.016164, 4.8916e-08, 1.6065e-41],
[1.0834e-05, -0.00014848, -2.819e-40, 1.2297e-43, -7.9449e-42, -3.4296e-07, 2.9798e-08, 0.0019153, 1.8324e-42, -1.0433e-43, 4.2349e-41, 1.4996e-08, 4.8916e-08, 0.017032, 1.9678e-40],
[5.3553e-39, 4.274e-39, 5.732e-17, -2.7405e-25, 6.4117e-22, 1.0356e-40, 2.3285e-41, 3.0455e-40, 4.2325e-14, 2.631e-29, -5.5852e-26, -9.298e-43, 1.6065e-41, 1.9678e-40, 3.3156e-07]
])
state_to_measurement_subset = np.array([
[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]
])
innovation_subset = measurement_subset - state_subset
pht = np.matmul(estimate_error_cov, state_to_measurement_subset.transpose())
hphtr = np.matmul(state_to_measurement_subset, pht) + measurement_cov_subset
hphtr_inv = np.linalg.inv(hphtr)
kalman_gain_subset = np.matmul(pht, hphtr_inv)
print("Kalman gain subset:")
print(kalman_gain_subset)
state += np.matmul(kalman_gain_subset, innovation_subset)
print("Corrected state:")
print(state)
Was this choice of 1e-6 arbitrary? Removing the x-z and y-z covariance values also seems to make it work properly, but I'm not sure if that is just for these sets of values, or if it would help generally. If it would always improve performance, then it may be worth updating the code of the r_l package to ignore x-z and y-z covariance values if 2-d mode is set to true. Or is this indicative of some deeper instability that is potentially unavoidable? Also, is a Kalman gain value that's outside the interval [0.0, 1.0] valid? It seems like if it's ever greater than 1, it will correct further than the value of the measurement, and if it's negative, then it could correct in the wrong direction.
Expected behavior The filter doesn't produce a high kalman gain and doesn't end up overshooting corrections (which are also in the wrong direction).
Using Version: 2.7.4-1focal.20221124.034028 on ubuntu 20.04, but I assume this will happen on most recent versions.
Asked by igor-markovic on 2022-12-14 21:48:45 UTC
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.
Asked by Tom Moore on 2023-04-27 03:56:34 UTC