IMU 误差建模 —— Notes for “Quaternion kinematics for the error-state Kalman filter“

IMU 误差建模

—— Notes for “Quaternion kinematics for the error-state Kalman filter”

参考文献 “Quaternion kinematics for the error-state Kalman filter” ¹ 完整地推导了误差状态卡尔曼滤波器 (ESKF, the error-state Kalman filter). 内容的重点在 IMU 运动方程的建模, 然后套用 Kalman filter 的预测-更新框架获得了 ESKF. IMU 运动方程的建模分为两个部分, 1. 捷联惯导 IMU 的刚体运动学建模, 即确定部分, 2. IMU 信号误差建模, 即随机部分. 刚体运动学比较明确公式推导也很完备, 此处不展开. 我们讨论一下随机部分的建模.

I 误差性质

加速度计和陀螺仪的测量方程如下¹
$${\mathbf{a}}_m={\mathbf{R}}^{\mathrm{T}}({\mathbf{a}}_t-{\mathbf{g}}_t)+{\mathbf{a}}_{bt}+{\mathbf{a}}_n\text{\hspace{1em}(231)}$$

$${\mathbf{\omega }}_m={\mathbf{\omega }}_t+{\mathbf{\omega }}_{bt}+{\mathbf{\omega }}_n\text{\hspace{1em}(232)}$$

1. 高斯白噪声

${\mathbf{a}}_n$ 是加速度测量中的高斯白噪声, 快速变化影响当前时间测量值. 每一轴 (XYZ 三轴) 的加速度测量噪声认为是独立同分布的高斯白噪声. 每一轴上的随机变量的分布满足 $\mathcal{N}[0,{\sigma }_{{\tilde{\mathbf{a}}}_n}^2]$, 则各轴之间独立的三维随机变量写成
$${\mathbf{a}}_n(t)\sim \mathcal{N}[0_{1\times 3},{\sigma }_{{\tilde{\mathbf{a}}}_n}^2{\mathbf{I}}_{3\times 3}]\text{\hspace{1em}(I-1-1)}$$
该白噪声作为随机过程所含的不同时刻的随机变量之间具有零均值和独立性的特点, 即
$$\mathbf{E}[{\mathbf{a}}_n(t)]=0,\mathbf{E}[{\mathbf{a}}_n(t_1){\mathbf{a}}_n(t_2)]={\sigma }_{{\tilde{\mathbf{a}}}_n}^2\delta (t_1-t_2)\mathbf{I}\text{\hspace{1em}(I-1-2)}$$
其中 $\delta (\cdot )$ 为狄拉克脉冲函数 (Dirac delta function).

同理, 分析陀螺仪角速度测量中的高斯白噪声
$${\mathbf{\omega }}_n\sim \mathcal{N}[0_{1\times 3},{\sigma }_{{\tilde{\mathbf{\omega }}}_n}^2{\mathbf{I}}_{3\times 3}]\text{\hspace{1em}(I-1-3)}$$

在进一步分析加速度计随机游走偏差 Accelerometer bias 和陀螺仪随机游走偏差 Gyrometer bias 前, 先探讨一下随机游走和高斯白噪声之间的关系.

2. 维纳过程

标准维纳过程 (布朗运动) 定义²

A standard (one-dimensional) Wiener process (also called Brownian motion) is
a stochastic process { W t } t ≥ 0 + \{W_t\}_{t≥0+} {Wt​}t≥0+​ indexed by nonnegative real numbers $t$ with the following
properties:
(1) $W_0=0$.
(2) With probability 1, the function $t$ → $W_t$ is continuous in $t$.
(3) The process { W t } t ≥ 0 \{W_t\}_{t≥0} {Wt​}t≥0​ has stationary, independent increments.
(4) The increment $W_{t+s}-W_s$ has the normal distribution $\mathcal{N}[0,t]$ .

由标准维纳过程的定义可以知道随机变量 $W_t=W_t-W_0\sim \mathcal{N}[0,t]$, 其方差随着时间发散.

进一步细分时间区间 $[0,t]$ 为 $N\in {\mathbb{Z}}^{+}$ 个跨度为 $\Delta t$ 的等长小区间, $N\Delta t=t$. 同时令 $N\to \infty$, 就可以得到无穷小增量系列 (the infinitesimal increments), 即
$$\Delta W_i=W_{(i+1)\Delta t}-W_{i\Delta t},0\le i\le N-1\&i\in \mathbb{Z}\text{\hspace{1em}(I-2-1)}$$
由标准维纳过程 (布朗运动) 定义可知, $\Delta W_i\sim \mathcal{N}[0,\Delta t]$, 且 $\Delta W_i$ 与 $\Delta W_j$ 之间是独立的 (当 $i\ne j$). 那么
$$W_t=\underset{N\to \infty }{\lim }\sum _{i=0}^{N-1}\Delta W_i\triangleq {\int }_0^t\mathrm{d}W(t)\text{\hspace{1em}(I-2-2)}$$
另外, 在时间区域 $(i\Delta t,(i+1)\Delta t)$ 内, 任取 $\tau$ 时刻, 即 $\tau \in (i\Delta t,(i+1)\Delta t)$. 定义该时刻上标准高斯变量 ${\xi }_i(\tau )\sim \mathcal{N}[0,1]$, 则 ${\xi }_i(\tau )\Delta t\sim \mathcal{N}[0,\Delta t]$. 同时需要满足 ${\xi }_i$ 与 ${\xi }_j$ (当 $i\ne j$) 之间相互独立. 当时间区域趋于无穷小时, 即 $\Delta t\to 0$ 时, ${\xi }_i(\tau )$ 系列是高斯白噪声 ($0\le i\le N,N\to \infty$). ${\xi }_i(\tau )$ 系列按照时间顺序首尾拼接, 在时间区域 ($0\le \tau \le t$) 上形成高斯白噪声, 记为 $\xi (\tau )\sim \mathcal{N}[0,1]$.
结合维纳过程定义和上述构造的高斯变量系列, 在无穷小时间 $(i\Delta t,(i+1)\Delta t)$ 内, $\Delta W_i$ 与 ${\xi }_i(\tau )\Delta t$ 分布相同, 用 ${\xi }_i(\tau )\Delta t$ 替代式 (I-2-2) 中的 $\Delta W_i$
$$W_t=\underset{N\to \infty }{\lim }\sum _{i=0}^{N-1}\Delta W_i=\underset{N\to \infty }{\lim }\sum _{i=0}^{N-1}{\xi }_i(\tau )\Delta t={\int }_0^t{\xi }_{\lfloor \frac{\tau }{\Delta t}\rfloor }(\tau )\mathrm{d}t={\int }_0^t\xi (\tau )\mathrm{d}t\text{\hspace{1em}(I-2-3)}$$
以上我们说明了标准维纳过程 (布朗运动) 是高斯白噪声在时间 $[0,t]$ 上的积分.

简单推论
$${\int }_{i\Delta t}^{(i+1)\Delta t}\xi (\tau )\mathrm{d}t=W_{(i+1)\Delta t}-W_{i\Delta t}\sim \mathcal{N}[0,\Delta t]\text{\hspace{1em}(I-2-4)}$$

3. 一维随机游走

One-dimensional random walks 的定义³

A random walk on the integers $\mathbb{Z}$ with step distribution $\mathcal{F}$ and initial state $x\in \mathbb{Z}$ is a sequence $S_n$ of random variables whose increments are independent, identically distributed random variables ${\xi }_i$ with common distribution $\mathcal{F}$, that is,
$$S_n=x+\sum _{i=1}^n{\xi }_i$$

根据以上定义, 对比式 (I-2-3) 可知 $W_t$ 就是初始变量 $x$ 为 0 的随机游走, 对应随机变量是 ${\xi }_i(\tau )\Delta t$. 因为维纳过程和随机游走都具有的马尔科夫特性, 可以以任意时刻状态为初始变量, 后续过程为相同性质的随机游走过程.

4. 随机游走偏差

我们再回过来进行 IMU 的误差建模. ${\mathbf{a}}_{bt}$ 是加速度数值中的随机游走偏差, 该随机游走误差是一个慢变的误差累积过程.

随机游走误差的运动学方程¹
$${\dot{\mathbf{a}}}_{bt}={\mathbf{a}}_w\text{\hspace{1em}(230d)}$$

$${\dot{\mathbf{\omega }}}_{bt}={\mathbf{\omega }}_w\text{\hspace{1em}(230e)}$$

随机游走误差的变化率是高斯白噪声, 同时每根轴上独立但有相同的随机过程. 即 XYZ 每一轴上的角速度随机游走误差是各轴独立的高斯白噪声的从 0 时刻到当前时刻 $t$ 的积分. 白噪声的积分不再是白噪声, 而是维纳过程 (Wiener process) 或是布朗运动 (Brownian motion).

根据中心极限定理, 随机游走的极限是布朗运动², 而 IMU 测量也正好比较频繁, 故较容易接近极限状态. 维纳过程具有马尔科夫特性, 当前时刻的随机游走误差是上一时刻误差的基础上累加当前时刻的一个白噪声误差变量. 而这个被累加的三个轴上独立同分布的白噪声变量
$${\mathbf{a}}_w\sim \mathcal{N}[0,{\sigma }_{{\mathbf{a}}_w}^2\mathbf{I}]\text{\hspace{1em}(I-4-1)}$$
由 (230d) 可知
$${\mathbf{a}}_{bt}(t)={\int }_0^t{\mathbf{a}}_w(\tau )\mathrm{d}\tau \text{\hspace{1em}(I-4-2)}$$
由标准维纳过程 (布朗运动) 定义可知, ${\mathbf{a}}_{bt}(t)$ 满足正态分布, 即
$${\mathbf{a}}_{bt}(t)\sim \mathcal{N}[0,{\sigma }_{{\mathbf{a}}_w}^{2}t\mathbf{I}]\text{\hspace{1em}(I-4-3)}$$
角加速度的随机游走偏差随时间在逐渐发散.

同理, 分析陀螺仪角速度的的随机游走偏差. 陀螺仪三个轴上的白噪声变量
$${\mathbf{\omega }}_w\sim \mathcal{N}[0,{\sigma }_{{\mathbf{\omega }}_w}^2\mathbf{I}]\text{\hspace{1em}(I-4-4)}$$
陀螺仪角速度的随机游走偏差
$${\mathbf{\omega }}_{bt}\sim \mathcal{N}[0,{\sigma }_{{\mathbf{\omega }}_w}^{2}t\mathbf{I}]\text{\hspace{1em}(I-4-5)}$$

II 系统模型

1. 增广状态

系统的误差状态
$$\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]\text{\hspace{1em}(266)}$$

系统误差状态中 $\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\end{array}\right]$ 是真正需要估计的误差状态, 比如是机器人的位姿误差、速度误差、姿态误差. 而 $\left[\begin{array}{c}\delta {\mathbf{a}}_b\\ \delta {\mathbf{\omega }}_b\\ \delta \mathbf{g}\end{array}\right]$ 是增广的误差状态 (Augmented error states), 作用有

(a) 增加对加速度计和陀螺仪各自的随机游走偏差 ($\delta {\mathbf{a}}_b$ 和 $\delta {\mathbf{\omega }}_b$, 进而 ${\mathbf{a}}_b$ 和 ${\mathbf{\omega }}_b$) 的在线估计, 而没有其他方法消除这些变化的随机误差过程.

(b) 增加重力加速度矢量的估计, 就可以设定任意初始姿态 $q_0$ 与参考框架重合, 而得到 R { q 0 } = I \mathbf{R}\{q_0\} = \mathbf I R{q0​}=I, 而由对应的重力加速 $\mathbf{g}=[g_x,g_y,g_z{]}^{\mathrm{T}}$ 来表示初始姿态 (参考框架) 与外部导航坐标系 (如东北天坐标系) 之间的相对姿态, 同时顺带估计了参考水平面¹. 要不然的话, 为了保证线性度, 每次都需要从相同初始姿态开始运行.

© 如果 $\delta {\mathbf{a}}_b$、 $\delta {\mathbf{\omega }}_b$、$\delta \mathbf{g}$ 不作为系统误差状态而留在系统方程, 没做进一部变换情况下, 将破坏标准系统方程在形式上的构建, 阻碍大家利用已熟知的控制理论.

2. 离散模型

Consider the continuous-time dynamic system¹ ,
$$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u},\mathbf{w})\text{\hspace{1em}(425)}$$
where $\mathbf{x}$ is the state vector, $\mathbf{u}$ is a vector of control signals containing noise $\tilde{u}$, so that the control measurements are ${\mathbf{u}}_m=\mathbf{u}+\tilde{\mathbf{u}}$, and $\mathbf{w}$ is a vector of random perturbations.

对式 (425) 在 ($\bar{\mathbf{x}}$, ${\mathbf{u}}_m$) 处一阶泰勒展开, 即
$$\begin{gathered} \mathbf{x}=\bar{\mathbf{x}}+\delta \mathbf{x} \\ \mathbf{u}={\mathbf{u}}_m-\tilde{\mathbf{u}} \\ \mathbf{w}=0+\mathbf{w} \\ \text{\hspace{1em}(III-2-1)} \end{gathered}$$
其中 ($\delta \mathbf{x},-\tilde{\mathbf{u}},\mathbf{w}$) 视作为小量, 可以得到
$$\begin{array}{rl}\dot{(\bar{\mathbf{x}}+\delta \mathbf{x})}& =f(\bar{\mathbf{x}}+\delta \mathbf{x},{\mathbf{u}}_m-\tilde{\mathbf{u}},0+\mathbf{w})\\ \Rightarrow \dot{\bar{\mathbf{x}}}+\dot{\delta \mathbf{x}}& =f(\bar{\mathbf{x}},{\mathbf{u}}_m,0)+\frac{\partial f}{\partial \mathbf{x}}{|}_{\bar{\mathbf{x}},{\mathbf{u}}_m}\delta \mathbf{x}+\frac{\partial f}{\partial \mathbf{u}}{|}_{\bar{\mathbf{x}},{\mathbf{u}}_m}(-\tilde{\mathbf{u}})+\frac{\partial f}{\partial \mathbf{w}}{|}_{\bar{\mathbf{x}},{\mathbf{u}}_m}\mathbf{w}\\ \Rightarrow \dot{\delta \mathbf{x}}& =\frac{\partial f}{\partial \mathbf{x}}{|}_{\bar{\mathbf{x}},{\mathbf{u}}_m}\delta \mathbf{x}-\frac{\partial f}{\partial \mathbf{u}}{|}_{\bar{\mathbf{x}},{\mathbf{u}}_m}\tilde{\mathbf{u}}+\frac{\partial f}{\partial \mathbf{w}}{|}_{\bar{\mathbf{x}},{\mathbf{u}}_m}\mathbf{w}\\ \Rightarrow \dot{\delta \mathbf{x}}& =\frac{\partial f}{\partial \delta \mathbf{x}}{|}_{\bar{\mathbf{x}},{\mathbf{u}}_m}\delta \mathbf{x}+\frac{\partial f}{\partial \tilde{\mathbf{u}}}{|}_{\bar{\mathbf{x}},{\mathbf{u}}_m}\tilde{\mathbf{u}}+\frac{\partial f}{\partial \mathbf{w}}{|}_{\bar{\mathbf{x}},{\mathbf{u}}_m}\mathbf{w}\end{array}\text{\hspace{1em}(III-2-2)}$$
就是文献中的式 (428)¹, 简写为

$$\dot{\delta \mathbf{x}}=\mathbf{A}\delta \mathbf{x}+\mathbf{B}\tilde{\mathbf{u}}+\mathbf{Cw}\text{\hspace{1em}(428)}$$

实际上, 式 (425) 中把 IMU 的加速度和角速度 $\mathbf{u}=\left[\begin{array}{c}{\mathbf{a}}_m-{\mathbf{a}}_n\\ {\mathbf{\omega }}_m-{\mathbf{\omega }}_n\end{array}\right]$ 作为该系统的控制输入. 注意此处 $\mathbf{u}$ 与实际真实 IMU 值 (${\mathbf{a}}_t$, ${\mathbf{\omega }}_t$) 的区别, 去除了作为状态量的游走偏差和加速度值 ($\delta {\mathbf{a}}_b$, $\delta {\mathbf{\omega }}_b$, $\delta \mathbf{g}$). 因为捷联惯导系统 IMU 状态是由外部平台决定的, 将 IMU 部分测量值视作捷联惯导系统的控制输入也合情合理.
形式上, 式 (428) 中的 $\tilde{\mathbf{u}}=\left[\begin{array}{c}{\mathbf{a}}_n\\ {\mathbf{\omega }}_n\end{array}\right]$ 是否作为误差状态方程 (428) 的控制输入？ 因为 $\tilde{\mathbf{u}}$ 是随机变量, 故这这样的对比不可行.

数值计算上需要将连续误差方程 (428) 两边在时间区间 ($n\Delta t,(n+1)\Delta t$) 上积分, 转换为离散误差方程

$$\delta {\mathbf{x}}_{n+1}=\delta {\mathbf{x}}_n+{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{A}\delta \mathbf{x}(\tau )\mathrm{d}\tau +{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{B}\tilde{\mathbf{u}}(\tau )\mathrm{d}\tau +{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{Cw}(\tau )\mathrm{d}\tau \text{\hspace{1em}(431)}$$

对比原始的 error-state kinematics 式 (238a-238f), 可知其中的

$$\mathbf{B}=\left[\begin{array}{cc}0& 0\\ -\mathbf{R}& 0\\ 0& -\mathbf{I}\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right]C=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ \mathbf{I}& 0\\ 0& \mathbf{I}\\ 0& 0\end{array}\right]\text{\hspace{1em}(451)}$$

3. 积分计算

下面针对离散误差方程 (431) 分别计算和说明.

(a) 很短的时间区间 ($n\Delta t,(n+1)\Delta t$) 上, 假设系数 $\mathbf{A}$、$\mathbf{B}$、$\mathbf{C}$ 都是常数矩阵, 取在 ($\bar{\mathbf{x}}$, ${\mathbf{u}}_m$) 处的值. 所以在积分时间步内把系统看作线性时不变系统 (LTI).

(b) 由式 (I-1-1) 和 (I-1-3) 可知 $\tilde{\mathbf{u}}$ 的分布
$$\tilde{\mathbf{u}}=\left[\begin{array}{c}{\mathbf{a}}_n\\ {\mathbf{\omega }}_n\end{array}\right]\sim \mathcal{N}\left[0,\left[\begin{array}{cc}{\sigma }_{{\tilde{\mathbf{a}}}_n}^2\mathbf{I}& 0\\ 0& {\sigma }_{{\tilde{\mathbf{\omega }}}_n}^2\mathbf{I}\end{array}\right]\right]\text{\hspace{1em}(III-3-1)}$$

© $\tilde{\mathbf{u}}$ 是混合进入测量值 ${\mathbf{u}}_m$ 的噪声信号, 是在上一采样时刻 ( $t=n\Delta t$ ) 产生的值. $\tilde{\mathbf{u}}$ 作为误差运动系统的控制输入部分, 只在采样时刻输入控制量, 在时间区间 ($n\Delta t,(n+1)\Delta t$) 内保持为常量, 即

$$\tilde{\mathbf{u}}(\tau )=\tilde{\mathbf{u}}(n\Delta t)\triangleq {\tilde{\mathbf{u}}}_n,n\Delta t<\tau <(n+1)\Delta t\text{\hspace{1em}(427)}$$

计算控制输入误差积分项
$${\int }_{n\Delta t}^{(n+1)\Delta t}\tilde{\mathbf{u}}(\tau )\mathrm{d}\tau =\Delta t{\tilde{\mathbf{u}}}_n\text{\hspace{1em}(III-3-2)}$$
下面计算 $\Delta t{\tilde{\mathbf{u}}}_n$ 期望和协方差
$$\mathbf{E}[\Delta t{\tilde{\mathbf{u}}}_n]=\Delta t\mathbf{E}[{\tilde{\mathbf{u}}}_n]=0$$

$$\mathbf{E}\left[(\Delta t{\tilde{\mathbf{u}}}_n-\mathbf{E}[\Delta t{\tilde{\mathbf{u}}}_n])(\Delta t{\tilde{\mathbf{u}}}_n-\mathbf{E}[\Delta t{\tilde{\mathbf{u}}}_n]{)}^{\mathrm{T}}\right]=\mathbf{E}\left[\Delta t{\tilde{\mathbf{u}}}_n{\tilde{\mathbf{u}}}_n^{\mathrm{T}}\Delta t\right]=\left[\begin{array}{cc}{\sigma }_{{\tilde{\mathbf{a}}}_n}^2\mathbf{I}& 0\\ 0& {\sigma }_{{\tilde{\mathbf{\omega }}}_n}^2\mathbf{I}\end{array}\right]{\Delta t}^2$$

$$\Delta t{\tilde{\mathbf{u}}}_n=\sim \mathcal{N}\left[0,\left[\begin{array}{cc}{\sigma }_{{\tilde{\mathbf{a}}}_n}^2\mathbf{I}& 0\\ 0& {\sigma }_{{\tilde{\mathbf{\omega }}}_n}^2\mathbf{I}\end{array}\right]{\Delta t}^2\right]\text{\hspace{1em}(III-3-3)}$$

(d) 随机噪声 (参考式 (I-4-1) 和 (I-4-4))
$$\mathbf{w}=\left[\begin{array}{c}{\mathbf{a}}_w\\ {\mathbf{\omega }}_w\end{array}\right]\sim \mathcal{N}\left[0,\left[\begin{array}{cc}{\sigma }_{{\mathbf{a}}_w}^2\mathbf{I}& 0\\ 0& {\sigma }_{{\mathbf{\omega }}_w}^2\mathbf{I}\end{array}\right]\right]\text{\hspace{1em}(III-3-4)}$$
在时间区间 ($n\Delta t,(n+1)\Delta t$) 上持续变化地影响系统. 对比式 (I-2-4) 可知随机变量积分项

$${\mathbf{w}}_n\triangleq {\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{w}(\tau )\mathrm{d}\tau \text{\hspace{1em}(433)}$$

为一高斯分布.

(e) 求随机积分项 ${\mathbf{w}}_n$ 的期望和协方差矩阵
$$\mathbf{E}[{\mathbf{w}}_n]=\mathbf{E}\left[{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{w}(\tau )\mathrm{d}\tau \right]={\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{E}[\mathbf{w}(\tau )]\mathrm{d}\tau =0$$

$$\begin{array}{rl}\mathbf{E}\left[({\mathbf{w}}_n-\mathbf{E}[{\mathbf{w}}_n])({\mathbf{w}}_n-\mathbf{E}[{\mathbf{w}}_n]{)}^{\mathrm{T}}\right]& =\mathbf{E}\left[{\mathbf{w}}_n{\mathbf{w}}_n^{\mathrm{T}}\right]\\ & =\mathbf{E}\left[{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{w}(\tau )\mathrm{d}\tau {\int }_{n\Delta t}^{(n+1)\Delta t}{\mathbf{w}}^{\mathrm{T}}(\tau )\mathrm{d}\tau \right]\\ & =\mathbf{E}\left[{\int }_{n\Delta t}^{(n+1)\Delta t}{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{w}(\tau ){\mathbf{w}}^{\mathrm{T}}(\upsilon )\mathrm{d}\tau \mathrm{d}\upsilon \right]\\ & ={\int }_{n\Delta t}^{(n+1)\Delta t}{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{E}\left[\mathbf{w}(\tau ){\mathbf{w}}^{\mathrm{T}}(\upsilon )\right]\mathrm{d}\tau \mathrm{d}\upsilon \end{array}\text{\hspace{1em}(III-3-5)}$$

已知 $\mathbf{w}(t)$ 是随机白噪声, 故当 $\tau \ne \upsilon$ 时, $\mathbf{w}(t)$ 与 $\mathbf{w}(\upsilon )$ 独立不相关, 即
$$\mathbf{E}\left[\mathbf{w}(\tau ){\mathbf{w}}^{\mathrm{T}}(\upsilon )\right]=\left[\begin{array}{cc}{\sigma }_{{\mathbf{a}}_w}^2\mathbf{I}& 0\\ 0& {\sigma }_{{\mathbf{\omega }}_w}^2\mathbf{I}\end{array}\right]\delta (\tau -\upsilon )\text{\hspace{1em}(III-3-6)}$$
继续推导式 (III-3-5)
$$\begin{array}{rl}\mathbf{E}\left[({\mathbf{w}}_n-\mathbf{E}[{\mathbf{w}}_n])({\mathbf{w}}_n-\mathbf{E}[{\mathbf{w}}_n]{)}^{\mathrm{T}}\right]& ={\int }_{n\Delta t}^{(n+1)\Delta t}{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{E}\left[\mathbf{w}(\tau ){\mathbf{w}}^{\mathrm{T}}(\upsilon )\right]\mathrm{d}\tau \mathrm{d}\upsilon \\ & ={\int }_{n\Delta t}^{(n+1)\Delta t}{\int }_{n\Delta t}^{(n+1)\Delta t}\left[\begin{array}{cc}{\sigma }_{{\mathbf{a}}_w}^2\mathbf{I}& 0\\ 0& {\sigma }_{{\mathbf{\omega }}_w}^2\mathbf{I}\end{array}\right]\delta (\tau -\upsilon )\mathrm{d}\tau \mathrm{d}\upsilon \\ & ={\int }_{n\Delta t}^{(n+1)\Delta t}\left[\begin{array}{cc}{\sigma }_{{\mathbf{a}}_w}^2\mathbf{I}& 0\\ 0& {\sigma }_{{\mathbf{\omega }}_w}^2\mathbf{I}\end{array}\right]\mathrm{d}\upsilon \\ & =\left[\begin{array}{cc}{\sigma }_{{\mathbf{a}}_w}^2\mathbf{I}& 0\\ 0& {\sigma }_{{\mathbf{\omega }}_w}^2\mathbf{I}\end{array}\right]\Delta t\end{array}\text{\hspace{1em}(III-3-7)}$$
最终, 可知随机积分项
$${\mathbf{w}}_n\sim \mathcal{N}\left[0,\left[\begin{array}{cc}{\sigma }_{{\mathbf{a}}_w}^2\mathbf{I}& 0\\ 0& {\sigma }_{{\mathbf{\omega }}_w}^2\mathbf{I}\end{array}\right]\Delta t\right]\text{\hspace{1em}(III-3-8)}$$

4. 系统方程整理

这样式 (431) 就可以写成式 (435) 形式
$$\delta {\mathbf{x}}_{n+1}=\underset{\mathrm{deterministic}}{\underbrace{e^{\mathbf{A}\Delta t}\delta {\mathbf{x}}_n}}+\underset{\mathrm{stochastic}}{\underbrace{\mathbf{B}\Delta t{\tilde{\mathbf{u}}}_n+\mathbf{C}{\mathbf{w}}_n}}\text{\hspace{1em}(III-4-1)}$$
现在可以把两个随机项合并为一项,
$$\mathbf{B}\Delta t{\tilde{\mathbf{u}}}_n+\mathbf{C}{\mathbf{w}}_n=\left[\begin{array}{cc}B& C\end{array}\right]\left[\begin{array}{c}\Delta t{\tilde{\mathbf{u}}}_n\\ {\mathbf{w}}_n\end{array}\right]\text{\hspace{1em}(III-4-2)}$$
计算合并后的随机项的期望和协方差
$$\mathbf{E}[\mathbf{B}\Delta t{\tilde{\mathbf{u}}}_n+\mathbf{C}{\mathbf{w}}_n]=0\text{\hspace{1em}(III-4-3)}$$

$$\begin{array}{rl}& \mathbf{E}\left[(\mathbf{B}\Delta t{\tilde{\mathbf{u}}}_n+\mathbf{C}{\mathbf{w}}_n)(\mathbf{B}\Delta t{\tilde{\mathbf{u}}}_n+\mathbf{C}{\mathbf{w}}_n{)}^{\mathrm{T}}\right]\\ =& \mathbf{B}\mathbf{E}\left[(\Delta t{\tilde{\mathbf{u}}}_n)(\Delta t{\tilde{\mathbf{u}}}_n{)}^{\mathrm{T}}\right]{\mathbf{B}}^{\mathrm{T}}+\mathbf{C}\mathbf{E}\left[{\mathbf{w}}_n{\mathbf{w}}_n^{\mathrm{T}}\right]{\mathbf{C}}^{\mathrm{T}}\\ & +\mathbf{B}\mathbf{E}\left[(\Delta t{\tilde{\mathbf{u}}}_n){\mathbf{w}}_n^{\mathrm{T}}\right]{\mathbf{C}}^{\mathrm{T}}+\mathbf{C}\mathbf{E}\left[{\mathbf{w}}_n(\Delta t{\tilde{\mathbf{u}}}_n{)}^{\mathrm{T}}\right]{\mathbf{B}}^{\mathrm{T}}\end{array}\text{\hspace{1em}(III-4-4)}$$

需要说明 $\Delta t{\tilde{\mathbf{u}}}_n$ 与 ${\mathbf{w}}_n$ 之间相互独立, 故有
$$\mathbf{E}\left[(\Delta t{\tilde{\mathbf{u}}}_n){\mathbf{w}}_n^{\mathrm{T}}\right]=\mathbf{E}\left[\Delta t{\tilde{\mathbf{u}}}_n\right]\mathbf{E}{\left[{\mathbf{w}}_n\right]}^{\mathrm{T}}=0\text{\hspace{1em}(III-4-5)}$$
同理
$$\mathbf{E}\left[{\mathbf{w}}_n(\Delta t{\tilde{\mathbf{u}}}_n{)}^{\mathrm{T}}\right]=0\text{\hspace{1em}(III-4-6)}$$
这样协方差计算式 (III-4-4) 就有
$$\begin{array}{rl}& \mathbf{E}\left[(\mathbf{B}\Delta t{\tilde{\mathbf{u}}}_n+\mathbf{C}{\mathbf{w}}_n)(\mathbf{B}\Delta t{\tilde{\mathbf{u}}}_n+\mathbf{C}{\mathbf{w}}_n{)}^{\mathrm{T}}\right]\\ =& \mathbf{B}\mathbf{E}\left[(\Delta t{\tilde{\mathbf{u}}}_n)(\Delta t{\tilde{\mathbf{u}}}_n{)}^{\mathrm{T}}\right]{\mathbf{B}}^{\mathrm{T}}+\mathbf{C}\mathbf{E}\left[{\mathbf{w}}_n{\mathbf{w}}_n^{\mathrm{T}}\right]{\mathbf{C}}^{\mathrm{T}}\\ \stackrel{(451),(\mathrm{III}-3-3),(\mathrm{III}-3-8)}{=}& \left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& {\sigma }_{{\tilde{\mathbf{a}}}_n}^2{\Delta t}^2\mathbf{I}& 0& 0& 0& 0\\ 0& 0& {\sigma }_{{\tilde{\mathbf{\omega }}}_n}^2{\Delta t}^2\mathbf{I}& 0& 0& 0\\ 0& 0& 0& {\sigma }_{{\mathbf{a}}_w}^2\Delta t\mathbf{I}& 0& 0\\ 0& 0& 0& 0& {\sigma }_{{\mathbf{\omega }}_w}^2\Delta t\mathbf{I}& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]\end{array}\text{\hspace{1em}(III-4-7)}$$
这样式 (III-4-1) 进一步简写为
$$\delta {\mathbf{x}}_{n+1}=e^{\mathbf{A}\Delta t}\delta {\mathbf{x}}_n+\mathbf{i}\text{\hspace{1em}(III-4-8)}$$

$$\mathbf{i}\sim \mathcal{N}\left[0_{6\times 1},\left[\begin{array}{cccccc}0& & & & & \\ & {\sigma }_{{\tilde{\mathbf{a}}}_n}^2{\Delta t}^2\mathbf{I}& & & 0& \\ & & {\sigma }_{{\tilde{\mathbf{\omega }}}_n}^2{\Delta t}^2\mathbf{I}& & & \\ & & & {\sigma }_{{\mathbf{a}}_w}^2\Delta t\mathbf{I}& & \\ & 0& & & {\sigma }_{{\mathbf{\omega }}_w}^2\Delta t\mathbf{I}& \\ & & & & & 0\end{array}\right]\right]\text{\hspace{1em}(III-4-9)}$$

另外由式 (451) 中 $\mathbf{B}$ 和 $\mathbf{C}$ 的结构, 合并的随机项不会直接在状态 $\delta \mathbf{p}$ 和 $\mathbf{g}$ 的运动方程中呈现, 为了减少计算量, 直接设定相应 $\mathbf{i}$ 的分量为 0

$$\mathbf{i}=\left[\begin{array}{c}0\\ {\mathbf{v}}_{\mathbf{i}}\\ {\mathbf{\theta }}_{\mathbf{i}}\\ {\mathbf{a}}_{\mathbf{i}}\\ {\mathbf{\omega }}_{\mathbf{i}}\\ 0\end{array}\right]\text{\hspace{1em}(459)}$$

结论

本文主要关注 IMU 误差建模部分, 对 IMU 噪声和扰动中的白噪声和随机游走进行了分析, 噪声和扰动在误差状态模型中的积分及分布, 体现了误差和扰动在系统中的传递过程.
本文完毕.

参考文献

[1] Joan Solà, "Quaternion kinematics for the error-state Kalman filter", arXiv, 2017, https://arxiv.org/abs/1711.02508

[2] "BROWNIAN MOTION", http://galton.uchicago.edu/~lalley/Courses/312/BrownianMotion312.pdf, in "Statistics 312: Stochastic Processes Autumn 2016", by Steve Lalley, http://galton.uchicago.edu/~lalley/Courses/312/

[3] “ONE-DIMENSIONAL RANDOM WALKS”, http://galton.uchicago.edu/~lalley/Courses/312/RW.pdf, in “Statistics 312: Stochastic Processes Autumn 2016”, by Steve Lalley, http://galton.uchicago.edu/~lalley/Courses/312/