ekf_localizer publishes unbiased pose as biased pose

[yvzksgl]

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

ekf_localizer confused about biased_pose and pose.

Expected behavior

https://github.com/autowarefoundation/autoware.universe/blob/566c57f671c4aa01cd0a52b214ffbbda16ef47f9/localization/ekf_localizer/src/ekf_module.cpp#L90-L92 biased_yaw and yaw variables should be exchanged.

Actual behavior

biased_yaw is unbiased and yaw is biased I will try to explain in a detailed way.

Steps to reproduce

https://github.com/autowarefoundation/autoware.universe/blob/566c57f671c4aa01cd0a52b214ffbbda16ef47f9/localization/ekf_localizer/src/ekf_localizer.cpp#L228-L231 biased_pose and pose are assigned with calling getCurrentPose(), https://github.com/autowarefoundation/autoware.universe/blob/566c57f671c4aa01cd0a52b214ffbbda16ef47f9/localization/ekf_localizer/src/ekf_module.cpp#L97-L103 Everyting looks fine up here, however when loking at https://github.com/autowarefoundation/autoware.universe/blob/566c57f671c4aa01cd0a52b214ffbbda16ef47f9/localization/ekf_localizer/src/ekf_module.cpp#L90-L92 biased_yaw variable contains unbiased yaw and yaw contains biased yaw.

Versions

No response

Possible causes

No response

Additional context

I read the https://github.com/autowarefoundation/autoware.universe/issues/1403, since, it's been a while from the issue I cannot compare it with the current architecture.

[KYabuuchi]

Hi! Thanks for reporting it. It may be wrong as you mentioned. However, the code regarding bias is a bit complicated, so I will carefully read the code to make sure.

[KYabuuchi]

@yvzksgl I checked the code again and found that the current implementation is correct.

Firstly, the internally stored yaw in the EKF includes a yaw bias. (The name IDX::YAW is confusing.) Specifically, kalman_filter_.getXelement(IDX::YAW) contains the yaw bias of the LiDAR calibration. To make this the YAW of the base_link, it is necessary to add the YAW_BIAS. Please refer to the state transition equation: https://github.com/autowarefoundation/autoware.universe/blob/89d4462f45e3af4773f6b930d680cd7ff07f92ea/localization/ekf_localizer/src/state_transition.cpp#L43-L50

Additionally, in the measurement pose update below, you can observe that the NDT's YAW angle is used directly. (Note that it is normalized to ensure a difference with ekf_yaw less thatn 2π, but no bias correction is applied.) This is consistent with EKF storing biased yaw internally, since the raw NDT output contains a calibration yaw bias.

https://github.com/autowarefoundation/autoware.universe/blob/89d4462f45e3af4773f6b930d680cd7ff07f92ea/localization/ekf_localizer/src/ekf_module.cpp#L227-L235

[KYabuuchi]

@idorobotics @yvzksgl Hi. What is the current status of this issue? Is someone in the working group working on improving the code?

If my explanation above is fine, I'll create a PR to improve the code so that users don't misunderstand it.

[yvzksgl]

Hello @KYabuuchi, sorry for my late response. Last week I had to go on a business travel and this week I was trying to understand where the yaw_bias is updated. It looks all good to me now. If you don't mind I just want to ask a question about, do you know what does mean

multiple inputs with yaw estiamation

in here https://github.com/autowarefoundation/autoware.universe/blob/566c57f671c4aa01cd0a52b214ffbbda16ef47f9/localization/ekf_localizer/README.md?plain=1#L235

[KYabuuchi]

@yvzksgl No problem. I am glad my explanation makes sense to you.

Regarding multiple inputs with yaw estimation, please refer to the figure below. If multiple pose_estimators are used, the input poses to the EKF will contain multiple yaw biases. However, the current EKF assumes the existence of only one yaw_bias. Therefore, it cannot correctly handle these multiple yaw biases.

If we are using a single NDT, that is not a problem.

[yvzksgl]

I see, thank you for your great explanation and patience!

[KYabuuchi]

The PR which improves documentation is merged https://github.com/autowarefoundation/autoware.universe/pull/5834