# 扩展卡尔曼滤波器定位器概述

**扩展卡尔曼滤波器定位器**通过将二维车辆动力学模型与输入的自车姿态和自车速度信息相结合，估计出更稳健且噪声更少的机器人姿态和速度。该算法特别适用于快速移动的机器人，例如自动驾驶系统。

## 流程图

Autoware EKF 定位器的整体流程图如下所示。

<p align="center">
  <img src="./media/ekf_flowchart.png" width="800">
</p>

## 功能特性

该软件包包含以下特性：

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

<p align="center">
<img src="./media/ekf_delay_comp.png" width="800">
</p>

<p align="center">
  <img src="./media/ekf_smooth_update.png" width="800">
</p>

<p align="center">
  <img src="./media/calculation_delta_from_pitch.png" width="800">
</p>

## 节点

### 订阅主题

| 名称 | 类型 | 说明 |
| --- | --- | --- |
| `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                |

## 卡尔曼滤波器模型

### 更新函数中的动力学模型

<p align="center">
<img src="./media/ekf_dynamics.png" width="320">
</p>

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

### 时间延迟模型

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

<p align="center">
<img src="./media/delay_model_eq.png" width="320">
</p>

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

## 使用 Autoware NDT 的测试结果

<p align="center">
<img src="./media/ekf_autoware_res.png" width="600">
</p>

## 诊断

<p align="center">
<img src="./media/ekf_diagnostics.png" width="320">
</p>

<p align="center">
<img src="./media/ekf_diagnostics_callback_pose.png" width="320">
</p>
<p align="center">
<img src="./media/ekf_diagnostics_callback_twist.png" width="320">
</p>

### 导致警告状态的条件

- 节点未处于激活状态。
- 通过姿态/速度主题进行测量更新的连续次数超过 `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.