Covariance values of ekf input sources should be calculated

[meliketanrikulu]

In the current autoware architecture, pose and twist data are given as input to EKF. However, the covariance values of these data are filled with the calculated default values.
Covariance values are important for EKF. While generating position values in EKF, the reliability of the new data is determined by considering the covariance of the input data. In this way, if there are noisy values in the input data, EKF should take these values into less consideration. EKF makes this decision by looking at the covariances. For this reason, it is necessary to dynamically calculate the covariance values entering the EKF.

I look forward to your views on this subject.

[kminoda]

Thank you for the suggestion! Do you have any concrete ideas on how to calculate the covariance dynamically on which sensors?

Currently, there are three input sources in EKF: IMU, odometry, and the results of NDT (=LiDAR Localization).
Among these three, I believe that the covariance of NDT can and should ideally be calculated dynamically, for example as a function of transform probability.
For the other two sensors (IMU & odometry), IMHO it would be a bit more difficult to dynamically change the covariance since the covariance would be hardware-dependant.

BTW the covariance of IMU and odometry can be calibrated beforehand using the CalibrationTool which we, TIER IV, recently made public. Note that this is just a static parameter and does not mean that the covariance can be dynamically calculated onboard.

[meliketanrikulu]

Thanks for your reply @kminoda. While working on the vehicle, we detected that the odometry values ​​we received from the vehicle were inaccurate for a short time for the linear velocity and accordingly the position, which is the output of ekf, shifted. Related video is here. In this video, you can see the objects outside the road slide into the road.

On top of that, we saw that the problem in this region was solved when we set the covariance values ​​related to linear velocity only to very high values ​​(set to 1000). However, constantly keeping the linear velocity-related covariance values ​​this high naturally caused the position to flicker in the ekf output. Related video is here

We foresee that short-term errors may occur in ndt pose and imu data like this. Ekf can tolerate the error in these cases by looking at the covariance values.

I think the covariance calculations can be done via RMS(root mean square) error. RMS errors are calculated based on the data distribution, by looking at how far the new incoming data deviates from the mean value. In this way, I think we can get accurate results at least for high frequency odometry and imu data. The sensors we use now also produce error values ​​in a similar way.

[meliketanrikulu]

Meanwhile we will try your calibration tool and share the results with you. Thanks for your suggestions @kminoda .

[kminoda]

Thanks for sharing the interesting results!

we detected that the odometry values ​​we received from the vehicle were inaccurate for a short time for the linear velocity and accordingly the position

May I ask why you think that the inaccuracy of odometry values caused the problem? It would be much more clear if you would share with us the ROSBAG (and the map, if possible) or the plot of the odometry velocity.

I think the covariance calculations can be done via RMS(root mean square) error. RMS errors are calculated based on the data distribution, by looking at how far the new incoming data deviates from the mean value. In this way, I think we can get accurate results at least for high frequency odometry and imu data.

Would you elaborate more on the definition of RMS? Do you mean that, for example, setting the covariance of an input data $x(t)$ proportional to $RMS = ||x(t) - \frac{1}{t}\int_{0}^{t}x(t)||$ ?

[meliketanrikulu]

Thanks for your reply. Here is our data:
rosbag
map

[kminoda]

Thanks for sharing! So in which period of time in your rosbag did the above issue occur? As I briefly looked into the rosbag, it seems that there's a weird jump around 350[s] / 410[s]. Maybe this caused the issue you've mentioned?

Although no one in TIER IV is working on a similar topic, I guess it is possible to mitigate the adverse effect of such a jumpy behavior in the input data (though I think there are some alternative solutions other than the dynamic covariance estimation). Since it is difficult for us to provide the exact solution for the issue now as it seems strongly dependent on the hardware, I would be glad if you could share with us any concrete ideas or prototypes for the solution.

[meliketanrikulu]

Hello @kminoda . Thanks for your support . To verify the linear velocity from the instrument in hand, we would compare it with the linear velocity data from gnss-ins and observed that the velocity from the vehicle was always higher than it should have been. At the same time, we observed that the linear velocity data coming from the vehicle was louder. So for now we choose to use linear velocity from gnss-ins.
Here is our video : https://www.youtube.com/watch?v=hPVPqXyqHDA
In this video blue one is gnss-ins based linear velocity and red one is vehicle odometry based linear velocity.
We are currently continuing our tests.