Improve Roll and Pitch estimation in Localization

[mitsudome-r]

We can see that there is a wobbling of ego vehicle points as the vehicle drives through speed bumps. We would like to make sure that these are estimated correctly in Localization (perhaps in EKF).

example scene: https://www.youtube.com/watch?v=jd_HBTl_Gtw

[meliketanrikulu]

Progress:

To understand the source of the problem, I performed the following tests one by one:

1. To make sure that EKF is the source of the problem:
   I ran the localization completely with NDT. I gave NDT output as initial pose to NDT and published the map to baselink transform with NDT poses. In this way, I completely removed EKF from the system. I compared these results with the default autoware results.
   You can see the results in the video here

   Test Results: As seen in this video, the point cloud shakes up and down in the default autoware (NDT + EKF) when passing through the speed bump. This error does not occur when run with NDT only.

2. Simulation Environment Test: Ensuring that EKF inputs, sensor calibrations or any other vehicle specific problems are not causing the error.
   I created the same scenario in a simulation environment and reproduced the same problem with awsim_labs_sensor_kit and sample_vehicle.
   You can see the results in the video here

   Test Results: In the simulation environment, I also saw that errors occurred when passing over bumps. From here I came to the conclusion that any vehicle specific problem (calibration, input twist data ) is not the source of the problem.
   At the same time, I visualized the changes in pitch angle when it passed through the speed bump, both in the simulation environment and with real life data.I observed similar behavior in both. You can see these results below:

Next Steps:

After making sure that EKF is only the source of the problem, I will try to solve the problem by debugging EKF in the simulation environment.

[meliketanrikulu]

I checked differences between NDT output and EKF output poses. There was a shift on position Z and roll , pitch values. 1D filter using for these values estimation in ekf_localizer. I updated 1D filter parameters and these shifts decreased. (I increased the 1D filter parameters) As a result shift decreased but noise increased

After this changes the wobbling in the point cloud has little bit decreased, but it is not effective enough.
You can see final results in this video.
As a result of these tests, I came to the conclusion that this problem could not be completely solved by parameter optimization.
When I looked at the ekf_localizer package in detail, I saw that the information from the TwistWithCovarianceStamped message was not used when estimating the values ​​calculated with the 1D filter (roll,pitch, position z). In other words, when calculating these values, the angular velocity data from the IMU and the linear velocity data from the vehicle are not used. However, we have these datas and I believe that the problem will be solved when we estimate using them.

[meliketanrikulu]

I tried to use the Roll and Pitch values ​​from the IMU directly in EKF. Also I added linear acceleration calculation for calculating position Z.

1. Simulation Test :
   The problem was completely solved with this method in the simulation environment:
   Test video is here
   NDT and EKF roll and pitch values gave very similar results in simulation environment:
   

2. Real World Test
   In tests performed on the real vehicle, I saw that the results improved, but the problem was not completely solved.
   Test video is here
   

3. GNSS/INS Test :
   In addition, when I performed these tests using GNSS/INS directly (published TF from GNSS/INS and closed EKF and NDT . Vehicle localized only with GNSS/INS ), I saw that the same problem continued. Wobbling problem seems to be the same as when I updated EKF using IMU data directly.
   Test video is here
   Note: The roll and pitch data of GNSS/INS data are exactly the same as the roll and pitch in imu (calculated from orientation field in imu msg).

[meliketanrikulu]

I tested EKF which is IMU added with 3D Distortion Corrector PR
I've seen the results get better as a result. I achieved very good results when IMU added to EKF and 3D Distortion Corrector were used together. However, the problem is not completely solved yet. So I'll keep working on it
The test video link is here

You can see the effect of the step-by-step operations (adding IMU to EKF , adding 3D Distortion Corrector and NDT Only Localization) in the image below. This image shows the maximum wobble of a point cloud on a rectangular traffic sign plate on the map while passing through the speed bump for 4 different test cases.

[meliketanrikulu]

I will create a PR for this issue using the results below as reference.

1. Simulation Test:
   In the tests carried out in the simulation environment, you can see below that using imu directly solves the roll, pitch, and position z in EKF instead of calculating them with the 1D Filter:
   
   Buna ek olarak aşağıdaki videoları inceleyebilirsiniz:
   Default Autoware test video is here
   EKF with IMU Integration test video is here
2. Real World Test:
   Although the same level of performance cannot be achieved in real-world tests due to different factors, there is an improvement. It also seems that the results are further improved by using the 3D Distortion Corrector, which has been newly merged into Autoware.You can find the results below:
   
   Real World Test Video is here

[meliketanrikulu]

I created new PR for solving this problem . Tested with adding delay compensation to roll and pitch.
#8095

[meliketanrikulu]

Related PR is merged. I will close the issue