EKF gives bad results in speed bump and after a sudden brake or turning scenario

[meliketanrikulu]

Checklist

• [X] I've read the contribution guidelines.
• [X] I've searched other issues and no duplicate issues were found.
• [X] I'm convinced that this is not my fault but a bug.

Description

We encountered the problem of localization shaking when going through speed bump. I checked the inputs for EKF. There doesn't appear to be any delays or errors regarding velocity data. NDT orientation y and orientation y values from GNSS overlap with each other and the values come as they should, but the EKF output responds late because of the sudden change. This causes the localization to shake.

Expected behavior

The map and point cloud must always be on top of each other. I expect not to see any shaking

Actual behavior

I see that the localization shifts on the z axis when going through speed bump. I observe the distortion in localization by checking whether the concatenated point cloud match with the map.

Here is the test videos:

Test Video1 Test Video2

At these moments, when I look at the orientation data coming from GNSS, the orientation output of NDT, and the orientation data output of EKF, I see that NDT and GNSS give close and accurate data. However, when I look at the orientation values in the EKF output, I see that they react late. (The value I take into consideration here is the pitch value because the distortion occurs on the pitch.)

-- /localization/pose_estimator/pose/orientation/pitch_deg --> NDT Output (Green ) -- /localization/pose_twist_fusion_filter/pose/orientation/pitch_deg --> EKF Output (Yellow) -- /sensing/gnss/pose/orientation/pitch_deg --> GNSS Output (Purple)

Steps to reproduce

1.) Bag files and videos are here: https://drive.google.com/drive/folders/1Nvj1HkRecVttlfBPTliMTGJc7OGRjyYj?usp=sharing

2.) Sensor kit: https://github.com/leo-drive/golf_sensor_kit_launch

3.) Individual Params: https://github.com/leo-drive/autoware_individual_params

4.)PC Map: https://drive.google.com/file/d/1ltgV-rD0a6O7Nv3uxrzs47OFcrcX4DrA/view?usp=drive_link

5.) EKF parameters are here

https://github.com/leo-drive/autoware_launch/blob/golf/autoware_launch/config/localization/ekf_localizer.param.yaml

Versions

No response

Possible causes

I know that EKF filters sudden changes because it processes data from the past, but working this way will cause many problems and needs to be solved.

Additional context

No response

[kminoda]

One thing you could do is to tune the parameters for Simple1DFilter in ekf_localizer. Currently it's not prepared as a default parameters, so you need to edit the code to add a parameters here: https://github.com/autowarefoundation/autoware.universe/blob/bae5ea79c0879d02aab728e355205a0fb17e6fdc/localization/ekf_localizer/src/ekf_localizer.cpp#L98-L100

[meliketanrikulu]

@kminoda Thank you for your response. I appreciate your help. The shift problem in EKF was solved when I played with these parameters (pitch_filter_proc_dev parameter affected the result). That's why I created two PRs so that I could change them in the parameter file.

https://github.com/autowarefoundation/autoware_launch/pull/710

https://github.com/autowarefoundation/autoware.universe/pull/5707

While localizing with NDT, the shift between EKF decreased from 400 ms to 200 ms. Since the frequency of NDT is low, I think it may be normal that this is not 0. Because when I fed GNSS data to EKF at 100 Hz with new parameters, this shift disappeared. There is no delay or shift anymore. However, this problem was not solved when I localized both with GNSS and NDT. That's why I'm taking a look at other problems in my system right now.

[KYabuuchi]

Since all relevant PRs have been merged, I close this issue. Thanks for all of your support.