IMU – MathSword数值计算软件

1 背景
在使用传感器采集到一些数据后，需要计算车辆位姿。在融合GPS位置、IMU角速度、轮速后，可以通过ESKF进行求解。ESKF本质还是一个EKF过程，只是这里面更新量换成了值较小的误差状态量。由于是EKF过程，只需要导出下面公式(10)里的$F、F_i、H$矩阵，就可以使用EKF过程进行递推。

2 原理
设真值状态变量 $\mathbf{x}=[\mathbf{p},\theta,\mathbf{b}]$,其分别表示位置、朝向、车辆坐标系下角速度偏置，误差状态量为 $\delta\mathbf{x}=[\delta\mathbf{p},\delta\theta,\delta\mathbf{b}]$，$t$时刻含有噪声状态量为 $\mathbf{x}_t=[\mathbf{p}_t,\theta_t,\mathbf{b}_t]$。状态变量之间使用下式进行关联
(1)
$$
\begin{cases}
\mathbf{p}_t &=\mathbf{p}+\delta\mathbf{p}
\\
\theta_t&=\theta+\delta\theta&,R_t=\exp(\theta_t^\wedge) \approx \exp(\theta^\wedge)\exp(\delta\theta^\wedge)=R\exp(\delta\theta^\wedge)
\\
\mathbf{b}_t&=\mathbf{b}+\delta\mathbf{b}
\end{cases}
$$
以常见的四轮汽车模型为例，现在已知车辆坐标系下的后左右轮速$v_l,v_r$以及角速度$\bar{\mathbf{w}}$,这里以后左右轮中心作为车辆坐标系原点，设$\bar{\mathbf{v}}=[\dfrac{v_x+v_y}{2},0,0]$，速度与角速度噪声为$\eta_v,\eta_w$，接下来需要找到满足如下方程的$F,F_i$
(2)
$$
\delta\mathbf{x}=F\delta\mathbf{x}+F_i\begin{bmatrix}
\eta_v
\\
\eta_w
\end{bmatrix}
$$
根据物理模型有
(3)
$$
\begin{aligned}
&\begin{cases}
\dot{\mathbf{p}}_t &=R_t(\bar{\mathbf{v}}+\eta_v)
\\
\dot{R}_t&=R_t(\bar{\mathbf{w}}-\mathbf{b}_t-\eta_w)^\wedge
\\
\dot{\mathbf{b}}_t&=\eta_w
\end{cases}
\\
&\begin{cases}
\dot{\mathbf{p}} &=R\bar{\mathbf{v}}
\\
\dot{R}&=R(\bar{\mathbf{w}}-\mathbf{b})^\wedge
\\
\dot{\mathbf{b}}&=\mathbf{0}
\end{cases}
\end{aligned}
$$
对于位置更新，结合(1)(3)可得
(4)
$$
\begin{aligned}
\delta \dot{\mathbf{p}} &=\dot{\mathbf{p}}_t-\dot{\mathbf{p}}
\\
&=(R_t-R)\bar{\mathbf{v}}+R_t\eta_v
\\
&\approx (R\exp(\delta \theta^\wedge)-R)\bar{\mathbf{v}}+R\exp(\delta \theta^\wedge)\eta_v
\\
&\approx (R(I+\delta \theta^\wedge)-R)\bar{\mathbf{v}}+R(I+\delta \theta^\wedge)\eta_v
\\
&=R\delta\theta^\wedge\bar{\mathbf{v}}+R\delta\theta^\wedge\eta_v+R\eta_v
\\
&=-R(\bar{\mathbf{v}}+\eta_v)^\wedge\delta\theta+R\eta_v
\\
&\approx -R\bar{\mathbf{v}}^\wedge\delta\theta+R\eta_v
\end{aligned}
$$
对于旋转部分，结合(1)(3)可得
(5)
$$
\begin{aligned}
\dot{R}_t&=\dot{R}\exp(\delta\theta^\wedge)+R\dot{\exp(\delta \theta^\wedge)}
\\&=\dot{R}\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)(\mathbf{J}_r(\delta\theta)\delta\dot{\theta})^\wedge
\\&\Downarrow
\\
\dot{R}_t&\approx\dot{R}\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
R_t(\bar{\mathbf{w}}-\mathbf{b}_t-\eta_w)^\wedge&=R(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
R\exp(\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge&=R(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
\exp(\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge&=(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)+\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
\delta\dot{\theta}^\wedge&=(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge-\exp(-\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)
\\
&\approx(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge-(I-\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b})^\wedge(I+\delta\theta^\wedge)
\\
&=(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge
\\
&=-(\delta\mathbf{b}+\eta_w)^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})
\\
&\approx -(\delta\mathbf{b}+\eta_w)^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge
\\
&= -(\delta\mathbf{b}+\eta_w)^\wedge+(\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b}))^\wedge
\\
&=-(\delta\mathbf{b}+\eta_w)^\wedge-((\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta)^\wedge
\\&\Downarrow
\\
\delta\dot{\theta}&=-(\delta\mathbf{b}+\eta_w)-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta
\end{aligned}
$$
对于角速度偏置部分，结合(1)(3)可以得到
(6)
$$
\delta\dot{\mathbf{b}}=\dot{\mathbf{b}}_t-\dot{\mathbf{b}} =\eta_w
$$
因此在一个小时间$\triangle t$内，综合(3)(4)(5)可以得到
(7)
$$
\begin{aligned}
&\begin{cases}
\delta \mathbf{p}_{t+\triangle t}&=\delta \mathbf{p}_t+\triangle t\delta \dot{\mathbf{p}}=\delta \mathbf{p}_t-\triangle tR\bar{\mathbf{v}}^\wedge \delta \theta_t+\triangle tR\eta_v
\\
\delta \theta_{t+\triangle t}&=\delta \theta_t+\triangle t\delta\dot{\theta}=\delta \theta_t-\triangle t(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta_t-\triangle t(\delta\mathbf{b}_t+\eta_w)
\\
\delta\mathbf{b}_{t+\triangle t}&=\delta\mathbf{b}_t+\triangle t\delta\dot{\mathbf{b}}=\delta\mathbf{b}_t+\triangle t\eta_w
\end{cases}
\\
&\Downarrow
\\
&F=\begin{bmatrix}
I &-\triangle tR\bar{\mathbf{v}}^\wedge &\mathbf{0}
\\
\mathbf{0} & I-\triangle t(\bar{\mathbf{w}}-\mathbf{b})^\wedge &-\triangle tI
\\
\mathbf{0}&\mathbf{0} & I
\end{bmatrix}
\approx \begin{bmatrix}
I &-\triangle tR\bar{\mathbf{v}}^\wedge &\mathbf{0}
\\
\mathbf{0} & \exp(-\triangle t(\bar{\mathbf{w}}-\mathbf{b})^\wedge) &-\triangle tI
\\
\mathbf{0}&\mathbf{0} & I
\end{bmatrix}
\\
&F_i=\triangle t\begin{bmatrix}
R &\mathbf{0}
\\
\mathbf{0} &-I
\\
\mathbf{0} & I
\end{bmatrix}
\end{aligned}
$$
接下来，以GPS位置 $\bar{\mathbf{p}}$ 作为观测，因此建立观测函数
(8)
$$
\begin{aligned}
\bar{\mathbf{p}} &=z(\mathbf{x},\delta \mathbf{x})
\\
&=\mathbf{p}_t+\triangle tR_t\bar{\mathbf{v}}
\\
&=\mathbf{p}+\delta\mathbf{p}+\triangle tR\exp(\delta\theta^\wedge)\bar{\mathbf{v}}
\\
&\Downarrow
\\
&\begin{cases}
\dfrac{\partial\bar{\mathbf{p}}}{\partial\delta \mathbf{p}}&=I
\\
\\
\dfrac{\partial\bar{\mathbf{p}}}{\partial\delta \theta}&=\triangle tR\dfrac{\partial(\exp(\delta\theta^\wedge)\bar{\mathbf{v}})}{\partial\delta \theta}\approx-\triangle tR(\exp(\delta\theta^\wedge)\bar{\mathbf{v}})^\wedge
\\
\\
\dfrac{\partial\bar{\mathbf{p}}}{\partial\delta \mathbf{b}}&=\mathbf{0}
\end{cases}
\\
&\Downarrow
\\
H&=\begin{bmatrix}
I & -\triangle tR(\exp(\delta\theta^\wedge)\bar{\mathbf{v}})^\wedge &\mathbf{0}
\end{bmatrix}\approx
\begin{bmatrix}
I & -\triangle tR\bar{v}^\wedge&\mathbf{0}
\end{bmatrix}
\end{aligned}
$$
注意，$H$计算中，忽略了二阶小量，这里$\triangle t、\delta\theta$均为小量。

EKF更新完成后，需要对协方差矩阵$P$进行重置。设 $\delta \mathbf{x}'=[\delta \mathbf{p}’,\delta \theta’,\delta\mathbf{b}’]$为$\delta \mathbf{x}$受到均值 $\delta \bar{\mathbf{x}}=[\delta \bar{\mathbf{p}},\delta \bar{\theta},\delta\bar{\mathbf{b}}]$的影响,因此按照逆重构形式(减法)找到一个满足$\delta \mathbf{x}'=G\delta\mathbf{x}+…$形式的重置矩阵$G$:
(9)
$$
\begin{aligned}
&\begin{cases}
\delta \mathbf{p}'&=\delta\mathbf{p}-\delta\bar{\mathbf{p}}
\\
\\
\delta \theta'&=\log(\exp(\delta\theta'^\wedge))^\vee
\\
&\approx \log(\exp(\delta\theta^\wedge)\exp(-\delta\bar{\theta})^\wedge)^\vee
\\
&\approx (I-\dfrac{1}{2}\delta\bar{\theta}^\wedge)\delta\theta-\delta\bar{\theta}
\\
\\
\delta\mathbf{b}'&=\delta\mathbf{b}-\delta\bar{\mathbf{b}}
\end{cases}
\\
&\Downarrow
\\
&G=\begin{bmatrix}
I &\mathbf{0} & \mathbf{0}
\\
\mathbf{0} &I-\dfrac{1}{2}\delta\bar{\theta}^\wedge&\mathbf{0}
\\
\mathbf{0} & \mathbf{0} & I
\end{bmatrix}
\end{aligned}
$$

3 算法过程
因此，最终的ESKF过程为
(10)
$$
\begin{aligned}
\text{预测}:&\begin{cases}
\delta \mathbf{x}&=F\delta\mathbf{x}
\\
P&=FPF^T+F_iQF_i^T
\end{cases}
\\
\text{增益}:&\begin{cases}
K&=PH^T(HPH^T+V)^{-1}
\end{cases}
\\
\text{更新}:&\begin{cases}
\delta \mathbf{x}&=K(\bar{\mathbf{p}}-z(\mathbf{x},\delta\mathbf{x}))
\\
\mathbf{x}_{t+\triangle t}&=\mathbf{x}_t+\delta\mathbf{x}
\\
P&=(I-KH)P(I-KH)^T+KVK^T
\end{cases}
\\
\text{重置}:&\begin{cases}
P&=GPG^T
\end{cases}
\end{aligned}
$$
公式里$V$为$3\times 3$的GPS观察的协方差矩阵，而$Q$是噪声协方差矩阵，$Q=\mathbf{Diag}(Cov(\eta_v),Cov(\eta_w))$，特别的，对于如下形式的$Q$可进行简化
(11)
$$
\begin{aligned}
Q&=\begin{bmatrix}
\alpha I&\mathbf{0}
\\
\mathbf{0} &\beta I
\end{bmatrix}
\\
&\Downarrow
\\
F_iQF_i^T&=\triangle t^2\begin{bmatrix}
\alpha I &\mathbf{0} & \mathbf{0}
\\
\mathbf{0} &\beta I & \mathbf{0}
\\
\mathbf{0} & \mathbf{0} &\beta I
\end{bmatrix}
\end{aligned}
$$
注意:
① 公式(10)当中$P$的表达式与参考[2]里的不一致，公式(10)里的表达式不局限于$P$是否为方阵，且其有更好的数值稳定性。
② 由于 $\delta \bar{\theta}$为0附近一个小量，因此在大部分场景里，可以忽略不计，这时 $G$就等价于单位阵，因此不使用重置步骤。

③ $Q、V$由仪器提供，或者按经验设置。

④ 计算过程中，注意上面公式里的位置$\mathbf{p}$是全局坐标系，$\bar{\mathbf{v}}、\mathbf{w}、\mathbf{b}$为车辆局部坐标系，旋转矩阵$R$为局部坐标系到世界坐标系的表示方式。

参考
[1] Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.
[2] 高翔. 简明ESKF推导. 2022

1 背景
在一般的车载传感器上，都会提供IMU惯性测量单元，它主要提供各个时刻的线性加速度(沿着x、y、z的加速度)以及角速度(绕着x、y、z方向的角速度)测量值。由于这些测量值之间与车本身的位姿有一定关系，因此可以通过IMU测量值来估计车本身的位姿。

2 原理
学过初中物理就应该知道，一个物体，已知速度与加速度，这时经过一段时间后，可以计算出各个时间点的速度以及位移值，在整个过程计算中速度、加速度在不断变化，因此求每个时刻的值需要用到积分。由于处理的离散数据，这里的积分可直接累计（累加）的方式进行积分近似。因此所有公式均是基于此推导出来。唯一不同的是，公式里会涉及到旋转矩阵。这里根据文献^[1]，提供如下的IMU的数学模型
(1)
$$
\begin{aligned}
\bar{\omega}_b(t) &= \omega_b(t) + b^\omega(t)+\eta^\omega(t)
\\
\bar{a}_b(t)&=R_{wb}^T(a_w(t)-g_w(t)) + b^a(t)+\eta^a(t)
\end{aligned}
$$
公式中参数含义如下：
$\bar{\omega}_b(t)$:表示$t$时刻测量到车载坐标系下的角速度。
$\omega_b(t)$:表示$t$时刻计算到的车载坐标系下的角速度。
$b^\omega(t)$:表示$t$时刻角速度的偏置，这个参数需要优化。
$\eta^\omega(t)$:表示$t$时刻角速度的白噪声。
$\bar{a}_b(t)$:表示$t$时刻测量到车载坐标系下的线性加速度。
$a_w(t)$:表示$t$时刻计算到的世界坐标系下的线性加速度。
$g_w(t)$:表示$t$时刻世界坐标系下的重力加速度。
$b^a(t)$:表示$t$时刻线性加速度的偏置，这个参数需要优化。
$\eta^a(t)$:表示$t$时刻线性加速度的白噪声。
$R_{wb}(t)$:表示$t$时刻从车载坐标系到世界坐标系的旋转变换。

对于世界坐标系下的速度$v_w(t)$以及位置$p_w(t)$，根据运动方程有
(2)
$$
\begin{aligned}
\dot{v}_w(t) &= a_w(t)
\\
\dot{p}_w(t) &=v_w(t)
\\
\dot{R}_{wb}&=R_{wb}\omega_b^\wedge
\end{aligned}
$$
对上面的公式采用欧拉积分有
(3)
$$
\begin{aligned}
v_w(t+\Delta t) &= v_w(t) + a_w(t)\Delta t
\\
p_w(t+\Delta t)&=p_w(t)+v_w(t)\Delta t+\dfrac{1}{2}a_w(t)\Delta t^2
\\
R_{wb}(t+\Delta t)&= R_{wb}(t)\exp(\omega_b(t)\Delta t)
\end{aligned}
$$
将公式(1)代入公式(3)有
(4)
$$
\begin{aligned}
v_w(t+\Delta t) &= v_w(t)+g_w(t)\Delta t+R_{wb}(t)(\bar{a}_b(t)-b^a(t)-\eta^a(t))\Delta t
\\
p_w(t+\Delta t)&=p_w(t)+v_w(t)\Delta t+\dfrac{1}{2}g_w(t)\Delta t^2+\dfrac{1}{2}R_{wb}(t)(\bar{a}_b(t)-b^a(t)-\eta^a(t))\Delta t^2
\\
R_{wb}(t+\Delta t)&= R_{wb}(t)\exp((\bar{\omega}_b(t)-b^\omega(t) – \eta^\omega(t))\Delta t)
\end{aligned}
$$
这里记
(5)
$$
\begin{aligned}
R_i&=R_{wb}(t)|_{t=i}
\\
p_i&=p_w(t)|_{t=i}
\\
v_i&=v_w(t)|_{t=i}
\\
\bar{\omega}_i&=\bar{\omega}_b(t)|_{t=i}
\\
\omega_i&=\omega_b(t)|_{t=i}
\\
\bar{a}_i&=\bar{a}_b(t)|_{t=i}
\\
a_i&=a_w(t)|_{t=i}
\\
b^\omega_i&=b^\omega(t)|_{t=i}
\\
\eta^\omega_i&=\eta^\omega(t)|_{t=i}
\\
b^a_i&=b^a(t)|_{t=i}
\\
\eta^a_i&=\eta^a(t)|_{t=i}
\\
g&=g_w(t)
\\
\Delta t_{ij}&=\sum_{k=i}^{j-1} \Delta t
\end{aligned}
$$
上面假设重力加速度$g_w(t)$为常量。
这里定义如下增量公式
(6)
$$
\begin{aligned}
\Delta R_{ij} &=R_i^TR_j
\\
&=\prod_{k=i}^{j-1}\exp((\bar{\omega}_k-b^\omega_k – \eta^\omega_k)\Delta t)
\\
\\
\Delta v_{ij}&=v_j-v_i
\\
&=R_i^T(v_j-v_i-g\Delta t_{ij})
\\
&= \sum_{k=i}^{j-1}\Delta R_{ik}(\bar{a}_k-b^a_k-\eta^a_k)\Delta t
\\
\\
\Delta p_{ij}&=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t_{ij}^2)
\\
&=\sum_{k=i}^{j-1}[\Delta v_{ik}\Delta t+\dfrac{1}{2}\Delta R_{ik}(\bar{a}_k-b^a_k-\eta^a_k)\Delta t^2]
\end{aligned}
$$
最终根据文献[1]得到如下预积分模型(实际使用的就是如下的模型,即如下约等于符号后的表达式)
(7)
$$
\begin{aligned}
\Delta \bar{R}_{ij}&=R_i^TR_j\exp(\delta \phi_{ij})
\\
&\approx R_i^TR_j
\\
\\
\Delta \bar{v}_{ij}&=R_i^T(v_j-v_i-g\Delta t_{ij})+\delta v_{ij}
\\
&\approx R_i^T(v_j-v_i-g\Delta t_{ij})
\\
\\
\Delta \bar{p}_{ij}&=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t^2_{ij})+\delta p_{ij}
\\
&\approx R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t^2_{ij})
\end{aligned}
$$
公式中$\Delta \bar{R}_{ij}$、$\Delta \bar{v}_{ij}$、$\Delta \bar{p}_{ij}$为定义的预积分测量变量，$\delta R_{ij}$、$\delta v_{ij}$、$\delta p_{ij}$为对应白噪声。

如果公式(1)当中的偏置不为常数，则每次需要更新预积分测量变量，这里采用如下公式更新
(8)
$$
\begin{aligned}
\Delta \bar{R}_{ij}&\approx \Delta \bar{R}_{ij} \exp (\dfrac{ \partial \Delta \bar{R}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i)
\\
\Delta \bar{v}_{ij}&\approx \Delta \bar{v}_{ij}+\dfrac{ \partial \Delta \bar{v}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i+\dfrac{ \partial \Delta \bar{v}_{ij}}{ \partial b^a_i} \delta b^a_i
\\
\Delta \bar{p}_{ij}&\approx \Delta \bar{p}_{ij}+\dfrac{ \partial \Delta \bar{p}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i+\dfrac{ \partial \Delta \bar{p}_{ij}}{ \partial b^a_i} \delta b^a_i
\end{aligned}
$$
上面公式中的偏导数可以参考文献[2].

接下来，我们可以将计算的$\Delta \bar{R}_{ij}$、$\Delta \bar{v}_{ij}$、$\Delta \bar{p}_{ij}$作为测量值，和外部其它变量建立约束。对相关变量进行更新，在实际优化求解中需要优化的变量为$R_i$、$p_i$、$v_i$、$R_j$、$p_j$、$v_j$、$\delta b^\omega_i$、$\delta b^a_i$。注意变量按如下方式更新
(9)
$$
\begin{aligned}
R_i &= R_i \exp(\delta \phi_i)
\\
p_i &= p_i + R_i \delta p_i
\\
v_i &= v_i + \delta v_i
\\
R_j &= R_j \exp(\delta \phi_j)
\\
p_j &= p_j + R_j \delta p_j
\\
v_j &= v_j + \delta v_j
\\
\delta b^\omega_i &= \delta b^\omega_i + \bar{\delta b^\omega_i}
\\
\delta b^a_i &= \delta b^a_i + \bar{\delta b^a_i}
\end{aligned}
$$

参考
[1] Forster C , Carlone L , Dellaert F , et al. IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation (supplementary material)[J]. Georgia Institute of Technology, 2015.

[2] 苏笑云.IMU预积分的理解和推导

[3] 高翔.简明预积分推导

标签： IMU

基于GPS、IMU、轮速的ESKF

IMU预积分