Unleashing Robotics: Mastering Quaternion Kinematics with Python-Chapter4-1（原创系列教程）ESKF与系统误差状态运动学（1）

第4章:ESKF与IMU驱动系统的误差状态运动学1（Error-State Kalman Filter and IMU-Driven System Error Kinematics）

Unleashing Robotics: Mastering Quaternion Kinematics with Python - Chapter4(原创系列教程)
第4章:ESKF与IMU驱动系统的误差状态运动学1（Error-State Kalman Filter and IMU-Driven System Error Kinematics）
本系列教程禁止转载，主要是为了有不同见解的同学可以方便联系我，我的邮箱 fanzexuan135@163.com

终于讲到ESKF了，由于本章内容太多，超过blog字数限制，我分开两部分讲解，正文开始：

4.1 为什么要写这一章节

通过前三章的学习，你已经具备了进入这个复杂且最重要内容的基础知识，开始吧。

我们希望使用Hamilton四元数表示空间中的方向或姿态,为一个集成了加速度计和陀螺仪读数(带有bias和噪声)的惯性系统编写误差状态方程。

加速度计和陀螺仪读数通常来自惯性测量单元(IMU)。集成IMU读数会导致推算定位系统,随时间漂移。避免漂移的方法是将这些信息与绝对位置读数(如GPS或视觉)融合。

误差状态卡尔曼滤波器(ESKF)是我们可以用于此目的的工具之一。在卡尔曼滤波范式中,ESKF最显著的优点有:

• 方向误差状态是最小的(即参数数量与自由度相同),避免了过参数化(或冗余)及其导致的协方差矩阵奇异的风险,这通常是由于强加约束而导致的。

• 误差状态系统始终在原点附近操作,因此远离可能的参数奇异性、万向锁问题等,保证了线性化的有效性始终成立。

• 误差状态始终很小,这意味着所有二阶乘积都可以忽略。这使得计算Jacobian矩阵变得非常容易和快速。有些Jacobian矩阵甚至可能是常数或等于可用的状态量。

• 误差动力学是缓慢的,因为所有大信号动力学都已集成到标称状态中。这意味着我们可以以低于预测的频率应用KF校正(这是观察误差的唯一方法)。

4.2 误差状态卡尔曼滤波器解释

在误差状态滤波器公式中,我们讨论真实状态、标称状态和误差状态值,真实状态表示为标称状态和误差状态的合适组合(线性和、四元数乘积或矩阵乘积)。我们认为标称状态是大信号(可以非线性方式积分),而误差状态是小信号(因此可以线性积分并适合线性高斯滤波)。

误差状态滤波器可以解释如下。一方面,高频IMU数据uₘ被集成到标称状态x中。这个标称状态没有考虑噪声项w和其他可能的模型缺陷。因此,它会累积误差。这些误差被收集在误差状态δx中,并用误差状态卡尔曼滤波器(ESKF)估计,这一次纳入了所有噪声和扰动。误差状态由小信号量组成,其演化函数由(时变)线性动态系统正确定义,其动态、控制和测量矩阵是根据标称状态的值计算的。与标称状态的积分并行,ESKF预测误差状态的高斯估计。它只预测,因为目前没有其他测量信息可用于校正这些估计。滤波器校正是在到达IMU以外的信息(例如GPS、视觉等)时执行的,这些信息能够使误差可观察,并且通常发生在比积分阶段低得多的频率。这种校正提供了误差状态的后验高斯估计。在此之后,将误差状态的均值注入标称状态,然后重置为零。误差状态的协方差矩阵被方便地更新以反映这种重置。系统就这样永远运行下去。

下面我们通过一个具体的四足机器人的应用案例,来说明ESKF在实际系统中的应用。这个案例来自Bledt等人2018年IROS的论文[1],他们提出了一种使用IMU和腿部运动学的四足机器人状态估计方法。

该方法的核心是一个ESKF,其中IMU提供标称状态的预测,而腿部运动学提供位置和速度的观测值用于校正。具体来说:

• 标称状态包括机器人的位置、速度、方向(四元数)、IMU偏差。它通过积分IMU数据来传播。

• 误差状态包括上述标称状态的误差。它在每个IMU积分步骤中也被传播,但使用线性化的动力学。

• 当一条腿着地时,根据腿部运动学计算的足端位置,可以估计出机器人的位置和速度。这个信息被用作ESKF的观测值来校正误差状态。

• 校正后的误差状态被注入标称状态,并重置误差状态及其协方差。

下面是一个简化版的Python代码,展示了这个过程:

# 标称状态传播
def predict_nominal_state(nominal_state, imu_data, dt):
    # 使用IMU数据和积分时间步长dt传播标称状态
    # ...

# 误差状态传播
def predict_error_state(nominal_state, error_state_cov, dt):
    # 计算误差状态的线性化动力学矩阵F和G
    # 传播误差状态协方差: P = F * P * F.T + G * Q * G.T
    # ...
    return error_state_cov

# ESKF校正步骤  
def correct_eskf(nominal_state, error_state_cov, leg_odometry):
    # 计算观测值和观测矩阵H
    # 计算卡尔曼增益: K = P * H.T * (H * P * H.T + R)^-1
    # 更新误差状态: dx = K * (leg_odometry - predicted_odometry)
    # 注入误差状态到标称状态: nominal_state += dx
    # 更新误差状态协方差: P = (I - K * H) * P
    # 重置误差状态: dx = 0
    # ...
    return nominal_state, error_state_cov

# 主循环
while True:
    # IMU积分
    predict_nominal_state(nominal_state, imu_data, dt)
    error_state_cov = predict_error_state(nominal_state, error_state_cov, dt)

    # 如果有腿部里程计信息,执行ESKF校正
    if leg_odometry_available:
        nominal_state, error_state_cov = correct_eskf(nominal_state, error_state_cov, leg_odometry)

这个例子展示了如何使用ESKF融合IMU和腿部里程计信息,以估计四足机器人的状态。这部分细节不懂没关系，下面小节会详细介绍，主要是让你了解ESKF的核心思想:使用IMU进行标称状态预测,使用其他传感器信息校正误差状态,然后将误差注入标称状态并重置。

ESKF是一种强大的技术,可以融合IMU和其他传感器,以获得准确和鲁棒的状态估计。它在足式机器人和其他移动机器人中得到了广泛的应用。掌握ESKF的原理和实现,对于从事这些机器人的状态估计工作非常重要。

参考文献:
[1] Bledt, Gerardo, et al. “Contact model fusion for event-based locomotion in unstructured terrains.” 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.

4.3 连续时间的系统运动学

所有涉及的变量定义总结在表3中。关于约定,有两个重要的决定值得一提:

• 角速率 $\mathbf{\omega }$ 是相对于标称四元数局部定义的。这允许我们直接使用陀螺仪测量值 ${\mathbf{\omega }}_m$,因为它们提供机体参考的角速率。
• 角度误差 $\delta \mathbf{\theta }$ 也是相对于标称方向局部定义的。这不一定是最佳的处理方式,但它对应于大多数IMU集成工作中的选择——我们可以称之为经典方法。有证据表明[2],全局定义的角度误差具有更好的性质。这也将在本文的第7节中探讨,但这里的大部分发展、示例和算法都基于这个局部定义的角度误差。

以下是所有变量的定义表:

	真实值	标称值	误差值	组合关系	测量值	噪声
完整状态	${\mathbf{x}}_t$	$\mathbf{x}$	$\delta \mathbf{x}$	${\mathbf{x}}_t=\mathbf{x}\oplus \delta \mathbf{x}$
位置	${\mathbf{p}}_t$	$\mathbf{p}$	$\delta \mathbf{p}$	${\mathbf{p}}_t=\mathbf{p}+\delta \mathbf{p}$
速度	${\mathbf{v}}_t$	$\mathbf{v}$	$\delta \mathbf{v}$	${\mathbf{v}}_t=\mathbf{v}+\delta \mathbf{v}$
四元数	${\mathbf{q}}_t$	$\mathbf{q}$	$\delta \mathbf{q}$	${\mathbf{q}}_t=\mathbf{q}\otimes \delta \mathbf{q}$
旋转矩阵	${\mathbf{R}}_t$	$\mathbf{R}$	$\delta \mathbf{R}$	${\mathbf{R}}_t=\mathbf{R}\delta \mathbf{R}$
角度向量			$\delta \mathbf{\theta }$	$\delta \mathbf{q}=e^{\delta \mathbf{\theta }/2},\delta \mathbf{R}=e^{[\delta \mathbf{\theta }{]}_{\times }}$
加速度计偏差	${\mathbf{a}}_{b,t}$	${\mathbf{a}}_b$	$\delta {\mathbf{a}}_b$	${\mathbf{a}}_{b,t}={\mathbf{a}}_b+\delta {\mathbf{a}}_b$		${\mathbf{a}}_w$
陀螺仪偏差	${\mathbf{\omega }}_{b,t}$	${\mathbf{\omega }}_b$	$\delta {\mathbf{\omega }}_b$	${\mathbf{\omega }}_{b,t}={\mathbf{\omega }}_b+\delta {\mathbf{\omega }}_b$		${\mathbf{\omega }}_w$
重力向量	${\mathbf{g}}_t$	$\mathbf{g}$	$\delta \mathbf{g}$	${\mathbf{g}}_t=\mathbf{g}+\delta \mathbf{g}$
加速度	${\mathbf{a}}_t$				${\mathbf{a}}_m$	${\mathbf{a}}_n$
角速率	${\mathbf{\omega }}_t$				${\mathbf{\omega }}_m$	${\mathbf{\omega }}_n$

4.3.1 真实状态的运动学

真实的运动学方程为:

$$\begin{array}{rl}{\dot{\mathbf{p}}}_t& ={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t& ={\mathbf{a}}_t\\ {\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t\\ {\dot{\mathbf{a}}}_{b,t}& ={\mathbf{a}}_w\\ {\dot{\mathbf{\omega }}}_{b,t}& ={\mathbf{\omega }}_w\\ {\dot{\mathbf{g}}}_t& =0\end{array}$$

这里,真实加速度 ${\mathbf{a}}_t$ 和角速率 ${\mathbf{\omega }}_t$ 是通过IMU以机体坐标系下的带噪声的传感器读数 ${\mathbf{a}}_m$ 和 ${\mathbf{\omega }}_m$ 的形式获得的,即

$$\begin{array}{rl}{\mathbf{a}}_m& ={\mathbf{R}}_t^{\top }({\mathbf{a}}_t-{\mathbf{g}}_t)+{\mathbf{a}}_{b,t}+{\mathbf{a}}_n\\ {\mathbf{\omega }}_m& ={\mathbf{\omega }}_t+{\mathbf{\omega }}_{b,t}+{\mathbf{\omega }}_n\end{array}$$

其中 R t = R { q t } \mathbf{R}_t = \mathbf{R}\{\mathbf{q}_t\} Rt​=R{qt​}。有了这些,真实值可以分离出来:

$$\begin{array}{rl}{\mathbf{a}}_t& ={\mathbf{R}}_t({\mathbf{a}}_m-{\mathbf{a}}_{b,t}-{\mathbf{a}}_n)+{\mathbf{g}}_t\\ {\mathbf{\omega }}_t& ={\mathbf{\omega }}_m-{\mathbf{\omega }}_{b,t}-{\mathbf{\omega }}_n\end{array}$$

代入上面的式子,得到运动学系统:

$$\begin{array}{rl}{\dot{\mathbf{p}}}_t& ={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t& ={\mathbf{R}}_t({\mathbf{a}}_m-{\mathbf{a}}_{b,t}-{\mathbf{a}}_n)+{\mathbf{g}}_t\\ {\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes ({\mathbf{\omega }}_m-{\mathbf{\omega }}_{b,t}-{\mathbf{\omega }}_n)\\ {\dot{\mathbf{a}}}_{b,t}& ={\mathbf{a}}_w\\ {\dot{\mathbf{\omega }}}_{b,t}& ={\mathbf{\omega }}_w\\ {\dot{\mathbf{g}}}_t& =0\end{array}$$

我们可以将其表示为 ${\dot{\mathbf{x}}}_t={\mathbf{f}}_t({\mathbf{x}}_t,\mathbf{u},\mathbf{w})$。这个系统具有状态 ${\mathbf{x}}_t$,由IMU带噪声读数 ${\mathbf{u}}_m$ 驱动,并受白高斯噪声 $\mathbf{w}$ 的扰动,它们都定义为

$${\mathbf{x}}_t=\left[\begin{array}{c}{\mathbf{p}}_t\\ {\mathbf{v}}_t\\ {\mathbf{q}}_t\\ {\mathbf{a}}_{b,t}\\ {\mathbf{\omega }}_{b,t}\\ {\mathbf{g}}_t\end{array}\right],\mathbf{u}=\left[\begin{array}{c}{\mathbf{a}}_m-{\mathbf{a}}_n\\ {\mathbf{\omega }}_m-{\mathbf{\omega }}_n\end{array}\right],\mathbf{w}=\left[\begin{array}{c}{\mathbf{a}}_w\\ {\mathbf{\omega }}_w\end{array}\right]$$

需要注意的是,在上述公式中,重力向量 ${\mathbf{g}}_t$ 将由滤波器估计。它有一个恒定的演化方程 ${\dot{\mathbf{g}}}_t=0$,这对应于已知恒定的量。该系统从固定且任意已知的初始方向 ${\mathbf{q}}_t(t=0)={\mathbf{q}}_0$ 开始,它通常不在水平面内,因此初始重力向量通常是未知的。为简单起见,通常取 ${\mathbf{q}}_0=(1,0,0,0)$,因此 R 0 = R { q 0 } = I \mathbf{R}_0=\mathbf{R}\{\mathbf{q}_0\}=\mathbf{I} R0​=R{q0​}=I。我们估计在 ${\mathbf{q}}_0$ 帧中表示的 ${\mathbf{g}}_t$,而不是在水平帧中表示的 ${\mathbf{q}}_t$,这样方向的初始不确定性就转移到了重力方向的初始不确定性上。我们这样做是为了提高线性度:事实上,方程 ${\dot{\mathbf{v}}}_t={\mathbf{R}}_t({\mathbf{a}}_m-{\mathbf{a}}_{b,t}-{\mathbf{a}}_n)+{\mathbf{g}}_t$ 现在在 ${\mathbf{g}}_t$ 中是线性的,${\mathbf{g}}_t$ 携带了所有的不确定性,而初始方向 ${\mathbf{q}}_0$ 是已知的,没有不确定性,因此 ${\mathbf{q}}_t$ 从无不确定性开始。一旦估计出重力向量,就可以恢复水平面,并且如果需要,可以重新调整整个状态和恢复的运动轨迹以反映估计的水平面。参见[3]了解进一步的理由。当然这是可选的,读者可以自由地从系统中删除所有与重力相关的方程,并采用更经典的方法,即考虑 $\mathbf{g}\equiv (0,0,-9.8xx)$,其中xx是实验现场重力向量的适当小数位,以及不确定的初始方向 ${\mathbf{q}}_0$。

4.3.2 标称状态运动学

标称状态运动学对应于没有噪声或扰动的建模系统,

$$\begin{array}{rl}\dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b)+\mathbf{g}\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes ({\mathbf{\omega }}_m-{\mathbf{\omega }}_b)\\ {\dot{\mathbf{a}}}_b& =0\\ {\dot{\mathbf{\omega }}}_b& =0\\ \dot{\mathbf{g}}& =0\end{array}$$

4.3.3 误差状态运动学

目标是确定误差状态的线性化动力学。对于每个状态方程,我们写出其组合关系(表3中),求解误差状态并简化所有二阶无穷小量。我们在这里给出完整的误差状态动态系统,然后进行评论和证明。

$$\begin{array}{rl}\delta \dot{\mathbf{p}}& =\delta \mathbf{v}\\ \delta \dot{\mathbf{v}}& =-\mathbf{R}[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-\mathbf{R}{\mathbf{a}}_n\\ \delta \dot{\mathbf{\theta }}& =-[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b{]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n\\ \delta {\dot{\mathbf{a}}}_b& ={\mathbf{a}}_w\\ \delta {\dot{\mathbf{\omega }}}_b& ={\mathbf{\omega }}_w\\ \delta \dot{\mathbf{g}}& =0\end{array}$$

位置、两个偏差和重力误差的方程 $\delta \dot{\mathbf{p}}=\delta \mathbf{v}$, $\delta {\dot{\mathbf{a}}}_b={\mathbf{a}}_w$, $\delta {\dot{\mathbf{\omega }}}_b={\mathbf{\omega }}_w$ 和 $\delta \dot{\mathbf{g}}=0$ 是从线性方程导出的,它们的误差状态动力学是微不足道的。

例如,考虑真实位置方程 ${\dot{\mathbf{p}}}_t={\mathbf{v}}_t$ 和标称位置方程 $\dot{\mathbf{p}}=\mathbf{v}$,它们的组合 ${\mathbf{p}}_t=\mathbf{p}+\delta \mathbf{p}$ 来自表格,求解 $\delta \dot{\mathbf{p}}$ 以得到 $\delta \dot{\mathbf{p}}=\delta \mathbf{v}$。

速度和方向误差的方程 $\delta \dot{\mathbf{v}}=-\mathbf{R}[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-\mathbf{R}{\mathbf{a}}_n$ 和 $\delta \dot{\mathbf{\theta }}=-[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b{]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n$ 需要对非线性方程 ${\dot{\mathbf{v}}}_t={\mathbf{R}}_t({\mathbf{a}}_m-{\mathbf{a}}_{b,t}-{\mathbf{a}}_n)+{\mathbf{g}}_t$ 和 ${\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes ({\mathbf{\omega }}_m-{\mathbf{\omega }}_{b,t}-{\mathbf{\omega }}_n)$ 进行一些非平凡的处理,以获得线性化的动力学。它们的证明在下面两节中给出。

方程 $\delta \dot{\mathbf{v}}=-\mathbf{R}[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-\mathbf{R}{\mathbf{a}}_n$:线性速度误差。 我们希望确定 $\delta \dot{\mathbf{v}}$,即速度误差的动力学。我们从以下关系开始

$$\begin{array}{rl}{\mathbf{R}}_t& =\mathbf{R}(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times })+\mathcal{O}(\Vert \delta \mathbf{\theta }{\Vert }^2)\\ \dot{\mathbf{v}}& =\mathbf{R}{\mathbf{a}}_B+\mathbf{g}\end{array}$$

其中第一个方程是 ${\mathbf{R}}_t$ 的小信号近似,在第二个方程中我们重写了 $\dot{\mathbf{v}}=\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b)+\mathbf{g}$,但引入了 ${\mathbf{a}}_B$ 和 $\delta {\mathbf{a}}_B$,定义为机体坐标系中的大信号和小信号加速度,

$$\begin{array}{rl}{\mathbf{a}}_B& \triangleq {\mathbf{a}}_m-{\mathbf{a}}_b\\ \delta {\mathbf{a}}_B& \triangleq -\delta {\mathbf{a}}_b-{\mathbf{a}}_n\end{array}$$

这样我们就可以将惯性坐标系中的真实加速度写成大小信号项的组合,

$${\mathbf{a}}_t={\mathbf{R}}_t({\mathbf{a}}_B+\delta {\mathbf{a}}_B)+\mathbf{g}+\delta \mathbf{g}$$

我们通过以两种不同形式(左右两边的发展)写出 ${\dot{\mathbf{v}}}_t$ 的表达式 ${\dot{\mathbf{v}}}_t={\mathbf{R}}_t({\mathbf{a}}_m-{\mathbf{a}}_{b,t}-{\mathbf{a}}_n)+{\mathbf{g}}_t$ 来进行处理,其中 $\mathcal{O}(\Vert \delta \mathbf{\theta }{\Vert }^2)$ 项已被忽略,

$$\begin{array}{rl}\dot{\mathbf{v}}+\delta \dot{\mathbf{v}}& ={\dot{\mathbf{v}}}_t=\mathbf{R}(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times })({\mathbf{a}}_B+\delta {\mathbf{a}}_B)+\mathbf{g}+\delta \mathbf{g}\\ \mathbf{R}{\mathbf{a}}_B+\mathbf{g}+\delta \dot{\mathbf{v}}& =\mathbf{R}{\mathbf{a}}_B+\mathbf{R}\delta {\mathbf{a}}_B+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }{\mathbf{a}}_B+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }\delta {\mathbf{a}}_B+\mathbf{g}+\delta \mathbf{g}\end{array}$$

这导致了从左右两边移除 $\mathbf{R}{\mathbf{a}}_B+\mathbf{g}$ 后的结果

$$\delta \dot{\mathbf{v}}=\mathbf{R}\delta {\mathbf{a}}_B+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }{\mathbf{a}}_B+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }\delta {\mathbf{a}}_B+\delta \mathbf{g}$$

消去二阶项并重组一些叉积(利用 $[\mathbf{a}{]}_{\times }\mathbf{b}=-[\mathbf{b}{]}_{\times }\mathbf{a}$),我们得到

$$\delta \dot{\mathbf{v}}=\mathbf{R}\delta {\mathbf{a}}_B-\mathbf{R}[{\mathbf{a}}_B{]}_{\times }\delta \mathbf{\theta }+\delta \mathbf{g}$$

然后,回想 ${\mathbf{a}}_B\triangleq {\mathbf{a}}_m-{\mathbf{a}}_b$ 和 $\delta {\mathbf{a}}_B\triangleq -\delta {\mathbf{a}}_b-{\mathbf{a}}_n$,

$$\delta \dot{\mathbf{v}}=\mathbf{R}(-[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{a}}_b-{\mathbf{a}}_n)+\delta \mathbf{g}$$

经过适当的重新整理,导致线性速度误差的动力学,

$$\begin{gathered} \delta \dot{\mathbf{v}}=-\mathbf{R}[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\delta \mathbf{\theta }-\mathbf{R} \\ delta{\mathbf{a}}_b+\delta \mathbf{g}-\mathbf{R}{\mathbf{a}}_n \end{gathered}$$

为了进一步清理这个表达式,我们通常可以假设加速度计噪声是白的、不相关的和各向同性的,

$$\begin{array}{rl}\mathbb{E}[{\mathbf{a}}_n]& =0\\ \mathbb{E}[{\mathbf{a}}_n{\mathbf{a}}_n^{\top }]& ={\sigma }_a^2\mathbf{I}\end{array}$$

即协方差椭球是以原点为中心的球体,这意味着它的均值和协方差矩阵在旋转下是不变的(证明:$\mathbb{E}[\mathbf{R}{\mathbf{a}}_n]=\mathbf{R}\mathbb{E}[{\mathbf{a}}_n]=0$ 和 $\mathbb{E}[(\mathbf{R}{\mathbf{a}}_n)(\mathbf{R}{\mathbf{a}}_n{)}^{\top }]=\mathbf{R}\mathbb{E}[{\mathbf{a}}_n{\mathbf{a}}_n^{\top }]{\mathbf{R}}^{\top }=\mathbf{R}{\sigma }_a^2\mathbf{I}{\mathbf{R}}^{\top }={\sigma }_a^2\mathbf{I}$)。然后我们可以重新定义加速度计噪声向量,绝对不会产生任何后果,根据

$${\mathbf{a}}_n\leftarrow \mathbf{R}{\mathbf{a}}_n$$

这给出了

$$\delta \dot{\mathbf{v}}=-\mathbf{R}[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-{\mathbf{a}}_n$$

方程 $\delta \dot{\mathbf{\theta }}=-[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b{]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n$:方向误差。 我们希望确定 $\delta \dot{\mathbf{\theta }}$,即角度误差的动力学。我们从以下关系开始

$$\begin{array}{rl}{\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }\end{array}$$

它们分别是四元数导数的真实定义和标称定义。

与加速度一样,为了清晰起见,我们将角速率中的大小信号项分组,

$$\begin{array}{rl}\mathbf{\omega }& \triangleq {\mathbf{\omega }}_m-{\mathbf{\omega }}_b\\ \delta \mathbf{\omega }& \triangleq -\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n\end{array}$$

这样 ${\mathbf{\omega }}_t$ 就可以写成一个标称部分和一个误差部分,

$${\mathbf{\omega }}_t=\mathbf{\omega }+\delta \mathbf{\omega }$$

我们通过两种不同的方式(左右两边的发展)来计算 ${\dot{\mathbf{q}}}_t$,

$$\begin{array}{rl}\dot{(\mathbf{q}\otimes \delta \mathbf{q})}& ={\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t\\ \dot{\mathbf{q}}\otimes \delta \mathbf{q}+\mathbf{q}\otimes \delta \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \delta \mathbf{q}\otimes {\mathbf{\omega }}_t\end{array}$$

简化前导 $\mathbf{q}$ 并分离出 $\delta \dot{\mathbf{q}}$,我们得到

$$\begin{array}{rlrlr}{\dot{\mathbf{p}}}_t& ={\mathbf{v}}_t& & & \\ {\dot{\mathbf{v}}}_t& ={\mathbf{R}}_t(\mathbf{a}m-\mathbf{a}b,t-{\mathbf{a}}_n)+{\mathbf{g}}_t& & & \\ {\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes (\mathbf{\omega }m-\mathbf{\omega }b,t-\mathbf{\omega }n)\dot{\mathbf{a}}b,t& =\mathbf{a}w\dot{\mathbf{\omega }}b,t& ={\mathbf{\omega }}_w{\dot{\mathbf{g}}}_t& =0\end{array}$$

这导致一个标量等式和一个向量等式

$$\begin{array}{rl}0& =\delta {\mathbf{\omega }}^{\top }\delta \mathbf{\theta }+\mathcal{O}(|\delta \mathbf{\theta }{|}^2)\\ \delta \dot{\mathbf{\theta }}& =\delta \mathbf{\omega }-[\mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }-\frac{1}{2}[\delta \mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }+\mathcal{O}(\Vert \delta \mathbf{\theta }{\Vert }^2)\end{array}$$

第一个方程导致 $\delta {\mathbf{\omega }}^{\top }\delta \mathbf{\theta }=\mathcal{O}(\Vert \delta \mathbf{\theta }{\Vert }^2)$,它由二阶无穷小量形成,不是很有用。第二个方程在忽略所有二阶项后得出

$$\delta \dot{\mathbf{\theta }}=-[\mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }+\delta \mathbf{\omega }$$

最后,回想 $\delta \mathbf{\omega }\triangleq -\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n$,我们得到角度误差的线性化动力学,

$$\delta \dot{\mathbf{\theta }}=-[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b{]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n$$
上面使用的符号说明:

• ${\mathbf{a}}_n$: 加速度计噪声向量
• $\mathbb{E}[\cdot ]$: 期望值运算符
• ${\sigma }_a^2$: 加速度计噪声的方差
• $\mathbf{I}$: 单位矩阵
• $\mathbf{R}$: 旋转矩阵
• $\delta \dot{\mathbf{v}}$: 速度误差的导数
• ${\mathbf{a}}_m$: 测量加速度
• ${\mathbf{a}}_b$: 加速度偏置
• $[\cdot {]}_{\times }$: 向量到反对称矩阵的变换
• $\delta {\mathbf{a}}_b$: 加速度偏置误差
• $\delta \mathbf{g}$: 重力加速度误差
• $\delta \dot{\mathbf{\theta }}$: 方向误差的导数
• ${\mathbf{\omega }}_m$: 测量角速度
• ${\mathbf{\omega }}_b$: 角速度偏置
• $\delta {\mathbf{\omega }}_b$: 角速度偏置误差
• ${\mathbf{\omega }}_n$: 角速度噪声
• ${\mathbf{q}}_t$: 真实四元数
• $\mathbf{q}$: 标称四元数
• $\otimes$: 四元数乘法

4.4 离散时间的系统运动学

上面的微分方程需要积分成差分方程,以考虑离散时间间隔 $\Delta t>0$。积分方法可能有所不同。在某些情况下,人们将能够使用精确的闭式解。在其他情况下,可以采用不同精度的数值积分。下一章讲解关于积分方法的相关细节。

积分需要针对以下子系统进行:

1. 标称状态。
2. 误差状态。
   (a) 确定性部分:状态动力学和控制。
   (b) 随机部分:噪声和扰动。

4.4.1 标称状态运动学

我们可以将标称状态的差分方程写成

$$\begin{array}{rl}\mathbf{p}& \leftarrow \mathbf{p}+\mathbf{v}\Delta t+\frac{1}{2}(\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b)+\mathbf{g})\Delta t^2\\ \mathbf{v}& \leftarrow \mathbf{v}+(\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b)+\mathbf{g})\Delta t\\ \mathbf{q}& \leftarrow \mathbf{q}\otimes \mathbf{q}\{({\mathbf{\omega }}_m-{\mathbf{\omega }}_b)\Delta t\}\\ {\mathbf{a}}_b& \leftarrow {\mathbf{a}}_b\\ {\mathbf{\omega }}_b& \leftarrow {\mathbf{\omega }}_b\\ \mathbf{g}& \leftarrow \mathbf{g}\end{array}$$

其中 $\mathbf{x}\leftarrow f(\mathbf{x},\bullet )$ 表示类型为 ${\mathbf{x}}_{k+1}=f({\mathbf{x}}_k,{\bullet }_k)$ 的时间更新, R ≜ R { q } \mathbf{R} \triangleq \mathbf{R}\{\mathbf{q}\} R≜R{q} 是与当前标称方向 $\mathbf{q}$ 相关的旋转矩阵,而 q { v } \mathbf{q}\{\mathbf{v}\} q{v} 是根据(101)与旋转 $\mathbf{v}$ 相关的四元数。

我们也可以使用更精确的积分,如四阶龙格库塔法（下章讲解，下周更新）。

4.4.2 误差状态运动学

确定性部分按正常积分,随机部分的积分导致随机脉冲(后面章节讲解这个概念),因此,

$$\begin{array}{rl}\delta \mathbf{p}& \leftarrow \delta \mathbf{p}+\delta \mathbf{v}\Delta t\\ \delta \mathbf{v}& \leftarrow \delta \mathbf{v}+(-\mathbf{R}[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g})\Delta t+{\mathbf{v}}_i\\ \delta \mathbf{\theta }& \leftarrow {\mathbf{R}}^{\top }\{({\mathbf{\omega }}_m-{\mathbf{\omega }}_b)\Delta t\}\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b\Delta t+{\mathbf{\theta }}_i\\ \delta {\mathbf{a}}_b& \leftarrow \delta {\mathbf{a}}_b+{\mathbf{a}}_i\\ \delta {\mathbf{\omega }}_b& \leftarrow \delta {\mathbf{\omega }}_b+{\mathbf{\omega }}_i\\ \delta \mathbf{g}& \leftarrow \delta \mathbf{g}\end{array}$$

这里,${\mathbf{v}}_i$,${\mathbf{\theta }}_i$,${\mathbf{a}}_i$ 和 ${\mathbf{\omega }}_i$ 是施加到速度、方向和偏差估计的随机脉冲,由白高斯过程建模。它们的均值为零,它们的协方差矩阵是通过在步长 $\Delta t$ 上积分 ${\mathbf{a}}_n$,${\mathbf{\omega }}_n$,${\mathbf{a}}_w$ 和 ${\mathbf{\omega }}_w$ 的协方差得到的,

$$\begin{array}{rl}{\mathbf{V}}_i& ={\sigma }_{{\tilde{a}}_n}^2\Delta t^2\mathbf{I}[{\mathrm{m}}^2/{\mathrm{s}}^2]\\ {\mathbf{\Theta }}_i& ={\sigma }_{{\tilde{\omega }}_n}^2\Delta t^2\mathbf{I}[{\mathrm{rad}}^2]\\ {\mathbf{A}}_i& ={\sigma }_{a_w}^2\Delta t\mathbf{I}[{\mathrm{m}}^2/{\mathrm{s}}^4]\\ {\mathbf{\Omega }}_i& ={\sigma }_{{\omega }_w}^2\Delta t\mathbf{I}[{\mathrm{rad}}^2/{\mathrm{s}}^2]\end{array}$$

其中 ${\sigma }_{{\tilde{a}}_n}[\mathrm{m}/{\mathrm{s}}^2]$,${\sigma }_{{\tilde{\omega }}_n}[\mathrm{rad}/\mathrm{s}]$,${\sigma }_{a_w}[\mathrm{m}/{\mathrm{s}}^4]$ 和 ${\sigma }_{{\omega }_w}[\mathrm{rad}/{\mathrm{s}}^2]$ 将根据IMU数据表中的信息或从实验测量中确定。

这部分使用的符号说明:

• $\delta \mathbf{p}$: 位置误差
• $\delta \mathbf{v}$: 速度误差
• $\Delta t$: 时间步长
• $\mathbf{R}$: 旋转矩阵
• $[\cdot {]}_{\times }$: 向量到反对称矩阵的变换
• $\delta \mathbf{\theta }$: 方向误差
• $\delta {\mathbf{a}}_b$: 加速度偏置误差
• $\delta \mathbf{g}$: 重力加速度误差
• ${\mathbf{\omega }}_m$: 测量角速度
• ${\mathbf{\omega }}_b$: 角速度偏置
• $\delta {\mathbf{\omega }}_b$: 角速度偏置误差
• ${\mathbf{v}}_i$: 施加到速度估计的随机脉冲
• ${\mathbf{\theta }}_i$: 施加到方向估计的随机脉冲
• ${\mathbf{a}}_i$: 施加到加速度偏置估计的随机脉冲
• ${\mathbf{\omega }}_i$: 施加到角速度偏置估计的随机脉冲
• ${\mathbf{V}}_i$: ${\mathbf{v}}_i$的协方差矩阵
• ${\mathbf{\Theta }}_i$: ${\mathbf{\theta }}_i$的协方差矩阵
• ${\mathbf{A}}_i$: ${\mathbf{a}}_i$的协方差矩阵
• ${\mathbf{\Omega }}_i$: ${\mathbf{\omega }}_i$的协方差矩阵
• ${\sigma }_{{\tilde{a}}_n}$: 速度积分过程的加速度噪声标准差
• ${\sigma }_{{\tilde{\omega }}_n}$: 方向积分过程的角速度噪声标准差
• ${\sigma }_{a_w}$: 加速度偏置游走噪声标准差
• ${\sigma }_{{\omega }_w}$: 角速度偏置游走噪声标准差

4.4.3 误差状态雅可比矩阵和扰动矩阵

雅可比矩阵可以通过简单地检查上一节中的误差状态差分方程获得。

为了以紧凑的形式写出这些方程,我们考虑标称状态向量 $\mathbf{x}$,误差状态向量 $\delta \mathbf{x}$,输入向量 ${\mathbf{u}}_m$,以及扰动脉冲向量 $\mathbf{i}$,如下所示,

$$\mathbf{x}=\left[\begin{array}{c}\mathbf{p}\\ \mathbf{v}\\ \mathbf{q}\\ {\mathbf{a}}_b\\ {\mathbf{\omega }}_b\\ \mathbf{g}\end{array}\right],\delta \mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\\ \delta {\mathbf{a}}_b\\ \delta {\mathbf{\omega }}_b\\ \delta \mathbf{g}\end{array}\right],{\mathbf{u}}_m=\left[\begin{array}{c}{\mathbf{a}}_m\\ {\mathbf{\omega }}_m\end{array}\right],\mathbf{i}=\left[\begin{array}{c}{\mathbf{v}}_i\\ {\mathbf{\theta }}_i\\ {\mathbf{a}}_i\\ {\mathbf{\omega }}_i\end{array}\right]$$

误差状态系统现在是

$$\delta \mathbf{x}\leftarrow f(\mathbf{x},\delta \mathbf{x},{\mathbf{u}}_m,\mathbf{i})={\mathbf{F}}_x(\mathbf{x},{\mathbf{u}}_m)\cdot \delta \mathbf{x}+{\mathbf{F}}_i\cdot \mathbf{i}$$

ESKF预测方程写为:

$$\begin{array}{rl}\widehat{\delta \mathbf{x}}& \leftarrow {\mathbf{F}}_x(\mathbf{x},{\mathbf{u}}_m)\cdot \widehat{\delta \mathbf{x}}\\ \mathbf{P}& \leftarrow {\mathbf{F}}_x\mathbf{P}{\mathbf{F}}_x^{\top }+{\mathbf{F}}_i{\mathbf{Q}}_i{\mathbf{F}}_i^{\top }\end{array}$$

其中 δ x ∼ N { δ x ^ , P } \delta \mathbf{x} \sim \mathcal{N}\{\hat{\delta \mathbf{x}}, \mathbf{P}\} δx∼N{δx^,P};${\mathbf{F}}_x$ 和 ${\mathbf{F}}_i$ 是 $f()$ 关于误差和扰动向量的雅可比矩阵;${\mathbf{Q}}_i$ 是扰动脉冲的协方差矩阵。

上述雅可比矩阵和协方差矩阵的表达式如下所述。所有与状态相关的值都直接从标称状态中提取。

$${\mathbf{F}}_x=\frac{\partial f}{\partial \delta \mathbf{x}}{|}_{\mathbf{x},{\mathbf{u}}_m}=\left[\begin{array}{cccccc}\mathbf{I}& \mathbf{I}\Delta t& 0& 0& 0& 0\\ 0& \mathbf{I}& -\mathbf{R}[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\Delta t& -\mathbf{R}\Delta t& 0& \mathbf{I}\Delta t\\ 0& 0& {\mathbf{R}}^{\top }\{({\mathbf{\omega }}_m-{\mathbf{\omega }}_b)\Delta t\}& 0& -\mathbf{I}\Delta t& 0\\ 0& 0& 0& \mathbf{I}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\end{array}\right]$$

$${\mathbf{F}}_i=\frac{\partial f}{\partial \mathbf{i}}{|}_{\mathbf{x},{\mathbf{u}}_m}=\left[\begin{array}{cccc}0& 0& 0& 0\\ \mathbf{I}& 0& 0& 0\\ 0& \mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\\ 0& 0& 0& 0\end{array}\right],{\mathbf{Q}}_i=\left[\begin{array}{cccc}{\mathbf{V}}_i& 0& 0& 0\\ 0& {\mathbf{\Theta }}_i& 0& 0\\ 0& 0& {\mathbf{A}}_i& 0\\ 0& 0& 0& {\mathbf{\Omega }}_i\end{array}\right]$$

请特别注意,${\mathbf{F}}_x$ 是系统的转移矩阵,可以用多种方式以不同的精度进行近似。我们在这里展示了它最简单的形式之一(欧拉形式)。

还请注意,由于误差 $\delta \mathbf{x}$ 的均值初始化为零,线性方程 $\widehat{\delta \mathbf{x}}\leftarrow {\mathbf{F}}_x(\mathbf{x},{\mathbf{u}}_m)\cdot \widehat{\delta \mathbf{x}}$ 总是返回零。你当然应该在你的代码中跳过这一行。但我建议你还是写出来,只是注释掉,这样你就确定没有忘记任何东西。

最后请注意,你不应该跳过协方差预测 $\mathbf{P}\leftarrow {\mathbf{F}}_x\mathbf{P}{\mathbf{F}}_x^{\top }+{\mathbf{F}}_i{\mathbf{Q}}_i{\mathbf{F}}_i^{\top }$!!实际上,项 ${\mathbf{F}}_i{\mathbf{Q}}_i{\mathbf{F}}_i^{\top }$ 不为零,因此这个协方差在不断增长——正如在任何预测步骤中一样。

4.5 用IMU融合互补传感器数据

在获得IMU以外的信息时,如GPS或视觉,我们继续校正ESKF。在一个设计良好的系统中,这应该使IMU偏差可观察并允许ESKF正确估计它们。有无数种可能性,最流行的是GPS + IMU,单目视觉 + IMU,以及双目视觉 + IMU。近年来,视觉传感器与IMU的结合吸引了大量关注,因此产生了大量科学活动。这些视觉 + IMU设置在GPS拒止环境中非常有趣,可以在移动设备(通常是智能手机)上实现,但也可以在无人机和其他小型、敏捷的平台上实现。

虽然IMU信息迄今为止用于对ESKF进行预测,但这些其他信息用于校正滤波器,从而观察IMU偏差误差。校正包括三个步骤:

1. 通过滤波器校正观察误差状态,
2. 将观察到的误差注入标称状态,并且
3. 重置误差状态。

这些步骤将在以下部分中展开。

4.5.1 通过滤波器校正观察误差状态

像往常一样,假设我们有一个传感器,它提供依赖于状态的信息,例如

$$\mathbf{y}=h({\mathbf{x}}_t)+\mathbf{v}$$

其中 $h()$ 是系统状态(真实状态)的一般非线性函数,而 $\mathbf{v}$ 是具有协方差 $\mathbf{V}$ 的白高斯噪声,

v ∼ N { 0 , V } \mathbf{v} \sim \mathcal{N}\{\mathbf{0}, \mathbf{V}\} v∼N{0,V}

我们的滤波器正在估计误差状态,因此滤波器校正方程

$$\begin{array}{rl}\mathbf{K}& =\mathbf{P}{\mathbf{H}}^{\top }(\mathbf{HP}{\mathbf{H}}^{\top }+\mathbf{V}{)}^{-1}\\ \widehat{\delta \mathbf{x}}& \leftarrow \mathbf{K}(\mathbf{y}-h({\hat{\mathbf{x}}}_t))\\ \mathbf{P}& \leftarrow (\mathbf{I}-\mathbf{KH})\mathbf{P}\end{array}$$

需要雅可比矩阵 $\mathbf{H}$ 关于误差状态 $\delta \mathbf{x}$ 定义,并在最佳真实状态估计 ${\hat{\mathbf{x}}}_t=\mathbf{x}\oplus \widehat{\delta \mathbf{x}}$ 处求值。由于误差状态均值在此阶段为零(我们还没有观察到它),我们有 ${\hat{\mathbf{x}}}_t=\mathbf{x}$,并且我们可以使用标称误差 $\mathbf{x}$ 作为求值点,导致

$$\mathbf{H}\equiv \frac{\partial h}{\partial \delta \mathbf{x}}{|}_{\mathbf{x}}$$

4.5.2 用于滤波器校正的雅可比矩阵计算

上面的雅可比矩阵可以用多种方式计算。最直观的一种是利用链式法则,

$$\mathbf{H}\triangleq \frac{\partial h}{\partial \delta \mathbf{x}}{|}_{\mathbf{x}}=\frac{\partial h}{\partial {\mathbf{x}}_t}{|}_{\mathbf{x}}\frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}{|}_{\mathbf{x}}={\mathbf{H}}_x{\mathbf{X}}_{\delta x}$$

这里,${\mathbf{H}}_x\triangleq \frac{\partial h}{\partial {\mathbf{x}}_t}{|}_{\mathbf{x}}$ 是 $h()$ 关于其自身参数的标准雅可比矩阵(即在常规EKF中使用的雅可比矩阵)。链式法则的第一部分取决于所使用的特定传感器的测量函数,这里不作介绍。

第二部分,${\mathbf{X}}_{\delta x}\triangleq \frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}{|}_{\mathbf{x}}$,是真实状态关于误差状态的雅可比矩阵。这一部分可以在这里推导,因为它只取决于ESKF的状态组合。我们有导数,

$${\mathbf{X}}_{\delta x}=\left[\begin{array}{cccccccc}\frac{\partial (\mathbf{p}+\delta \mathbf{p})}{\partial \delta \mathbf{p}}& & & & & & & \\ & \frac{\partial (\mathbf{v}+\delta \mathbf{v})}{\partial \delta \mathbf{v}}& & & & & & \\ & & 0& & & & & \\ & & & \frac{\partial (\mathbf{q}\otimes \delta \mathbf{q})}{\partial \delta \mathbf{\theta }}& & & & \\ & & & & \frac{\partial ({\mathbf{a}}_b+\delta {\mathbf{a}}_b)}{\partial \delta {\mathbf{a}}_b}& & & \\ & & & & & 0& & \\ & & & & & & \frac{\partial ({\mathbf{\omega }}_b+\delta {\mathbf{\omega }}_b)}{\partial \delta {\mathbf{\omega }}_b}& \\ & & & & & & & \frac{\partial (\mathbf{g}+\delta \mathbf{g})}{\partial \delta \mathbf{g}}\end{array}\right]$$

这导致所有恒等的 $3\times 3$ 块(例如,$\frac{\partial (\mathbf{p}+\delta \mathbf{p})}{\partial \delta \mathbf{p}}={\mathbf{I}}_3$),除了四元数项 ${\mathbf{Q}}_{\delta \theta }=\frac{\partial (\mathbf{q}\otimes \delta \mathbf{q})}{\partial \delta \mathbf{\theta }}$ 之外的 $4\times 3$ 。因此我们有如下形式,

$${\mathbf{X}}_{\delta x}\triangleq \frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}{|}_{\mathbf{x}}=\left[\begin{array}{ccc}{\mathbf{I}}_6& 0& 0\\ 0& {\mathbf{Q}}_{\delta \theta }& 0\\ 0& 0& {\mathbf{I}}_9\end{array}\right]$$

使用链式法则,以及极限 $\delta \mathbf{q}\underset{\delta \mathbf{\theta }\to 0}{⟶}\left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathbf{\theta }\end{array}\right]$,四元数项 ${\mathbf{Q}}_{\delta \theta }$ 可以推导如下,

$$\begin{array}{rl}{\mathbf{Q}}_{\delta \theta }& \triangleq \frac{\partial (\mathbf{q}\otimes \delta \mathbf{q})}{\partial \delta \mathbf{\theta }}{|}_{\mathbf{q}}\\ & =\frac{\partial (\mathbf{q}\otimes \delta \mathbf{q})}{\partial \delta \mathbf{q}}{|}_{\mathbf{q}}\frac{\partial \delta \mathbf{q}}{\partial \delta \mathbf{\theta }}{|}_{\widehat{\delta \mathbf{\theta }}=0}\\ & =\frac{\partial ([\mathbf{q}{]}_L\delta \mathbf{q})}{\partial \delta \mathbf{q}}{|}_{\mathbf{q}}\frac{\partial \left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathbf{\theta }\end{array}\right]}{\partial \delta \mathbf{\theta }}{|}_{\widehat{\delta \mathbf{\theta }}=0}\\ & =[\mathbf{q}{]}_L\frac{1}{2}\left[\begin{array}{ccc}0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\end{array}$$

这导致

$${\mathbf{Q}}_{\delta \theta }=\frac{1}{2}\left[\begin{array}{ccc}-q_x& -q_y& -q_z\\ q_w& -q_z& q_y\\ q_z& q_w& -q_x\\ -q_y& q_x& q_w\end{array}\right]$$

4.6 将观察到的误差注入标称状态

在ESKF更新之后,标称状态用观察到的误差状态更新,使用适当的组合(求和或四元数乘积,见表格),

$$\mathbf{x}\leftarrow \mathbf{x}\oplus \widehat{\delta \mathbf{x}}$$

即

p ← p + δ p ^ v ← v + δ v ^ q ← q ⊗ q { δ θ ^ } a b ← a b + δ a ^ b ω b ← ω b + δ ω ^ b g ← g + δ g ^ \begin{aligned} \mathbf{p} &\leftarrow \mathbf{p} + \hat{\delta \mathbf{p}} \\ \mathbf{v} &\leftarrow \mathbf{v} + \hat{\delta \mathbf{v}} \\ \mathbf{q} &\leftarrow \mathbf{q} \otimes \mathbf{q}\{\hat{\delta\boldsymbol{\theta}}\} \\ \mathbf{a}_b &\leftarrow \mathbf{a}_b + \hat{\delta \mathbf{a}}_b \\ \boldsymbol{\omega}_b &\leftarrow \boldsymbol{\omega}_b + \hat{\delta\boldsymbol{\omega}}_b \\ \mathbf{g} &\leftarrow \mathbf{g} + \hat{\delta \mathbf{g}} \end{aligned} pvqab​ωb​g​←p+δp^​←v+δv^←q⊗q{δθ^}←ab​+δa^b​←ωb​+δω^b​←g+δg^​​

4.7 ESKF重置

在误差注入标称状态之后,误差状态均值 $\widehat{\delta \mathbf{x}}$ 被重置。
在误差状态卡尔曼滤波器(ESKF)中,每次有新的观测数据更新之后,我们需要对状态估计进行重置操作。重置操作的目的是将估计误差注入到标称状态中,使得误差状态均值恢复到0附近。这样做的原因是为了避免误差状态发散,因为卡尔曼滤波器是基于小误差假设的。
ESKF中的重置操作包括两个步骤:

1. 将估计的误差均值 $\widehat{\delta \mathbf{x}}$ 重置为0。
2. 更新误差协方差矩阵 $\mathbf{P}$,使其与新的误差状态保持一致。

让我们称误差重置函数为 $g()$。它写成如下形式,

$$\delta \mathbf{x}\leftarrow g(\delta \mathbf{x})=\delta \mathbf{x}\boxminus \widehat{\delta \mathbf{x}}$$

其中 $\boxminus$ 表示 $\oplus$ 的逆组合。ESKF误差重置操作因此是,

$$\begin{array}{rl}\widehat{\delta \mathbf{x}}& \leftarrow 0\\ \mathbf{P}& \leftarrow \mathbf{GP}{\mathbf{G}}^{\top }\end{array}$$

其中 $\mathbf{G}$ 是雅可比矩阵,定义为

$$\mathbf{G}\triangleq \frac{\partial g}{\partial \delta \mathbf{x}}{|}_{\widehat{\delta \mathbf{x}}}$$

与上面的更新雅可比类似,这个雅可比在所有对角块上都是单位矩阵,除了在方向误差处。我们给出完整的表达式,并在下一节中推导方向误差块,

$$\mathbf{G}=\left[\begin{array}{ccc}{\mathbf{I}}_6& 0& 0\\ 0& \mathbf{I}-[\frac{1}{2}\widehat{\delta \mathbf{\theta }}{]}_{\times }& 0\\ 0& 0& {\mathbf{I}}_9\end{array}\right]$$

在大多数情况下,误差项 $\widehat{\delta \mathbf{\theta }}$ 可以忽略，这里精确的G可能有助于减少长期漂移,提高里程计系统的精度。

4.7.1 关于方向误差的重置操作的雅可比矩阵

我们想要获得新的角度误差 $\delta {\mathbf{\theta }}^{+}$ 关于旧误差 $\delta \mathbf{\theta }$ 和观察到的误差 $\widehat{\delta \mathbf{\theta }}$ 的表达式。考虑以下事实:

• 误差重置时真实方向不变,即 ${\mathbf{q}}_t^{+}={\mathbf{q}}_t$。这给出:

$${\mathbf{q}}^{+}\otimes \delta {\mathbf{q}}^{+}=\mathbf{q}\otimes \delta \mathbf{q}$$

• 观察到的误差均值已经注入到标称状态中:

$${\mathbf{q}}^{+}=\mathbf{q}\otimes \widehat{\delta \mathbf{q}}$$

结合这两个恒等式,我们得到 $\delta {\mathbf{q}}^{+}$ 的表达式,

$$\delta {\mathbf{q}}^{+}=({\mathbf{q}}^{+}{)}^{\ast }\otimes \mathbf{q}\otimes \delta \mathbf{q}=(\mathbf{q}\otimes \widehat{\delta \mathbf{q}}{)}^{\ast }\otimes \mathbf{q}\otimes \delta \mathbf{q}={\widehat{\delta \mathbf{q}}}^{\ast }\otimes \delta \mathbf{q}=[{\widehat{\delta \mathbf{q}}}^{\ast }{]}_L\cdot \delta \mathbf{q}$$

考虑到 ${\widehat{\delta \mathbf{q}}}^{\ast }\approx \left[\begin{array}{c}1\\ -\frac{1}{2}\widehat{\delta \mathbf{\theta }}\end{array}\right]$,上述恒等式可以展开为

$$\begin{array}{r}\left[\begin{array}{cc}1& \frac{1}{2}\delta {\mathbf{\theta }}^{+}\end{array}\right]\left[\begin{array}{cc}1& \frac{1}{2}{\widehat{\delta \mathbf{\theta }}}^{\top }\\ -\frac{1}{2}\widehat{\delta \mathbf{\theta }}& \mathbf{I}-[\frac{1}{2}\widehat{\delta \mathbf{\theta }}{]}_{\times }\end{array}\right]\cdot \left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathbf{\theta }\end{array}\right]+\mathcal{O}(|\delta \mathbf{\theta }{|}^2)\end{array}$$

这给出了一个标量方程和一个向量方程,

$$\begin{array}{rl}\frac{1}{4}{\widehat{\delta \mathbf{\theta }}}^{\top }\delta \mathbf{\theta }& =\mathcal{O}(\Vert \delta \mathbf{\theta }{\Vert }^2)\\ \delta {\mathbf{\theta }}^{+}& =-\widehat{\delta \mathbf{\theta }}+\left(\mathbf{I}-[\frac{1}{2}\widehat{\delta \mathbf{\theta }}{]}_{\times }\right)\delta \mathbf{\theta }+\mathcal{O}(\Vert \delta \mathbf{\theta }{\Vert }^2)\end{array}$$

其中第一个不是很有信息量,因为它只是无穷小量的关系。从第二个方程可以看出 ${\widehat{\delta \mathbf{\theta }}}^{+}=0$,这是我们从重置操作中期望的。雅可比矩阵由简单检查得到,

$$\frac{\partial \delta {\mathbf{\theta }}^{+}}{\partial \delta \mathbf{\theta }}=\mathbf{I}-[\frac{1}{2}\widehat{\delta \mathbf{\theta }}{]}_{\times }$$

这部分我们主要在讨论如何根据观测到的方向误差 $\widehat{\delta \mathbf{\theta }}$ 来重置和更新真实的方向误差 $\delta \mathbf{\theta }$。我们想求出新的误差 $\delta {\mathbf{\theta }}^{+}$ 关于旧误差 $\delta \mathbf{\theta }$ 和观测误差 $\widehat{\delta \mathbf{\theta }}$ 的表达式,以及它们之间的雅可比矩阵。这对于误差状态卡尔曼滤波很重要,因为它需要根据观测来更新状态估计。

我们从两个基本事实出发:

1. 真实方向在重置时不变,即 ${\mathbf{q}}_t^{+}={\mathbf{q}}_t$
2. 观测误差 $\widehat{\delta \mathbf{\theta }}$ 已被注入到标称状态 ${\mathbf{q}}^{+}$ 中

利用这两个事实,我们可以推导出新的误差 $\delta {\mathbf{q}}^{+}$ (四元数形式)与旧误差 $\delta \mathbf{q}$ 和观测误差 $\widehat{\delta \mathbf{q}}$ 的关系:

$$\delta {\mathbf{q}}^{+}={\widehat{\delta \mathbf{q}}}^{\ast }\otimes \delta \mathbf{q}=[{\widehat{\delta \mathbf{q}}}^{\ast }{]}_L\cdot \delta \mathbf{q}$$

其中 $[{\widehat{\delta \mathbf{q}}}^{\ast }{]}_L$ 是 ${\widehat{\delta \mathbf{q}}}^{\ast }$ 的左乘算子。接下来通过一系列展开,我们可以得到 $\delta {\mathbf{\theta }}^{+}$ 关于 $\delta \mathbf{\theta }$ 和 $\widehat{\delta \mathbf{\theta }}$ 的表达式:

$$\delta {\mathbf{\theta }}^{+}=-\widehat{\delta \mathbf{\theta }}+\left(\mathbf{I}-{\left[\frac{1}{2}\widehat{\delta \mathbf{\theta }}\right]}_{\times }\right)\delta \mathbf{\theta }+\mathcal{O}(\Vert \delta \mathbf{\theta }{\Vert }^2)$$

从这个表达式中,我们可以看出 ${\widehat{\delta \mathbf{\theta }}}^{+}=0$,这正是我们重置操作所期望的。

最后,通过对上式取导数,我们可以得到 $\delta {\mathbf{\theta }}^{+}$ 对 $\delta \mathbf{\theta }$ 的雅可比矩阵:

$$\frac{\partial \delta {\mathbf{\theta }}^{+}}{\partial \delta \mathbf{\theta }}=\mathbf{I}-{\left[\frac{1}{2}\widehat{\delta \mathbf{\theta }}\right]}_{\times }$$

这个雅可比矩阵在卡尔曼滤波的误差状态传播中非常有用。

这样我们就解决了如何根据观测到的方向误差来更新真实的方向误差估计,并给出了具体的数学表达式和推导过程。其中使用的主要符号说明如下:

• ${\mathbf{q}}_t$: 真实四元数
• $\mathbf{q}$: 标称四元数
• $\delta \mathbf{q}$: 四元数误差
• $\widehat{\delta \mathbf{q}}$: 观测到的四元数误差
• $\otimes$: 四元数乘法
• $\ast$: 四元数共轭
• $[\cdot {]}_L$: 左乘算子
• $[\cdot {]}_{\times }$: 向量到反对称矩阵的变换
• $\delta \mathbf{\theta }$: 向量形式的方向误差
• $\widehat{\delta \mathbf{\theta }}$: 观测到的方向误差

第5章将探讨使用全局定义的角度误差的ESKF。第4章的差异将在第5章中体现。

参考文献:

[1] Bledt, Gerardo, et al. “Contact model fusion for event-based locomotion in unstructured terrains.” 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.
[2] Li, Mingyang, and Anastasios I. Mourikis. “High-precision, consistent EKF-based visual-inertial odometry.” The International Journal of Robotics Research 32.6 (2013): 690-711.
[3] Lupton, Todd, and Salah Sukkarieh. “Efficient integration of inertial observations into visual SLAM without initialization.” 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2009.