IMU预积分相关推导

IMU数学模型

$$\begin{array}{rl}{\tilde{\omega }}_t^b& ={\omega }_t^b+b_t^g+n_t^g\\ {\tilde{a}}_t^b& =a_t^b+b_t^a+n_t^a\\ & =R_w^t(a_t^w-g^w)+b_t^a+n_t^a\end{array}$$

• ${\tilde{\omega }}_t^b$：$t$时刻陀螺仪的读数，上标$b$表示在Body系
• ${\omega }_t^b$：角速度真值
• $b_t^g$：陀螺仪的随机游走
• $n_t^g$：陀螺仪的噪声
• ${\tilde{a}}_t^b$：加速计的度数
• $a_t^b$：Body系下加速度真值，其包含重力加速度$g^w$
• $R_w^t$：世界系到$t$时刻Body系的旋转
• $a_t^w$：世界系下加速度得真值，不包含重力加速度$g^w$
• $g^w$：世界系下重力加速度
• $b_t^a$：加速计的随机游走
• $n_t^a$：加速计的噪声

噪声性质：
$$\begin{array}{rl}连续时间：& \\ n_t^g& \sim \mathcal{N}(0,{\sigma }_g^2)\\ n_t^a& \sim \mathcal{N}(0,{\sigma }_a^2)\\ n_t^{ba}={\dot{b}}_t^a& \sim \mathcal{N}(0,{\sigma }_{ba}^2)\\ n_t^{bg}={\dot{b}}_t^g& \sim \mathcal{N}(0,{\sigma }_{bg}^2)\\ 离散时间：采样间隔\Delta t& \\ n_k^g& \sim \mathcal{N}(0,\frac{1}{\Delta t}{\sigma }_g^2)\\ n_k^a& \sim \mathcal{N}(0,\frac{1}{\Delta t}{\sigma }_a^2)\\ n_k^{ba}={\dot{b}}_k^a& \sim \mathcal{N}(0,\Delta t{\sigma }_{ba}^2)\\ n_k^{bg}={\dot{b}}_k^g& \sim \mathcal{N}(0,\Delta t{\sigma }_{bg}^2)\end{array}$$

连续时间积分

状态变量：$[p_t^w,v_t^w,q_t^w,b_t^a,b_t^g{]}^T$

• $p_t^w$：$t$时刻imu在世界系下的位置
• $v_t^w$：$t$时刻imu在世界系下的速度
• $q_t^w$：$t$时刻imu相对世界系的旋转

运动模型：
$$\begin{array}{rl}{\dot{p}}_t^w& =v_t^w\\ {\dot{v}}_t^w& =a_t^w\\ {\dot{q}}_t^w& =q_t^w\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\omega }_t^b\end{array}\right]\\ \text{or}{\dot{R}}_t^w& =R_t^w\cdot [{\omega }_t^b{]}_{\times }\\ {\dot{b}}_t^a& =n_t^{ba}\\ {\dot{b}}_t^g& =n_t^{bg}\end{array}$$
从$t$时刻到$t+\Delta t$时刻状态积分
$$\begin{array}{rl}{p}_{t+\Delta t}^w& =p_t^w+{\int }_t^{t+\Delta t}{v}_{\tau }^{w}d\tau +{\iint }_t^{t+\Delta t}{a}_{\tau }^{w}d{\tau }^2\\ & =p_t^w+{\int }_t^{t+\Delta t}{v}_{\tau }^{w}d\tau +{\iint }_t^{t+\Delta t}(R_{\tau }^w{a}_{\tau }^b+g^w)d{\tau }^2\\ & =p_t^w+\frac{1}{2}{g}^w\Delta t^2+{\int }_t^{t+\Delta t}{v}_{\tau }^{w}d\tau +{\iint }_t^{t+\Delta t}{R}_{\tau }^w{a}_{\tau }^{b}d{\tau }^2\\ v_{t+\Delta t}^w& =v_t^w+{\int }_t^{t+\Delta t}{a}_{\tau }^{w}d\tau \\ & =v_t^w+{\int }_t^{t+\Delta t}(R_{\tau }^w{a}_{\tau }^b+g^w)d\tau \\ & =v_t^w+g^w\Delta t+{\int }_t^{t+\Delta t}{R}_{\tau }^w{a}_{\tau }^{b}d\tau \\ q_{t+\Delta t}^w& =q_t^w\otimes {\int }_t^{t+\Delta t}{q}_{\tau }^i\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\omega }_{\tau }^b\end{array}\right]d\tau \\ R_{t+\Delta t}^w& =R_t^{w}Exp\left({\int }_t^{t+\Delta t}{\omega }_{\tau }^{b}d\tau \right)\end{array}$$

离散时间积分

Imu的采样间隔为$\Delta t$，假设在$\Delta t$时间内$a_t^w$和${\omega }_t^b$恒定
$$\begin{array}{rl}{p}_{t+\Delta t}^w& =p_t^w+v_t^w\Delta t+\frac{1}{2}{g}^w\Delta t^2+\frac{1}{2}{R}_t^w{a}_t^b\Delta t^2\\ v_{t+\Delta t}^w& =v_t^w+g^w\Delta t+R_t^w{a}_t^b\Delta t\\ R_{t+\Delta t}^w& =R_t^{w}Exp({\omega }_t^b\Delta t)\end{array}$$
离散时间$[i,j]$之间的Imu数据累积：
$$\begin{array}{rl}{R}_j^w& =R_i^w\prod _{k=i}^{j-1}Exp({\omega }_k^b\Delta t)\\ v_j^w& =v_i^w+\sum _{k=i}^{j-1}{g}^w\Delta t+\sum _{k=i}^{j-1}{R}_k^w{a}_k^b\Delta t\\ p_j^w& =p_i^w+\sum _{k=i}^{j-1}{v}_k^w\Delta t+\frac{1}{2}\sum _{k=i}^{j-1}{g}^w\Delta t^2+\frac{1}{2}\sum _{k=i}^{j-1}{R}_k^w{a}_k^b\Delta t^2\end{array}$$

离散时间预积分

写法1

上式两侧同时乘以$R_w^i$，整理得：
$$\begin{array}{rl}\Delta R_j^i& =R_w^i{R}_j^w=\prod _{k=i}^{j-1}Exp({\omega }_k^b\Delta t)\\ \Delta v_j^i& =R_w^i(v_j^w-v_i^w-g^w\Delta t_{ij})\\ & =\sum _{k=i}^{j-1}{R}_k^i{a}_k^b\Delta t\\ \Delta p_j^i& =R_w^i(p_j^w-p_i^w-v_i^w\Delta t_{ij}-\frac{1}{2}{g}^w\Delta t_{ij}^2)\\ & =\sum _{k=i}^{j-1}\left[\Delta v_k^i\Delta t+\frac{1}{2}{R}_k^i{a}_k^b\Delta t^2\right]\end{array}$$
其中$\Delta p_j^i$项证明如下：
$$\begin{array}{rl}& p_j^w-p_i^w-v_i^w\Delta t_{ij}-\frac{1}{2}{g}^w\Delta t_{ij}^2\\ & =\sum _{k=i}^{j-1}{v}_k^w\Delta t+\frac{1}{2}\sum _{k=i}^{j-1}{g}^w\Delta t^2+\frac{1}{2}\sum _{k=i}^{j-1}{R}_k^w{a}_k^b\Delta t^2-v_i^w\Delta t_{ij}-\frac{1}{2}{g}^w\Delta t_{ij}^2\\ & =\sum _{k=i}^{j-1}{v}_k^w\Delta t-\sum _{k=i}^{j-1}{v}_i^w\Delta t+\frac{1}{2}(j-i)g^w\Delta t^2-\frac{1}{2}(j-i{)}^2{g}^w\Delta t^2+\frac{1}{2}\sum _{k=i}^{j-1}{R}_k^w{a}_k^b\Delta t^2\\ & =\sum _{k=i}^{j-1}(v_k^w-v_i^w)\Delta t-\sum _{k=i}^{j-1}(k-i)g^w\Delta t^2+\frac{1}{2}\sum _{k=i}^{j-1}{R}_k^w{a}_k^b\Delta t^2\\ & =\sum _{k=i}^{j-1}(v_k^w-v_i^w-g^w\Delta t_{ik})\Delta t+\frac{1}{2}\sum _{k=i}^{j-1}{R}_k^w{a}_k^b\Delta t^2\\ & =\sum _{k=i}^{j-1}\left[R_i^w\Delta v_k^i\Delta t\right]+\frac{1}{2}\sum _{k=i}^{j-1}{R}_k^w{a}_k^b\Delta t^2\end{array}$$
递推写法：
$$\begin{array}{rl}\Delta R_{k+1}^i& =\Delta R_k^{i}Exp({\omega }_k^b\Delta t)\\ \Delta v_{k+1}^i& =\Delta v_k^i+R_k^i{a}_k^b\Delta t\\ \Delta p_{k+1}^i& =\Delta p_k^i+\Delta v_k^i\Delta t+\frac{1}{2}{R}_k^i{a}_k^b\Delta t^2\end{array}$$

写法2

另一种写法：
$$\begin{array}{rl}Rotation:{\gamma }_{k+1}^i& ={\gamma }_k^i\circ Exp({\omega }_k^b\Delta t)\\ Velocity:{\beta }_{k+1}^i& ={\beta }_k^i+{\gamma }_k^i{a}_k^b\Delta t\\ Position:{\alpha }_{k+1}^i& ={\alpha }_k^i+{\beta }_k^i\Delta t+\frac{1}{2}{\gamma }_k^i{a}_k^b\Delta t^2\end{array}$$

• 用四元数$S^3$表示$\gamma$：
  $$\begin{array}{rl}{\gamma }_{k+1}^i& ={\gamma }_k^i\otimes Exp({\omega }_k^b\Delta t)\\ & ={\gamma }_k^i\otimes \exp (\frac{1}{2}{\omega }_k^b\Delta t)\\ & \approx {\gamma }_k^i\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\omega }_k^b\Delta t\end{array}\right]\end{array}$$

• 用旋转矩阵$SO(3)$表示$\gamma$：
  $$\begin{array}{rl}{\gamma }_{k+1}^i& ={\gamma }_k^{i}Exp({\omega }_k^b\Delta t)\\ & ={\gamma }_k^i\exp ([{\omega }_k^b\Delta t{]}_{\times })\end{array}$$

这里的一系列的操作只为导出预积分量$[\alpha ,\beta ,\gamma ]$，它们的计算与系统其他状态无关，只与Imu的输入相关。注意的是预积分量中包含重力加速度的积分量。

欧拉法与中值法

离散时间$[i,j]$之间，从第$k$到$k+1$时刻的积分。

欧拉法

$$\begin{array}{rl}{\omega }_k^b& ={\tilde{\omega }}_k^b-b_k^g\\ {\gamma }_{k+1}^i& ={\gamma }_k^{i}Exp({\omega }_k^b\Delta t)\\ a_k^b& ={\tilde{a}}_k^b-b_k^a\\ {\beta }_{k+1}^i& ={\beta }_k^i+{\gamma }_k^i{a}_k^b\Delta t\\ {\alpha }_{k+1}^i& ={\alpha }_k^i+{\beta }_k^i\Delta t+\frac{1}{2}{\gamma }_k^i{a}_k^b\Delta t^2\\ b_{k+1}^a& =b_k^a+n_k^{ba}\Delta t\\ b_{k+1}^g& =b_k^g+n_k^{bg}\Delta t\end{array}$$

中值法

$$\begin{array}{rl}{\omega }_k^b& =\frac{1}{2}[({\tilde{\omega }}_k^b-b_k^g)+({\tilde{\omega }}_{k+1}^b-b_k^g)]\\ {\gamma }_{k+1}^i& ={\gamma }_k^{i}Exp({\omega }_k^b\Delta t)\\ a_k^b& =\frac{1}{2}\left[({\tilde{a}}_k^b-b_k^a)+Exp({\omega }_k^b\Delta t)({\tilde{a}}_{k+1}^b-b_k^a)\right]\\ {\beta }_{k+1}^i& ={\beta }_k^i+{\gamma }_k^i{a}_k^b\Delta t\\ {\alpha }_{k+1}^i& ={\alpha }_k^i+{\beta }_k^i\Delta t+\frac{1}{2}{\gamma }_k^i{a}_k^b\Delta t^2\\ b_{k+1}^a& =b_k^a+n_k^{ba}\Delta t\\ b_{k+1}^g& =b_k^g+n_k^{bg}\Delta t\end{array}$$

误差卡尔曼运动方程

将真值(true)状态变量写成名义(nominal)“加”误差(error)状态变量的形式，即:$x=\hat{x}\oplus \delta x$，需要推导误差项的迭代方程：
$$\begin{array}{rl}{x}_{k+1}={\hat{x}}_{k+1}+\delta x_{k+1}& =f({\hat{x}}_k+\delta x_k,{\hat{u}}_k+\delta u_k)\\ & =f({\hat{x}}_k,{\hat{u}}_k)+{\frac{\partial f}{\partial x_k}|}_{{\hat{x}}_k,{\hat{u}}_k}\cdot \delta x_k+{\frac{\partial f}{\partial u_k}|}_{{\hat{x}}_k,{\hat{u}}_k}\cdot \delta u_k\\ & =f({\hat{x}}_k,{\hat{u}}_k)+{\frac{\partial f}{\partial x_k}|}_{{\hat{x}}_k,{\hat{u}}_k}\cdot \delta x_k+{\frac{\partial f}{\partial u_k}|}_{{\hat{x}}_k,{\hat{u}}_k}\cdot \frac{\partial \delta u_k}{\partial n_k}\cdot n_k\\ & =f({\hat{x}}_k,{\hat{u}}_k)+{\frac{\partial f}{\partial x_k}|}_{{\hat{x}}_k,{\hat{u}}_k}\cdot \delta x_k+{\frac{\partial f}{\partial n_k}|}_{{\hat{x}}_k,{\hat{u}}_k}\cdot n_k\\ 误差递推：\delta x_{k+1}& =F_k\cdot \delta x_k+G_k\cdot n_k\\ 协方差递推：P_{k+1}& =F_k{P}_k{F}_k^T+G_{k}Q{G}_k^T\end{array}$$
这里运动方程$f()$采用中值法，采样扰动求导的方法。

$k$时刻真值状态变量 $x_k$	$k$时刻名义状态变量	$k$时刻误差状态变量	$k$时刻相关噪声项
$x_k=\left[\begin{array}{c}{\alpha }_k^i\\ {\theta }_k^i\\ {\beta }_k^i\\ b_k^a\\ b_k^g\end{array}\right]\in {\mathbb{R}}^{15}$	${\hat{x}}_k=\left[\begin{array}{c}{\hat{\alpha }}_k^i\\ {\hat{\theta }}_k^i\\ {\hat{\beta }}_k^i\\ {\hat{b}}_k^a\\ {\hat{b}}_k^g\end{array}\right]\in {\mathbb{R}}^{15}$	$\delta x_k=\left[\begin{array}{c}\delta {\alpha }_k^i\\ \delta {\theta }_k^i\\ \delta {\beta }_k^i\\ \delta b_k^a\\ \delta b_k^g\end{array}\right]\in {\mathbb{R}}^{15}$	$n_k=\left[\begin{array}{c}{n}_k^a\\ n_k^g\\ n_{k+1}^a\\ n_{k+1}^a\\ n_k^{ba}\\ n_k^{bg}\end{array}\right]\in {\mathbb{R}}^{18}$

• 雅可比矩阵 $F_k\in {\mathbb{R}}^{15\times 15},G_k\in {\mathbb{R}}^{15\times 18}$
• 协方差矩阵 $P_k\in {\mathbb{R}}^{15\times 15},Q\in {\mathbb{R}}^{18\times 18}$
• ${\theta }_k^i\in {\mathbb{R}}^3{\gamma }_k^i=Exp({\theta }_k^i)\in SO(3)$

雅可比计算可采用如下方式：
$$\begin{array}{r}{\frac{\partial x_{k+1}}{\partial x_k}|}_{{\hat{x}}_k}=\underset{\delta x_k\to 0}{\lim }\frac{f({\hat{x}}_k+\delta x_k)-f({\hat{x}}_k)}{\partial \delta x_k}\end{array}$$
将$f({\hat{x}}_k+\delta x_k)$可以整理为$f({\hat{x}}_k)+\dot{f}({\hat{x}}_k)\cdot \delta x_k$的形式，可得到雅可比：$\dot{f}({\hat{x}}_k)$

真值状态变量：
$$\begin{array}{rl}{b}_{k+1}^a& =b_k^a+n_k^{ba}\Delta t\\ b_{k+1}^g& =b_k^g+n_k^{bg}\Delta t\\ {\omega }_k^b& =\frac{1}{2}\left[({\tilde{\omega }}_k^b-b_k^g+n_k^g)+({\tilde{\omega }}_{k+1}^b-b_{k+1}^g+n_{k+1}^g)\right]\\ {\theta }_{k+1}^i& ={\theta }_k^i+{\omega }_k^b\Delta t\\ {\gamma }_{k+1}^i& ={\gamma }_k^{i}Exp({\omega }_k^b\Delta t)\\ a_k^b& =\frac{1}{2}\left[({\tilde{a}}_k^b-b_k^a+n_k^a)+Exp({\omega }_k^b\Delta t)({\tilde{a}}_{k+1}^b-b_{k+1}^a+n_{k+1}^a)\right]\\ {\beta }_{k+1}^i& ={\beta }_k^i+{\gamma }_k^i{a}_k^b\Delta t\\ {\alpha }_{k+1}^i& ={\alpha }_k^i+{\beta }_k^i\Delta t+\frac{1}{2}{\gamma }_k^i{a}_k^b\Delta t^2\end{array}$$
名义状态变量：

将原始的Imu数据中值法进行积分。
$$\begin{array}{rl}{\hat{b}}_{k+1}^a& ={\hat{b}}_k^a\\ {\hat{b}}_{k+1}^g& ={\hat{b}}_k^g\\ {\hat{\omega }}_k^b& =\frac{1}{2}\left[({\tilde{\omega }}_k^b-{\hat{b}}_k^g)+({\tilde{\omega }}_{k+1}^b-{\hat{b}}_k^g)\right]\\ {\hat{\theta }}_{k+1}^i& ={\hat{\theta }}_k^i+{\hat{\omega }}_k^b\Delta t\\ {\hat{\gamma }}_{k+1}^i& ={\hat{\gamma }}_k^{i}Exp({\hat{\omega }}_k^b\Delta t)\\ {\hat{a}}_k^b& =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a)+Exp({\hat{\omega }}_k^b\Delta t)({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right]\\ {\hat{\beta }}_{k+1}^i& ={\hat{\beta }}_k^i+{\hat{\gamma }}_k^i{\hat{a}}_k^b\Delta t\\ {\hat{\alpha }}_{k+1}^i& ={\hat{\alpha }}_k^i+{\hat{\beta }}_k^i\Delta t+\frac{1}{2}{\hat{\gamma }}_k^i{\hat{a}}_k^b\Delta t^2\end{array}$$
其中隐含的的等价:

• 陀螺仪噪声不可观，${\hat{n}}_k^g={\hat{n}}_{k+1}^g=0$，偏置噪声不可观，${\hat{n}}_k^{bg}=0$
  $$\begin{array}{rl}{\hat{b}}_{k+1}^g& ={\hat{b}}_k^g+{\hat{n}}_k^{bg}\Delta t\\ & ={\hat{b}}_k^g\\ {\hat{\omega }}_k^b& =\frac{1}{2}\left[({\tilde{\omega }}_k^b-{\hat{b}}_k^g+{\hat{n}}_k^a)+({\tilde{\omega }}_{k+1}^b-{\hat{b}}_{k+1}^g+{\hat{n}}_{k+1}^a)\right]\\ & =\frac{1}{2}\left[({\tilde{\omega }}_k^b-{\hat{b}}_k^g)+({\tilde{\omega }}_{k+1}^b-{\hat{b}}_k^g)\right]\end{array}$$

• 加速计噪声不可观，${\hat{n}}_k^a={\hat{n}}_{k+1}^a=0$，偏置噪声不可观，${\hat{n}}_k^{ba}=0$
  $$\begin{array}{rl}{\hat{b}}_{k+1}^a& ={\hat{b}}_k^a+{\hat{n}}_k^{ba}\Delta t\\ & ={\hat{b}}_k^a\\ {\hat{a}}_k^b& =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a+{\hat{n}}_k^a)+Exp({\hat{\omega }}_k^b\Delta t)({\tilde{a}}_{k+1}^b-{\hat{b}}_{k+1}^a+{\hat{n}}_{k+1}^a)\right]\\ & =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a)+Exp({\hat{\omega }}_k^b\Delta t)({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right]\end{array}$$

预积分量的雅可比推导见：
链接: 对状态变量雅可比推导
链接: 对噪声雅可比推导

预积分结果分析

假设从$i$时刻到$j$时刻，对Imu数据进行预积分，共迭代$K=j-i$次。

初始状态：
$$\begin{array}{rl}{x}_i& =\left[\begin{array}{c}{\alpha }_i^i\\ {\theta }_i^i\\ {\beta }_i^i\\ b_i^a\\ b_i^g\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\\ b_i^a\\ b_i^g\end{array}\right]\delta x_i=\left[\begin{array}{c}\delta {\alpha }_i^i\\ \delta {\theta }_i^i\\ \delta {\beta }_i^i\\ \delta b_i^a\\ \delta b_i^g\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\end{array}\right]\\ & \\ P_i& =Cov(\delta x_i)\\ Q& =Cov(n)=diag\left[\frac{1}{\Delta t}{\sigma }_a^2,\frac{1}{\Delta t}{\sigma }_g^2,\frac{1}{\Delta t}{\sigma }_g^2,\Delta t{\sigma }_{ba}^2,\Delta t{\sigma }_{bg}^2\right]\end{array}$$
最终状态：
$$\begin{array}{rl}{\hat{x}}_j& =\left[\begin{array}{c}{\hat{\alpha }}_j^i\\ {\hat{\theta }}_j^i\\ {\hat{\beta }}_j^i\\ {\hat{b}}_j^a\\ {\hat{b}}_j^g\end{array}\right]\delta x_j=\left[\begin{array}{c}\delta {\alpha }_j^i\\ \delta {\theta }_j^i\\ \delta {\beta }_j^i\\ \delta b_j^a\\ \delta b_j^g\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\end{array}\right]\\ & \\ J& =\frac{\partial x_K}{\partial x_0}=\prod _{k=K}^1{F}_k\\ G& =\prod _{k=K}^1{G}_k\\ P_j& =Cov(\delta x_j)=J{P}_i{J}^T+GQ{G}^T\end{array}$$
以上迭代都是假设$i$时刻的偏置不变，当偏置更新时，假定预积分观测时随零偏线性变化的。当$i$时刻的偏置有$\delta b_i^a,\delta b_i^g$变化时，预积分修正如下：
$$\begin{array}{rl}{\alpha }_j^i& ={\hat{\alpha }}_j^i+J_{ba}^{\alpha }\delta b_i^a+J_{bg}^{\alpha }\delta b_i^g\\ {\beta }_j^i& ={\hat{\beta }}_j^i+J_{ba}^{\beta }\delta b_i^a+J_{bg}^{\beta }\delta b_i^g\\ {\gamma }_j^i& ={\hat{\gamma }}_j^{i}Exp\left(J_{bg}^{\gamma }\delta b_i^g\right)\end{array}$$
其中：
$$\begin{array}{rl}& J_{ba}^{\alpha }=\frac{\partial {\alpha }_j^i}{\partial b_i^a}{J}_{bg}^{\alpha }=\frac{\partial {\alpha }_j^i}{\partial b_i^g}\\ & J_{ba}^{\beta }=\frac{\partial {\beta }_j^i}{\partial b_i^a}{J}_{bg}^{\beta }=\frac{\partial {\beta }_j^i}{\partial b_i^g}\\ & J_{bg}^{\gamma }=\frac{\partial {\gamma }_j^i}{\partial b_i^g}\end{array}$$