ROS学习一周后

ESKF：基于流形的误差状态卡尔曼滤波推导详解

在 SLAM 和组合导航中，ESKF（Error-State Kalman Filter）是解决旋转（Rotation）非线性问题的标准答案。与直接滤波全量状态（Total State）不同，ESKF 将状态拆分为名义状态（Nominal State）和误差状态（Error State），在切空间（Tangent Space）上进行线性滤波。

1.1 坐标系

• $W$ (World): 世界坐标系（导航系），通常为 ENU（东北天），重力 $\mathbf{g} = [0, 0, -9.81]^T$。
• $B$ (Body): 机体坐标系（IMU 坐标系）。

我们将系统的真值状态 $\mathbf{x}_t$ 分解为名义状态 $\mathbf{x}$ 和误差状态 $\delta \mathbf{x}$，然后各项数据如表格:

变量	含义	维度	组合方式 ($\oplus$)
$\mathbf{p}$	位置 (Position)	$3$	$\mathbf{p}_t = \mathbf{p} + \delta \mathbf{p}$
$\mathbf{v}$	速度 (Velocity)	$3$	$\mathbf{v}_t = \mathbf{v} + \delta \mathbf{v}$
$\mathbf{q}$	姿态 (Quaternion)	$4$	$\mathbf{q}_t = \mathbf{q} \otimes \text{Exp}(\delta \boldsymbol{\theta})$
$\mathbf{b}_a$	加速度计零偏	$3$	$\mathbf{b}_{at} = \mathbf{b}_a + \delta \mathbf{b}_a$
$\mathbf{b}_g$	陀螺仪零偏	$3$	$\mathbf{b}_{gt} = \mathbf{b}_g + \delta \mathbf{b}_g$

1.2 IMU 传感器模型

IMU 的测量值 $\mathbf{a}_m, \boldsymbol{\omega}_m$ 由真值、零偏、噪声组成：

$$\begin{aligned} \mathbf{a}_m &= \mathbf{R}_t^T (\mathbf{a}_t - \mathbf{g}) + \mathbf{b}_{at} + \mathbf{n}_a \\ \boldsymbol{\omega}_m &= \boldsymbol{\omega}_t + \mathbf{b}_{gt} + \mathbf{n}_g \end{aligned}$$

其中：

• $\mathbf{R}_t$: 从 Body 到 World 的旋转矩阵。
• $\mathbf{n}_a, \mathbf{n}_g$: 高斯白噪声（Measurement Noise）。
• 零偏建模为随机游走：$\dot{\mathbf{b}}{at} = \mathbf{n}{ba}, \quad \dot{\mathbf{b}}{gt} = \mathbf{n}{bg}$。

1.3 连续时间运动学方程

真值运动方程 (True State Kinematics)

根据物理定律，真值的微分方程为：

$$\begin{aligned} \dot{\mathbf{p}}_t &= \mathbf{v}_t \\ \dot{\mathbf{v}}_t &= \mathbf{R}_t (\mathbf{a}_m - \mathbf{b}_{at} - \mathbf{n}_a) + \mathbf{g} \\ \dot{\mathbf{q}}_t &= \frac{1}{2} \mathbf{q}_t \otimes (\boldsymbol{\omega}_m - \mathbf{b}_{gt} - \mathbf{n}_g) \\ \dot{\mathbf{b}}_{at} &= \mathbf{n}_{ba} \\ \dot{\mathbf{b}}_{gt} &= \mathbf{n}_{bg} \end{aligned}$$

名义状态运动方程 (Nominal State Kinematics)

名义状态不考虑噪声，且认为当前的零偏估计是准确的：

$$\begin{aligned} \dot{\mathbf{p}} &= \mathbf{v} \\ \dot{\mathbf{v}} &= \mathbf{R} (\mathbf{a}_m - \mathbf{b}_a) + \mathbf{g} \\ \dot{\mathbf{q}} &= \frac{1}{2} \mathbf{q} \otimes (\boldsymbol{\omega}_m - \mathbf{b}_g) \\ \dot{\mathbf{b}}_a &= 0 \\ \dot{\mathbf{b}}_g &= 0 \end{aligned}$$

注： 对于一个绕轴 $\mathbf{u}$ 旋转 $\theta$ 角度的四元数，我们写成：

$$\mathbf{q} = \left[ \cos\left(\frac{\theta}{2}\right), \mathbf{u} \sin\left(\frac{\theta}{2}\right) \right]$$

注意到了吗？这里有一个 $\frac{\theta}{2}$。

• 角速度 $\omega$ 描述的是 $\theta$ 的变化率（即 $\dot{\theta}$）。
• 四元数变化率 $\dot{\mathbf{q}}$ 描述的是 $\cos(\frac{\theta}{2})$ 和 $\sin(\frac{\theta}{2})$ 的变化率。

当你对 $\frac{\theta}{2}$ 求导时，根据链式法则（Chain Rule），$\frac{\theta}{2}$ 里的那个系数 $1/2$ 就会被提出来：

$$\frac{d}{dt} \left( \frac{\theta}{2} \right) = \frac{1}{2} \dot{\theta} = \frac{1}{2} \omega$$

1.4 误差状态运动学推导 (核心)

位置误差 $\delta \dot{\mathbf{p}}$：

$$\delta \dot{\mathbf{p}} = \dot{\mathbf{p}}_t - \dot{\mathbf{p}} = \mathbf{v}_t - \mathbf{v} = \delta \mathbf{v}$$

速度误差 $\delta \dot{\mathbf{v}}$ (关键):

$$\dot{\delta \mathbf{v}} = \dot{\mathbf{v}}_t - \dot{\mathbf{v}}$$

代入真值和名义方程（$\mathbf{g}$ 相互抵消）：

$$\dot{\delta \mathbf{v}} = [\mathbf{R}_t (\mathbf{a}_m - \mathbf{b}_{at} - \mathbf{n}_a)] - [\mathbf{R} (\mathbf{a}_m - \mathbf{b}_a)]$$

线性化近似：

利用小量近似：$\mathbf{R}t \approx \mathbf{R}(\mathbf{I} + [\delta \boldsymbol{\theta}]\times)$，$\mathbf{b}{at} = \mathbf{b}_a + \delta \mathbf{b}_a$。
其中 $[\cdot]\times$ 为反对称矩阵符号。

代入并忽略二阶小量（如 $\delta \boldsymbol{\theta} \cdot \mathbf{n}_a$）：

$$\begin{aligned} \dot{\delta \mathbf{v}} &\approx \mathbf{R}(\mathbf{I} + [\delta \boldsymbol{\theta}]_\times)(\mathbf{a}_m - \mathbf{b}_a - \delta \mathbf{b}_a - \mathbf{n}_a) - \mathbf{R}(\mathbf{a}_m - \mathbf{b}_a) \\ &= \mathbf{R}(\mathbf{a}_m - \mathbf{b}_a) - \mathbf{R}\delta \mathbf{b}_a - \mathbf{R}\mathbf{n}_a + \mathbf{R}[\delta \boldsymbol{\theta}]_\times (\mathbf{a}_m - \mathbf{b}_a) - \mathbf{R}(\mathbf{a}_m - \mathbf{b}_a) \end{aligned}$$

消去首尾项，并利用叉乘性质 $[\mathbf{A}]\times \mathbf{B} = -[\mathbf{B}]\times \mathbf{A}$：

$$\mathbf{R}[\delta \boldsymbol{\theta}]_\times (\mathbf{a}_m - \mathbf{b}_a) = - \mathbf{R} [\mathbf{a}_m - \mathbf{b}_a]_\times \delta \boldsymbol{\theta}$$

最终得到：

$$\dot{\delta \mathbf{v}} = - \mathbf{R} [\mathbf{a}_m - \mathbf{b}_a]_\times \delta \boldsymbol{\theta} - \mathbf{R} \delta \mathbf{b}_a - \mathbf{R} \mathbf{n}_a$$

姿态误差 $\delta \dot{\boldsymbol{\theta}}$ (难点):

根据定义 $\mathbf{q}_t = \mathbf{q} \otimes \text{Exp}(\delta \boldsymbol{\theta})$，两边对时间求导。

左边 (真值)：

$$\dot{\mathbf{q}}_t = \frac{1}{2} \mathbf{q}_t \otimes (\boldsymbol{\omega}_m - \mathbf{b}_g - \delta \mathbf{b}_g - \mathbf{n}_g)$$

右边 (链式法则)：

$$\dot{\mathbf{q}}_t = \dot{\mathbf{q}} \otimes \text{Exp}(\delta \boldsymbol{\theta}) + \mathbf{q} \otimes \frac{d}{dt}\text{Exp}(\delta \boldsymbol{\theta})$$

利用名义方程 $\dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \otimes (\boldsymbol{\omega}_m - \mathbf{b}_g)$ 和近似 $\frac{d}{dt}\text{Exp}(\delta \boldsymbol{\theta}) \approx [\dot{\delta \boldsymbol{\theta}} \times]$（四元数形式）：

$$\dot{\mathbf{q}}_t \approx \frac{1}{2} \mathbf{q} \otimes (\boldsymbol{\omega}_m - \mathbf{b}_g) \otimes (\mathbf{1} + \frac{\delta \boldsymbol{\theta}}{2}) + \mathbf{q} \otimes \frac{\dot{\delta \boldsymbol{\theta}}}{2}$$

令左右两边相等，并经过繁琐的四元数代数消元，最终得到线性化关系：

$$\dot{\delta \boldsymbol{\theta}} = - [\boldsymbol{\omega}_m - \mathbf{b}_g]_\times \delta \boldsymbol{\theta} - \delta \mathbf{b}_g - \mathbf{n}_g$$

零偏误差:

由于零偏是随机游走：

$$\dot{\delta \mathbf{b}}_a = \mathbf{n}_{ba}, \quad \dot{\delta \mathbf{b}}_g = \mathbf{n}_{bg}$$

1.5 误差状态空间方程

将上述微分方程写成矩阵形式：

$$\dot{\delta \mathbf{x}} = \mathbf{F}_c \delta \mathbf{x} + \mathbf{G}_c \mathbf{n}$$

其中状态 $\delta \mathbf{x} = [\delta \mathbf{p}, \delta \mathbf{v}, \delta \boldsymbol{\theta}, \delta \mathbf{b}a, \delta \mathbf{b}_g]^T$，噪声 $\mathbf{n} = [\mathbf{n}_a, \mathbf{n}_g, \mathbf{n}{ba}, \mathbf{n}_{bg}]^T$。

连续时间雅可比矩阵 $\mathbf{F}_c$

$$\mathbf{F}_c = \begin{bmatrix} \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -\mathbf{R}[\mathbf{a}_m - \mathbf{b}_a]_\times & -\mathbf{R} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -[\boldsymbol{\omega}_m - \mathbf{b}_g]_\times & \mathbf{0} & -\mathbf{I} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \end{bmatrix}$$

连续时间噪声矩阵 $\mathbf{G}_c$

$$\mathbf{G}_c = \begin{bmatrix} \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ -\mathbf{R} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & -\mathbf{I} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \end{bmatrix}$$

1.6 离散化 (Discretization)

在代码实现中，我们需要离散形式 $\delta \mathbf{x}_{k+1} = \mathbf{F}_k \delta \mathbf{x}_k + \mathbf{i}_k$, 利用一阶泰勒展开（$\Delta t$ 为 IMU 采样间隔）：

$$\mathbf{F}_k = \mathbf{I} + \mathbf{F}_c \Delta t$$ $$\mathbf{F}_k = \begin{bmatrix} \mathbf{I} & \mathbf{I}\Delta t & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{I} & -\mathbf{R}[\mathbf{a}_m - \mathbf{b}_a]_\times \Delta t & -\mathbf{R}\Delta t & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{R}^T\{\dots\} \text{或 } \mathbf{I} & \mathbf{0} & -\mathbf{I}\Delta t \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \end{bmatrix}$$

离散噪声协方差 $\mathbf{Q}_k$：

$$\mathbf{Q}_k \approx \mathbf{G}_c \mathbf{Q}_{imu} \mathbf{G}_c^T \Delta t$$

其中 $\mathbf{Q}{imu} = \text{diag}(\sigma_a^2, \sigma_g^2, \sigma{ba}^2, \sigma_{bg}^2)$。

ESKF 完整流程

Step 1: 预测 (IMU 频率, 100Hz+)

1. 名义状态更新：利用非线性公式积分 $\mathbf{x}_{k+1}$。
2. 误差协方差更新：$\mathbf{P}_{k+1} = \mathbf{F}_k \mathbf{P}_k \mathbf{F}_k^T + \mathbf{Q}_k$。

Step 2: 观测更新 (GPS/Lidar 频率, 10Hz)

当观测 $\mathbf{y}$ 到来（例如 GPS 位置）：

1. 计算残差：$\mathbf{r} = \mathbf{y}{GPS} - \mathbf{p}{nominal}$。
2. 构建 Jacobian：$\mathbf{H} = [\mathbf{I}, \mathbf{0}, \mathbf{0}, \mathbf{0}, \mathbf{0}]$。
3. 计算增益：$\mathbf{K} = \mathbf{P} \mathbf{H}^T (\mathbf{H} \mathbf{P} \mathbf{H}^T + \mathbf{V})^{-1}$。
4. 计算误差：$\delta \mathbf{x} = \mathbf{K} \mathbf{r}$。

Step 3: 误差注入 (Injection)

将估计出的误差 $\delta \mathbf{x}$ 融合进名义状态：

$$\begin{aligned} \mathbf{p} &\leftarrow \mathbf{p} + \delta \mathbf{p} \\ \mathbf{v} &\leftarrow \mathbf{v} + \delta \mathbf{v} \\ \mathbf{q} &\leftarrow \mathbf{q} \otimes \text{Exp}(\delta \boldsymbol{\theta}) \quad \text{// 旋转更新是乘法} \\ \mathbf{b} &\leftarrow \mathbf{b} + \delta \mathbf{b} \end{aligned}$$

Step 4: 重置 (Reset)

误差已经注入名义状态，因此误差状态清零，但其不确定性需要保留：

$$\delta \mathbf{x} \leftarrow \mathbf{0}$$ $$\mathbf{P} \leftarrow (\mathbf{I} - \mathbf{K}\mathbf{H}) \mathbf{P}$$

(严谨的 ESKF 这里需要进行 Jacobian 投影，但在小误差假设下通常近似为上述公式)