输入消息的时间延迟补偿，能够正确整合具有不同时间延迟的输入信息。这对于高速移动的机器人（如自动驾驶车辆）尤为重要。（见下图）
自动估计偏航角偏差，防止因传感器安装角度误差导致的建模错误，从而提高估计精度。
马氏距离门控，通过概率方法检测异常值，确定哪些输入应该被使用或忽略。
平滑更新，卡尔曼滤波器的测量更新通常在获得测量值时执行，但可能会导致估计值发生较大变化，尤其是对于低频测量。由于算法可以考虑测量时间，因此可以将测量数据分成多部分，并在保持一致性的同时进行平滑整合（见下图）。
根据俯仰角计算垂直修正量，减轻在斜坡上的定位不稳定性。例如，当上坡时，由于 EKF 只考虑 3 自由度（x、y、偏航角），它会表现得好像车辆陷入地面（见“根据俯仰角计算修正量”图的左侧）。因此，EKF 根据公式修正 z 坐标（见“根据俯仰角计算修正量”图的右侧）。

节点

订阅主题

名称	类型	说明
`measured_pose_with_covariance`	`geometry_msgs::msg::PoseWithCovarianceStamped`	带有测量协方差矩阵的输入姿态源。
`measured_twist_with_covariance`	`geometry_msgs::msg::TwistWithCovarianceStamped`	带有测量协方差矩阵的输入速度源。
`initialpose`	`geometry_msgs::msg::PoseWithCovarianceStamped`	EKF 的初始姿态。在启动时，估计的姿态以零值初始化。每当发布此消息时，它都会被初始化。

发布主题

名称	类型	说明
`ekf_odom`	`nav_msgs::msg::Odometry`	估计的里程计。
`ekf_pose`	`geometry_msgs::msg::PoseStamped`	估计的姿态。
`ekf_pose_with_covariance`	`geometry_msgs::msg::PoseWithCovarianceStamped`	带协方差的估计姿态。
`ekf_biased_pose`	`geometry_msgs::msg::PoseStamped`	包括偏航角偏差的估计姿态。
`ekf_biased_pose_with_covariance`	`geometry_msgs::msg::PoseWithCovarianceStamped`	包括偏航角偏差的带协方差的估计姿态。
`ekf_twist`	`geometry_msgs::msg::TwistStamped`	估计的速度。
`ekf_twist_with_covariance`	`geometry_msgs::msg::TwistWithCovarianceStamped`	带协方差的估计速度。
`diagnostics`	`diagnostics_msgs::msg::DiagnosticArray`	诊断信息。

发布的 TF

base_link 从 map 坐标系到估计姿态的变换。

功能

预测

使用给定的预测模型，从先前估计的数据中预测当前机器人的状态。此计算以恒定间隔（predict_frequency [Hz]）调用。预测方程在本页面末尾描述。

测量更新

在更新之前，计算测量输入与预测状态之间的马氏距离，对于马氏距离超过给定阈值的输入，不执行测量更新。

使用最新的测量输入（measured_pose 和 measured_twist）更新预测状态。更新的频率与预测相同，通常为高频率，以便实现平滑的状态估计。

参数描述

参数在 launch/ekf_localizer.launch 中设置。

节点参数

{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json") }}

姿态测量参数

{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/pose_measurement.sub_schema.json") }}

速度测量参数

{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/twist_measurement.sub_schema.json") }}

过程噪声参数

{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/process_noise.sub_schema.json") }}

注意：位置 x 和 y 的过程噪声会根据非线性动力学自动计算。

简单一维滤波器参数

{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json") }}

诊断参数

{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/diagnostics.sub_schema.json") }}

其他参数

{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/misc.sub_schema.json") }}

如何调整 EKF 参数

0. 准备工作

检查姿态和速度消息中的时间戳是否正确设置为传感器时间，因为时间延迟是从这个值计算的。如果由于时钟同步问题难以设置合适的时间，请使用 twist_additional_delay 和 pose_additional_delay 来纠正时间。
检查测量姿态和速度之间的关系是否适当（姿态的导数是否与速度相似）。这种差异主要由单位错误（例如混淆弧度/度）或偏差噪声引起，它会导致较大的估计误差。

1. 调整传感器参数

为每个传感器设置标准差。pose_measure_uncertainty_time 是用于时间戳数据不确定性的参数。您还可以通过调整 *_smoothing_steps 来调整每个观测到的传感器数据的平滑步数。增加步数可以提高估计的平滑度，但也可能对估计性能产生不利影响。

pose_measure_uncertainty_time
pose_smoothing_steps
twist_smoothing_steps

2. 调整过程模型参数

proc_stddev_vx_c：设置为最大线性加速度。
proc_stddev_wz_c：设置为最大角加速度。
proc_stddev_yaw_c：此参数描述偏航角和偏航率之间的相关性。较大的值意味着偏航角的变化与估计的偏航率不相关。如果将其设置为 0，则意味着估计的偏航角变化等于偏航率。通常，应将其设置为 0。
proc_stddev_yaw_bias_c：此参数是偏航角偏差变化率的标准差。在大多数情况下，偏航角偏差是恒定的，因此可以非常小，但必须非零。

3. 调整门控参数

EKF 在通过观测更新之前使用马氏距离进行门控。门控大小由 pose_gate_dist 参数和 twist_gate_dist 决定。如果马氏距离大于此值，则忽略该观测。

此门控过程基于卡方分布的统计检验。根据模型，我们假设姿态的马氏距离遵循 3 自由度的卡方分布，速度的马氏距离遵循 2 自由度的卡方分布。

目前，协方差估计本身的准确性并不高，因此建议将显著性水平设置为非常小的值，以减少因假阳性而拒绝的情况。

显著性水平	2 自由度的阈值	3 自由度的阈值
`10^{-2}`	9.21	11.3
`10^{-3}`	13.8	16.3
`10^{-4}`	18.4	21.1
`10^{-5}`	23.0	25.9
`10^{-6}`	27.6	30.7
`10^{-7}`	32.2	35.4
`10^{-8}`	36.8	40.1
`10^{-9}`	41.4	44.8
`10^{-10}`	46.1	49.5

卡尔曼滤波器模型

更新函数中的动力学模型

其中，\theta_k 表示车辆的航向角，包括安装角度偏差。 b_k 是偏航角偏差的修正项，它被建模为使 (\theta_k + b_k) 成为 base_link 的航向角。姿态估计器预计会在地图坐标系中发布 base_link。然而，由于校准错误，偏航角可能会偏移。此模型补偿了这一误差，提高了估计精度。

时间延迟模型

通过扩展状态 [1]（见第 7.3 节固定滞后平滑）处理测量时间延迟。

注意，尽管由于可以基于扩展状态的特定结构进行解析扩展，维度变大了，但计算复杂度并没有显著变化。

使用 Autoware NDT 的测试结果

诊断

导致警告状态的条件

节点未处于激活状态。
通过姿态/速度主题进行测量更新的连续次数超过 pose_no_update_count_threshold_warn/twist_no_update_count_threshold_warn。
姿态/速度主题的时间戳超出了延迟补偿范围。
姿态/速度主题超出了用于协方差估计的马氏距离范围。
协方差椭圆的长轴或横向方向的大小超过了阈值 warn_ellipse_size 或 warn_ellipse_size_lateral_direction。

导致错误状态的条件

通过姿态/速度主题进行测量更新的连续次数超过 pose_no_update_count_threshold_error/twist_no_update_count_threshold_error。
协方差椭圆的长轴或横向方向的大小超过了阈值 error_ellipse_size 或 error_ellipse_size_lateral_direction。

已知问题

如果使用多个姿态估计器，输入到 EKF 的数据将包括每个源对应的多个偏航角偏差。然而，当前的 EKF 假设只有一个偏航角偏差。因此，当前 EKF 状态中的偏航角偏差 b_k 将没有意义，也无法正确处理这些多个偏航角偏差。因此，未来的工作包括为每个传感器引入带有偏航角估计的偏航角偏差。

参考文献

[1] Anderson, B. D. O., & Moore, J. B. (1979). Optimal filtering. Englewood Cliffs, NJ: Prentice-Hall.