文章目录

• 0. 前言
• 1. 离散时间的IMU运动学方程
• 2. 状态变量定义
• 3. 补充公式
• 4. IMU误差状态空间方程推导
• • 4.1. 旋转误差 δr^i+1\delta\hat{\mathbf{r}}_{i+1}δr^i+1​
  • 4.2. 速度误差 δv^i+1\delta\hat{\mathbf{v}}_{i+1}δv^i+1​
  • 4.3. 平移误差 δpi+1\delta \mathbf{p}_{i+1}δpi+1​
  • 4.4. 角速度零偏误差 δb^wi+1\delta \hat{b}_{w_{i+1}}δb^wi+1​​
  • 4.5. 加速度零偏误差 δb^ai+1\delta \hat{b}_{a_{i+1}}δb^ai+1​​
  • 4.6. 整理成矩阵状态空间方程的形式

0. 前言

本文推导的是 IMU 积分的误差状态空间方程，预积分的推导要比积分复杂，但是推到方法和本文是一样的，因此这里只给出积分的推导方式。

另外，本文的推导方式参考了 R2Live Supplementary Material 中给出的推导，这种方法是基于李群李代数使用 全增量 的方法计算误差状态空间方程，非常好理解。

1. 离散时间的IMU运动学方程

{pi+1=pi+viΔt+12[Ri(ami−bai−nai)−g]Δt2vi+1=vi+[Ri(ami−bai−nai)−g]ΔtRi+1=Riexp⁡[(ωmi−bωi−nωi)Δt]bωi+1=bωi+nbωiΔtbai+1=bai+nbaiΔt\left\{\begin{array}{l} \mathbf{p}_{i+1}=\mathbf{p}_i+\mathbf{v}_i \Delta t+\frac{1}{2}\left[\mathbf{R}_i\left(\mathbf{a}_{m i}-\mathbf{b}_{a_i}-\mathbf{n}_{a i}\right)-\mathbf{g}\right] \Delta t^2 \\ \mathbf{v}_{i+1}=\mathbf{v}_i+\left[\mathbf{R}_i\left(\mathbf{a}_{m i}-\mathbf{b}_{a i}-\mathbf{n}_{a i}\right)-\mathbf{g}\right] \Delta t \\ \mathbf{R}_{i+1}=\mathbf{R}_i \exp \left[\left(\mathbf{\omega}_{mi}-\mathbf{b}_{\omega i}-\mathbf{n}_{\omega i}\right) \Delta t\right] \\ \mathbf{b}_{\omega i+1}=\mathbf{b}_{\omega i}+\mathbf{n}_{b_\omega i} \Delta t \\ \mathbf{b}_{a_i+1}=\mathbf{b}_{a i}+\mathbf{n}_{b_a i} \Delta t \end{array}\right. ⎩⎨⎧​pi+1​=pi​+vi​Δt+21​[Ri​(ami​−bai​​−nai​)−g]Δt2vi+1​=vi​+[Ri​(ami​−bai​−nai​)−g]ΔtRi+1​=Ri​exp[(ωmi​−bωi​−nωi​)Δt]bωi+1​=bωi​+nbω​i​Δtbai​+1​=bai​+nba​i​Δt​

• 式中忽略了参考坐标系，一般可以选择 worldworldworld 系 W{W}W，此时 g=[0,0,9.8]Tg=[0, 0, 9.8]^Tg=[0,0,9.8]T；
• nai,nωi\mathbf{n}_{a i},\mathbf{n}_{\omega i}nai​,nωi​ 为IMU读数的高斯白噪声；
• nbai,nbωi\mathbf{n}_{b_a i},\mathbf{n}_{b_\omega i}nba​i​,nbω​i​ 为IMU零偏的随机游走的高斯分布。

2. 状态变量定义

• 名义状态 x^\hat{\mathbf{x}}x^：估计出来的状态，真正能算出来的状态；
• 真实状态 x\mathbf{x}x：无法计算出来的状态，永远不知道的状态，只能用公式表示出来；
• 误差状态 δx\mathbf{\delta x}δx：真实状态 x\mathbf{x}x 与 名义状态 x^\hat{\mathbf{x}}x^ 的差值，关系式为：x=x^+δx\mathbf{x} = \hat{\mathbf{x}} + \mathbf{\delta x}x=x^+δx，即 真实状态 = 名义状态 + 误差状态。

比如：

• 角速度 ω\mathbf{\omega}ω 的名义状态和真实状态：
  {名义值：ωi^=ωmi−b^ωi真实值：ωi=ωi^−δb^ωi−nωi=ωmi−b^ωi−δb^ωi−nωi\left\{\begin{array}{l} 名义值：\hat{\mathbf{\omega}_i} = \mathbf{\omega}_{m i} - \hat{\mathbf{b}}_{\omega i} \\ 真实值：\mathbf{\omega}_i = \hat{\mathbf{\omega}_i} - \delta\hat{\mathbf{b}}_{\omega i} - \mathbf{n}_{\omega i} = \mathbf{\omega}_{m i} - \hat{\mathbf{b}}_{\omega i} - \delta\hat{\mathbf{b}}_{\omega i} - \mathbf{n}_{\omega i} \end{array}\right. {名义值：ωi​^​=ωmi​−b^ωi​真实值：ωi​=ωi​^​−δb^ωi​−nωi​=ωmi​−b^ωi​−δb^ωi​−nωi​​

• 加速度 a\mathbf{a}a 的名义状态和真实状态：
  {名义值：ai^=ami−b^ai真实值：ai=ai^−δb^ai−nai=ami−b^ai−δb^ai−nai\left\{\begin{array}{l} 名义值：\hat{\mathbf{a}_i} = \mathbf{a}_{m i} - \hat{\mathbf{b}}_{a i} \\ 真实值：\mathbf{a}_i = \hat{\mathbf{a}_i} - \delta\hat{\mathbf{b}}_{a i} - \mathbf{n}_{a i} = \mathbf{a}_{m i} - \hat{\mathbf{b}}_{a i} - \delta\hat{\mathbf{b}}_{a i} - \mathbf{n}_{a i} \end{array}\right. {名义值：ai​^​=ami​−b^ai​真实值：ai​=ai​^​−δb^ai​−nai​=ami​−b^ai​−δb^ai​−nai​​

3. 补充公式

• 李代数上的增量和李群上的扰动之间的关系：
  exp⁡(ϕ+Δϕ)=exp⁡(JlΔϕ)⋅exp⁡(ϕ)=exp⁡(ϕ)⋅exp⁡(JrΔϕ)\begin{align} \exp (\phi+\Delta \phi) =\exp (J_l \Delta \phi) \cdot \exp (\phi) =\exp (\phi) \cdot \exp (J_r \Delta \phi)\end{align} exp(ϕ+Δϕ)=exp(Jl​Δϕ)⋅exp(ϕ)=exp(ϕ)⋅exp(Jr​Δϕ)​​

• SO(3)SO(3)SO(3)的伴随性质：
  R⊤exp⁡(ϕ)R=exp⁡(R⊤ϕ)\begin{align}R^{\top} \exp (\phi) R=\exp \left(R^{\top} \phi\right)\end{align} R⊤exp(ϕ)R=exp(R⊤ϕ)​​

• BCH近似公式：
  log⁡[exp⁡(ϕ1)exp⁡(ϕ2)]={Jl⁡(ϕ2)−1ϕ1+ϕ2,ϕ1≈0Jr⁡(ϕ1)−1ϕ2+ϕ1,ϕ2≈0\begin{align} \log \left[\exp \left(\phi_1\right) \exp \left(\phi_2\right)\right]=\left\{\begin{array}{l}\operatorname{J_l}\left(\phi_2\right)^{-1} \phi_1+\phi_2, \quad \phi_1 \approx 0 \\ \operatorname{J_r}\left(\phi_1\right)^{-1} \phi_2+\phi_1, \quad \phi_2 \approx 0 \end{array}\right.\end{align} log[exp(ϕ1​)exp(ϕ2​)]={Jl​(ϕ2​)−1ϕ1​+ϕ2​,ϕ1​≈0Jr​(ϕ1​)−1ϕ2​+ϕ1​,ϕ2​≈0​​​

4. IMU误差状态空间方程推导

4.1. 旋转误差 δr^i+1\delta\hat{\mathbf{r}}_{i+1}δr^i+1​

定义：δr^i+1=log⁡[R^i+1⊤Ritt]名义值：R^i+1=R^iexp⁡(w^iΔt)真实值：Ri+1=R^iexp⁡(δr^i)⋅exp⁡(ωiΔt)\begin{aligned} \text { 定义：} \delta \hat{r}_{i+1} & =\log \left[\hat{R}_{i+1}^{\top} R_{i t t}\right] \\ \text {名义值：} \hat{R}_{i+1} & =\hat{R}_i \exp \left(\hat{w}_i \Delta t\right) \\ \text {真实值：} R_{i+1} & =\hat{R}_i \exp \left(\delta \hat{r}_i\right) \cdot \exp \left(\omega_i \Delta t\right) \end{aligned}  定义：δr^i+1​名义值：R^i+1​真实值：Ri+1​​=log[R^i+1⊤​Ritt​]=R^i​exp(w^i​Δt)=R^i​exp(δr^i​)⋅exp(ωi​Δt)​

则旋转误差如下：

4.2. 速度误差 δv^i+1\delta\hat{\mathbf{v}}_{i+1}δv^i+1​

名义值：v^i+1=v^i+(R^ia^i−g)Δt真实值：vi+1=(v^i+δv^i)+[R^iexp⁡(δr^i)(a^i−δb^ai−nai)−g]Δt\begin{aligned} 名义值： \hat{v}_{i+1}&=\hat{v}_i+\left(\hat{R}_i \hat{a}_i-g\right) \Delta t \\ 真实值：v_{i+1} &= \left(\hat{v}_i+\delta \hat{v}_i \right) + \left[\hat{R}_i \exp \left(\delta \hat{r}_i\right)\left(\hat{a}_i-\delta \hat{b}_{a i}-n_{a i}\right)-g\right] \Delta t \end{aligned} 名义值：v^i+1​真实值：vi+1​​=v^i​+(R^i​a^i​−g)Δt=(v^i​+δv^i​)+[R^i​exp(δr^i​)(a^i​−δb^ai​−nai​)−g]Δt​

4.3. 平移误差 δpi+1\delta \mathbf{p}_{i+1}δpi+1​

名义值： p^i+1=pi^+v^iΔt+12(Ri^ai^−g)Δt2真实值： pi+1=(p^i+δp^i)+(v^i+δv^i)Δt+12[R^i⋅exp⁡(δr^i)(a^i−δb^ai−nai)−g]⋅Δt2\begin{aligned} & \text { 名义值： } \hat{p}_{i+1}=\hat{p_i}+\hat{v}_i \Delta t+\frac{1}{2}\left(\hat{R_i} \hat{a_i}-g\right) \Delta t^2 \\ & \text { 真实值： } p_{i+1}=\left(\hat{p}_i+\delta \hat{p}_i\right)+\left(\hat{v}_i+\delta \hat{v}_i\right) \Delta t+\frac{1}{2}\left[\hat{R}_i \cdot \exp \left(\delta \hat{r}_i\right)\left(\hat{a}_i-\delta \hat{b}_{ai}-n_{ai}\right) -g\right] \cdot \Delta t^2 \\ & \end{aligned} ​ 名义值： p^​i+1​=pi​^​+v^i​Δt+21​(Ri​^​ai​^​−g)Δt2 真实值： pi+1​=(p^​i​+δp^​i​)+(v^i​+δv^i​)Δt+21​[R^i​⋅exp(δr^i​)(a^i​−δb^ai​−nai​)−g]⋅Δt2​

4.4. 角速度零偏误差 δb^wi+1\delta \hat{b}_{w_{i+1}}δb^wi+1​​

名义值：b^wi+1=b^wi真实值：bwi+1=b^wi+δbwi+Δt⋅nbwi误差：δbwi+1=δbwi+Δt⋅nbwi\begin{aligned} \text { 名义值：} \hat{b}_{w_{ i+1}} &=\hat{b}_{w i} \\ \text { 真实值：} b_{w_{i+1} } &=\hat{b}_{w_i}+\delta b_{w i}+\Delta t \cdot n_{b_{wi}} \\ \text { 误差：} \delta b_{w_{i+1} } &=\delta b_{w i}+\Delta t \cdot n_{b_{wi}} \\ \end{aligned}  名义值：b^wi+1​​ 真实值：bwi+1​​ 误差：δbwi+1​​​=b^wi​=b^wi​​+δbwi​+Δt⋅nbwi​​=δbwi​+Δt⋅nbwi​​​

4.5. 加速度零偏误差 δb^ai+1\delta \hat{b}_{a_{i+1}}δb^ai+1​​

名义值：b^ai+1=b^ai真实值：bai+1=b^ai+δbai+Δt⋅nbai误差：δbai+1=δbai+Δt⋅nbai\begin{aligned} \text { 名义值：} \hat{b}_{a_{ i+1}} &=\hat{b}_{a i} \\ \text { 真实值：} b_{a_{i+1} } &=\hat{b}_{a_i}+\delta b_{a i}+\Delta t \cdot n_{b_{ai}} \\ \text { 误差：} \delta b_{a_{i+1} } &=\delta b_{a i}+\Delta t \cdot n_{b_{ai}} \\ \end{aligned}  名义值：b^ai+1​​ 真实值：bai+1​​ 误差：δbai+1​​​=b^ai​=b^ai​​+δbai​+Δt⋅nbai​​=δbai​+Δt⋅nbai​​​

4.6. 整理成矩阵状态空间方程的形式

定义IMU运动学的矩阵状态空间方程形式为

δx^i+1=Fx⋅δx^i+Fw⋅wi\delta \hat{\mathbf{x}}_{i+1}=\mathbf{F}_x \cdot \delta \hat{\mathbf{x}}_i + \mathbf{F}_w \cdot \mathbf{w}_i δx^i+1​=Fx​⋅δx^i​+Fw​⋅wi​

其中状态变量的定义为：

δx^i=[δr^i,δp^i,δv^i,δb^wi,δb^ai]⊤wi=[δnwi,δnai,δnbwi,δnbai]⊤\begin{aligned} \delta \hat{\mathbf{x}}_i &= \left[\delta \hat{\mathbf{r}}_i, ~~\delta \hat{\mathbf{p}}_i, ~~\delta \hat{\mathbf{v}}_i, ~~\delta \hat{\mathbf{b}}_{wi}, ~~\delta \hat{\mathbf{b}}_{ai}\right]^{\top} \\ \mathbf{w}_i &= \left[\delta\mathbf{n}_{wi}, ~~\delta\mathbf{n}_{ai}, ~~\delta\mathbf{n}_{b_{wi}}, ~~ \delta\mathbf{n}_{b_{ai}}\right]^{\top} \end{aligned} δx^i​wi​​=[δr^i​,  δp^​i​,  δv^i​,  δb^wi​,  δb^ai​]⊤=[δnwi​,  δnai​,  δnbwi​​,  δnbai​​]⊤​

则系数矩阵为：

Fx=[exp⁡(−ωiΔt^)00−IΔt00IIΔt00−R^i(a^i)×Δt0I0−R^iΔt000I000000]Fω=[−IΔt00000000−R^iΔt0000IΔt0000IΔt]\begin{aligned} \mathbf{F}_x &= \left[\begin{array}{ccccc} \exp \left(-\hat{\omega_i \Delta t}\right) & 0 & 0 & -I \Delta t & 0 \\ 0 & I & I \Delta t & 0 & 0\\ -\hat{R}_i\left(\hat{a}_i\right)_{\times} \Delta t & 0 & I & 0 & -\hat{R}_i \Delta t \\ 0 & 0 & 0 & I & 0 \\ 0 & 0 & 0 & 0 & 0 \end{array}\right] \\\\ F_\omega &=\left[\begin{array}{cccc} -I \Delta t & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & -\hat{R}_i \Delta t & 0 & 0 \\ 0 & 0 & I \Delta t & 0 \\ 0 & 0 & 0 & I \Delta t \end{array}\right] \end{aligned} Fx​Fω​​=​exp(−ωi​Δt^​)0−R^i​(a^i​)×​Δt00​0I000​0IΔtI00​−IΔt00I0​00−R^i​Δt00​​=​−IΔt0000​00−R^i​Δt00​000IΔt0​0000IΔt​​​

协方差矩阵的传播公式为：

Pi+1=FxPiFx⊤+FωQFω⊤\mathbf{P}_{i+1}=\mathbf{F}_x \mathbf{P}_i \mathbf{F}_x{ }^{\top}+\mathbf{F}_\omega \mathbf{Q} \mathbf{F}_\omega{ }^{\top} Pi+1​=Fx​Pi​Fx​⊤+Fω​QFω​⊤

其中 Q\mathbf{Q}Q 为测量噪声的协方差矩阵。