7、姿态估计算法中的扩展卡尔曼滤波器详解

姿态估计算法中的扩展卡尔曼滤波器详解

1. 卡尔曼滤波器基础与误差分布

在许多系统的状态估计中，卡尔曼滤波器是一种常用的工具。以质量 - 弹簧 - 阻尼系统为例，其误差分布通常为高斯分布。在这种情况下，3σ 边界提供了一种概率保证，即误差有 99.7% 的概率会保持在该边界内。

在实际应用中，真实状态往往是未知的，因此误差也难以确定。但在模拟环境中，真实状态是可以获取的，这样就能对滤波器的性能进行准确评估。从相关的模拟结果图中可以看到，位置误差大部分时间都保持在边界内，并且边界与实际误差历史较为接近；而速度误差则始终在边界内，不过对于给定的时间历程，边界相对较宽。

2. 扩展卡尔曼滤波器（EKF）概述

大多数自主车辆会使用陀螺仪来提供角速度测量值，同时使用光学传感器来提供绝对姿态测量值。陀螺仪的测量值通常会受到白噪声和偏置噪声的影响。卡尔曼滤波器在姿态估计中的主要目的是利用光学传感器和动态模型来估计陀螺仪测量中的偏置误差 β。

估计的角速度可以通过从原始陀螺仪测量值中减去估计的偏置误差来进行校正，公式如下：
[
\hat{\omega}(t_k) = \tilde{\omega}(t_k) - \hat{\beta}(t_k)
]
其中，(\hat{\omega}(t_k)) 是在 (t_k) 时刻估计的角速度，(\hat{\beta}(t_k)) 是在 (t_k) 时刻从待设计的卡尔曼滤波器中估计出的偏置。

3. 误差动态分析

为了得到姿态误差动态的控制微分方程，我们定义误差四元数 (\delta q) 为估计姿态和实际姿态之间的四元数。误差四元数的动