精华内容
下载资源
问答
  • Maple对矩阵元素积分

    2018-03-10 19:05:30
    在有限元刚度阵推导过程中,会遇到对矩阵进行积分的操作。 我们有两种方法来解决这个问题: 1. 通过map函数,把int操作映射到矩阵的各个元素上。具体操作如下: 2. 调用MTM软件包,就可以直接利用int命令对矩阵...

     

    在有限元刚度阵推导过程中,会遇到对矩阵进行积分的操作。

    我们有两种方法来解决这个问题:

    1. 通过map函数,把int操作映射到矩阵的各个元素上。具体操作如下:

    2. 调用MTM软件包,就可以直接利用int命令对矩阵进行积分。具体操作如下:

    MTM_int

     

    展开全文
  • 我在做python三重积分的时候,遇到如下问题, ``` from sympy import * from scipy import integrate from sympy.abc import x,y,z import numpy as np kk=[x+y,x*y] kan=Matrix(np.reshape(kk,(2,1))) f=...
  • 由复数矩阵元表达的失调光学系统衍射积分,研究了部分相干光交叉谱密度经过复杂光学系统的传输规律,并将之用于无腔型激光输出特性的研究。具体实验室等离子体X射线激光进行了分析,所得结果与已有实验符合较好。
  • 一.前言 本博客基本上借鉴了崔华坤的《VINS论文推导及代码解析》和 VINS-Mono理论学习... IMU Pre-integration介绍了IMU预积分模型,Foster的倆篇论文IMU预积分理论进行详细分析。 为什么需要IMU进行积分? 传...

    1. 前言

    本博客借鉴了崔华坤的《VINS论文推导及代码解析》和 VINS-Mono理论学习——IMU预积分 Pre-integration (Jacobian 协方差)的内容,因为确实写得太好了,然后有些地方加入自己一些理解。

    VINS-MONO论文中的IV-B. IMU Pre-integration介绍了IMU预积分模型,Foster的倆篇论文对IMU预积分理论进行详细分析。

    值得注意的是,本文使用了四元素中Hamilton(可参考《Quaternion kinematics for the error state Kalman filter》,右手系)的表示和JPL(可参考《Indirect Kalman Filter for 3D Attitude Estimation》,左手系)表示,因为Vins-mono里面也是用了俩种表示混用,俩者的具体区别可以看《Quaternion kinematics for the error state Kalman filter》的第17页,且论文中用到的JPL的右乘左乘定义还与《Indirect Kalman Filter for 3D Attitude Estimation》不同,这种问题估计让无数人心塞。/(ㄒoㄒ)/~~

    为什么需要对IMU进行预积分?
    传统捷联惯性导航算法,在已知 kk 时刻下的IMU状态量(姿态、速度和位移)情况下,利用IMU测量的线加速度和角速度,通过积分预算得到k+1k+1时刻下的状态量。

    然而在非线性优化的VIO中,各个节点的状态量都是估计值,当算法对这些状态量优化时,每次调整都需要在它们之间重新积分,导致绝对位姿被优化时对状态量进行重复积分。IMU预积分的提出使得优化算法可对IMU的相对测量进行处理,使它与绝对位姿解耦,或者只要线性运算就可以进行矫正

    2. IMU模型

    IMU测量值包括加速度计得到的 (测量值) 线加速度at^\hat{a_{t}} 和陀螺仪得到的角加速度w^t\hat{w}_{t} 【论文式(1)】
    a^t=at+bat+Rwtgw+na\hat{a}_{t}=a_{t}+b_{at}+R_{w}^{t}g^{w}+n_{a} w^t=wt+bwt+nw\hat{w}_{t}=w_{t}+b_{wt}+n_{w}其中tt下标表示在IMU的体(body)坐标系下,ata_{t}wtw_{t}分别表示IMU真实的线加速度和角速度,并受到加速度偏置(bias) batb_{at}、陀螺仪偏置bwtb_{wt}和附加噪声nan_annwn_{n_{w}}的影响。计算得到的线加速度at^\hat{a_{t}}是重力加速度和物体加速度的合矢量。
    假设附加噪声为高斯噪声:
    na(0,σa2), na(0,σw2)n_{a}\sim(0,\sigma_{a}^{2}), \ n_{a}\sim(0,\sigma_{w}^{2}) 加速度计偏置和陀螺仪偏置被建模为随机游走,其导数为高斯分布:【论文式(2)】
    b˙at=nba, b˙wt=nbw\dot{b}_{at}=n_{ba}, \ \dot{b}_{wt}=n_{bw} nbaN(0,σba2), nbwN(0,σbw2)n_{ba}\sim N(0,\sigma_{ba}^{2}), \ n_{bw} \sim N(0, \sigma_{bw}^{2})

    3. 基于世界坐标系下的IMU运动模型

    3.1 连续形式下的IMU运动模型

    对于图像帧kkk+1k+1, IMU body坐标系对应为bkb_{k}bk+1b_{k+1}位置、速度和姿态状态值PVQ(Pose、Velocity、Quaternion)可以根据[tk,tk+1][t_{k}, t_{k+1}]时间间隔内的IMU测量值,在世界坐标系下进行传递:【论文式(3)(4)】
    pbk+1w=pbkw+vbkwΔtk+t[tk,tk+1](Rtw(a^tbatna)gw)dt2p^{w}_{b_{k+1}}=p^{w}_{bk}+v_{b_{k}}^{w}\Delta t_{k}+\int \int_{t\in[t_{k}, t_{k+1}]}(R_{t}^{w}(\hat{a}_{t}-b_{at}-n_{a})-g^{w})dt^{2} vbk+1w=vbkw+t[tk,tk+1](Rtw(a^tbatna)gw)dtv_{b_{k+1}}^{w}=v_{bk}^{w}+\int_{t\in[t_{k},t_{k+1}]}(R^{w}_{t}(\hat{a}_{t}-b_{at}-n_{a})-g^{w})dt qbk+1w=qbkwt[tk,tk+1]12qtbk[(w^tbwtnw)0]dt=qbkt[tk,tk+1]12Ω(w^tbwtnw)qtbkdt(1)q_{b_{k+1}}^{w} = q_{b_{k}}^{w} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} q_{t}^{b_{k}} \otimes \begin{bmatrix} (\hat{w}_{t}-b_{wt}-n_{w})\\ 0 \end{bmatrix}dt \\ = q_{bk} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} \Omega (\hat{w}_{t}-b_{wt}-n_{w})q_{t}^{bk}dt \tag{1} Ω(w)=[[w]×wwT0], [w]×=[0wzwywz0wxwywx0]\Omega(w)=\begin{bmatrix} -[w]_{\times} & w\\ -w^{T}& 0 \end{bmatrix}, \ [w]_{\times} = \begin{bmatrix} 0 & -w_{z} & w_{y}\\ w_{z} & 0 & -w_{x}\\ -w_{y} & w_{x} & 0 \end{bmatrix}其中 Δtk\Delta t_{k}[tk,tk+1][t_{k}, t_{k+1}]之间的时间间隔,RtwR_{t}^{w}为t时刻IMU body坐标系到世界坐标系的旋转矩阵,qtbkq_{t}^{bk}为用四元素表示的 tt 时刻从IMU body坐标系到 kk 时刻IMU body坐标系的旋转,这里的四元素实部在后,虚部在前, 为了与论文保持一致,即四元素JPL表达。这里的Ω(w)\Omega(w)表示四元素右乘。


    关于公式(1)(1)的推导,这里首先引入四元素左乘右乘及导数定理:
    这里与《Indirect Kalman Filter for 3D Attitude Estimation》中公式(10)不同,主要是因为左右手系不同,我们引入左乘和右乘符号如下:
    qaqb=R(qb)qa=[sbzbybxbzbsbxbybybxbsbzbxbybzbsb][xayazasa]=L(qa)qb=[sazayaxazasaxayayaxasazaxayazasa][xbybzbsb]q_{a} \otimes q_{b} = R(q_{b})q_{a} = \begin{bmatrix} s_{b} & z_{b} & -y_{b} & x_{b}\\ -z_{b} & s_{b} & x_{b} & y_{b}\\ y_{b}& -x_{b} & s_{b} & z_{b}\\ -x_{b}& -y_{b} & -z_{b} & s_{b} \end{bmatrix}\begin{bmatrix} x_{a}\\ y_{a}\\ z_{a}\\ s_{a} \end{bmatrix} \\ = L(q_{a})q_{b} = \begin{bmatrix} s_{a} & -z_{a} & y_{a} & x_{a}\\ z_{a} & s_{a} & -x_{a} & y_{a}\\ -y_{a}& x_{a} & s_{a} & z_{a}\\ -x_{a}& -y_{a} & -z_{a} & s_{a} \end{bmatrix}\begin{bmatrix} x_{b}\\ y_{b}\\ z_{b}\\ s_{b} \end{bmatrix}
    为了简化,令q=[x y z s]=[w s]q=[x \ y \ z \ s] = [w \ s], 则有:
    R(q)=Ω(w)+sI4×4=[[w]×wwT0]+sI4×4R(q) = \Omega(w)+sI_{4\times4} = \begin{bmatrix} -[w]_{\times} & w\\ -w^{T}& 0 \end{bmatrix} + sI_{4\times4} L(q)=Ψ(w)+sI4×4=[[w]×wwT0]+sI4×4L(q) = \Psi(w)+sI_{4\times4} = \begin{bmatrix} [w]_{\times} & w\\ -w^{T}& 0 \end{bmatrix} + sI_{4\times4}
    对于四元素的求导,我们定义qtq_{t}tt时刻下的单位四元素(这里的四元素实部在后,虚部在前),wwqtq_{t}确定的角速度,则关于qtq_{t}的导数为: q˙t=12[[w]×wwT0]qt=12Ω(w)qt=12R([w0])qt=12qt[w0]\dot{q}_{t} = \frac{1}{2} \begin{bmatrix} -[w]_{\times} & w\\ -w^{T}& 0 \end{bmatrix}q_{t} = \frac{1}{2}\Omega(w)q_{t}=\frac{1}{2}R(\begin{bmatrix} w\\ 0 \end{bmatrix})q_{t} = \frac{1}{2}q_{t} \otimes \begin{bmatrix} w\\ 0 \end{bmatrix}

    当看到q˙=12qt[0w]\dot{q} = \frac{1}{2} q_{t} \otimes \begin{bmatrix} 0 \\ w \end{bmatrix} 说明该四元素实部在前,虚部在后(Hamilton表示),只是表示不同而已。

    因此对于IMU连续形式下的旋转状态(用四元素表示)推导,我们有:
    qbk+1w=qbkwqbk+1bk=qbkwt[tk,tk+1]qt˙bkdt=qbkwt[tk,tk+1]12qtbk[wtbk0]dt=qbkwt[tk,tk+1]12qtbk[wt^bwtnw0]dt==qbkt[tk,tk+1]12Ω(w^tbwtnw)qtbkdtq_{b_{k+1}^{w}}=q_{b_{k}}^{w} \otimes q_{b_{k+1}}^{b_{k}} = q_{b_{k}}^{w} \otimes \int _{t\in [t_{k}, t_{k+1}]} \dot{q_{t}}^{b_{k}} dt = q_{bk}^{w} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} q_{t}^{b_{k}}\otimes \begin{bmatrix} w_{t}^{b_{k}}\\ 0\end{bmatrix}dt \\ =q_{b_{k}}^{w} \otimes \int_{t \in [t_{k}, t_{k+1}]} \frac{1}{2}q_{t}^{b_{k}} \otimes \begin{bmatrix} \hat{w_{t}}-b_{wt}-n_{w}\\ 0\end{bmatrix} dt \\ = = q_{bk} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} \Omega (\hat{w}_{t}-b_{wt}-n_{w})q_{t}^{bk}dt


    3.2 离散形式下的IMU运动模型

    3.2.1 欧拉法离散形式

    使用欧拉法,即k+1k+1时刻的位姿是用第kk时刻的测量值a^bk\hat{a}_{b_{k}}, w^bk\hat{w}_{b_{k}}来计算的:
    pbk+1w=pbkw+vbkwΔtk+12a^bkδt2p^{w}_{b_{k+1}} = p_{b_{k}}^{w} + v_{b_{k}}^{w} \Delta t_{k} + \frac{1}{2}\hat{a}_{b_{k}}\delta t^{2} vbk+1w=vbkw+a^bkδtv_{b_{k+1}}^{w} = v_{b_{k}}^{w} + \hat{a}_{b_{k}} \delta t qbk+1w=qbkw[112w^bkδt]q^{w}_{b_{k+1}} = q^{w}_{b_{k}} \otimes \begin{bmatrix} 1\\ \frac{1}{2}\hat{w}_{b_{k}}\delta t \end{bmatrix} 其中 a^bk=qbkw(abkbak)gw\hat{a}_{b_{k}} = q_{b_{k}}^{w}(a_{b_{k}}-b_{ak})-g^{w}w^bk=wbkbwk\hat{w}_{b_{k}} = w_{b_{k}}-b_{wk}

    3.2.2 中值法离散形式

    使用中值法,即k+1k+1时刻的位姿是用俩个时刻kkk+1k+1测量值aa, ww的平均值来计算的:
    pbk+1w=pbkw+vbkwΔtk+12a^ˉtδt2p^{w}_{b_{k+1}} = p_{b_{k}}^{w} + v_{b_{k}}^{w} \Delta t_{k} + \frac{1}{2}\bar{\hat{a}}_{t}\delta t^{2} vbk+1w=vbkw+a^ˉtδtv_{b_{k+1}}^{w} = v_{b_{k}}^{w} + \bar{\hat{a}}_{t} \delta t qbk+1w=qbkw[112w^ˉtδt]q^{w}_{b_{k+1}} = q^{w}_{b_{k}} \otimes \begin{bmatrix} 1\\ \frac{1}{2}\bar{\hat{w}}_{t}\delta t \end{bmatrix} 其中 a^ˉt=12[qbkw(abkbak)gw+qbk+1w(abk+1bak+1)gw]\bar{\hat{a}}_{t} = \frac{1}{2}[q_{b_{k}}^{w}(a_{b_{k}}-b_{ak})-g^{w} + q_{b_{k+1}}^{w}(a_{b_{k+1}}-b_{ak+1})-g^{w}]w^ˉt=12(wbkbwk+wbk+1bwk+1)=12(wbk+wbk+1)bwk\bar{\hat{w}}_{t} = \frac{1}{2}(w_{b_{k}}-b_{wk}+w_{b_{k+1}}-b_{wk+1}) \\ = \frac{1}{2}(w_{b_{k}}+w_{b_{k+1}})-b_{w{k}} 假设在短时间内加速度计和陀螺仪的偏置不变,则有:bak=bak+1, bwk=bwk+1b_{ak}=b_{ak+1}, \ b_{wk} = b_{wk+1}

    4.IMU预积分 (基于第K帧IMU body坐标系下的运动模型)

    通过公式(1)(1)可以看到,IMU 的积分需要依赖与第 kk 帧的 vvRR (基于世界坐标系下的),当我们在后端进行非线性优化时,需要迭代更新第 kk 帧的 vvRR,这将导致我们需要根据每次迭代后的值重新进行积分,这将非常耗时。我们考虑将优化变量从第 kk 帧到第 k+1k+1 帧的 IMU 积分项中分离开来。

    4.1 连续形式下的IMU运动模型

    IMU预积分的思想就是将参考坐标系从世界坐标系ww调整为第kk帧的IMU body坐标系bkb_{k}下,可通过在等式俩端同时乘以RwbkR^{b_{k}}_{w}得到:【论文式[5][6]】
    Rwbkpbk+1w=Rwbk(pbkw+vbkwΔtk12gwΔtk2)+αbk+1bkR^{b_{k}}_{w}p^{w}_{b_{k+1}}=R^{b_{k}}_{w}(p_{b_{k}}^{w}+v^{w}_{b_{k}}\Delta t_{k}-\frac{1}{2}g^{w}\Delta t_{k}^{2}) + \alpha^{b_{k}}_{b_{k+1}} Rwbkvbk+1w=Rwbk(vbkwgwΔtk)+βbk+1bkR^{b_{k}}_{w}v_{b_{k+1}}^{w} = R_{w}^{b_{k}}(v_{b_{k}}^{w}-g^{w}\Delta t_{k})+\beta^{b_{k}}_{b_{k+1}} qwbkqbk+1w=γbk+1bk(2)q_{w}^{b_{k}} \otimes q_{b_{k+1}}^{w} = \gamma _{b_{k+1}} ^{b_{k}} \tag{2}
    其中 αbk+1bk=t[tk,tk+1]Rtbk(a^tbatna)dt2\alpha^{b_{k}}_{b_{k+1}} = \int \int _{t\in [t_{k}, t_{k+1}]} R_{t}^{b_{k}} (\hat{a}_{t}-b_{at}-n_{a}) dt^{2} βbk+1bk=t[tk,tk+1]Rtbk(a^tbatna)dt\beta_{b_{k+1}}^{b_{k}} = \int _{t\in [t_{k}, t_{k+1}]}R_{t}^{b_{k}}(\hat{a}_{t}-b_{at}-n_{a})dt γbk+1=t[tk,tk+1]12Ω(w^tbwtnw)γtbkdt\gamma_{b_{k+1}} = \int _{t \in [t_{k}, t_{k+1}]} \frac{1}{2} \Omega(\hat{w}_{t}-b_{wt}-n_{w}) \gamma_{t}^{b_{k}}dt

    此时的积分结果αbk+1bk\alpha^{b_{k}}_{b_{k+1}}βbk+1bk\beta^{b_{k}}_{b_{k+1}}γbk+1bk\gamma^{b_{k}}_{b_{k+1}}可以理解为bk+1b_{k+1}bkb_{k}的相对运动量,bkb_{k}的状态并不会对其产生影响,因此将其作为非线性优化变量,可以避免状态的重复传递

    注意,这是在假设IMU偏置bab_{a}bwb_{w}已经确定的情况下,实际上偏置也是需要优化的变量,那么每次迭代时,bab_{a}bwb_{w}发生改变,得重新根据公式求得所有帧之间的IMU预积分。

    当偏置变换很小时,可以将αbk+1bk\alpha_{b_{k+1}}^{b_{k}}βbk+1bk\beta_{b_{k+1}}^{b_{k}}γbk+1bk\gamma_{b_{k+1}}^{b_{k}}按其偏置的一阶近似来调整,否则就进行重新传递。【论文式[12]】(这部分只是抛出一个概念,后面会讲为什么这样写)
    αbk+1bkα^bk+1bk+Jbaαδba+Jbwαδbw\alpha_{b_{k+1}}^{b_{k}} \approx \hat{\alpha}_{b_{k+1}}^{b_{k}} + J_{b_{a}}^{ \alpha} \delta b_{a} + J_{b_{w}}^{ \alpha} \delta b_{w} βbk+1bkβ^bk+1bk+Jbaβδba+Jbwβδbw\beta_{b_{k+1}}^{b_{k}} \approx \hat{\beta}_{b_{k+1}}^{b_{k}} + J_{b_{a}}^{ \beta} \delta b_{a} + J_{b_{w}}^{ \beta} \delta b_{w} γbk+1bkγ^bk+1bk[112Jbwγδbw]\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \begin{bmatrix} 1 \\ \frac{1}{2}J_{b_{w}}^{\gamma}\delta b_{w} \end{bmatrix}

    4.2 离散形式下的IMU运动模型

    4.2.1 两帧之间 PVQ 增量的欧拉法离散形式

    欧拉法给出第i时刻与第i+1时刻的预积分量估计值的关系: 【论文式[7]】
    α^i+1bk=α^ibk+β^ibkδt+12R(γ^ibk)(a^ibai)δt2\hat{\alpha}^{b_{k}}_{i+1} = \hat{\alpha}^{b_{k}}_{i} + \hat{\beta}_{i}^{b_{k}}\delta t + \frac{1}{2} R(\hat{\gamma}_{i}^{b_{k}})(\hat{a}_{i}-b_{ai})\delta t^{2} β^i+1bk=β^ibk+12R(γ^ibk)(a^ibai)δt\hat{\beta}^{b_{k}}_{i+1} = \hat{\beta}^{b_{k}}_{i} + \frac{1}{2} R(\hat{\gamma}_{i}^{b_{k}})(\hat{a}_{i}-b_{ai})\delta t γ^i+1bk=γ^ibkγ^i+1i=γ^ibk[112(w^ibwi)δt]\hat{\gamma}^{b_{k}}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \hat{\gamma}^{i}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \begin{bmatrix} 1\\ \frac{1}{2}(\hat{w}_{i}-b_{wi})\delta t \end{bmatrix}
    其中ii[tk,tk+1][t_{k}, t_{k+1}]之间的离散时间

    4.2.2 两帧之间 PVQ 增量的中值法离散形式

    代码中采用的基于中值法的 IMU 预积分公式,这在Estimator::processIMU()函数中的IntegrationBase::push_back()函数中得以实现,注意这里积分出来的是前后两帧之间的 IMU 增量信息。
    α^i+1bk=α^ibk+β^ibkδt+12a^ˉiδt2\hat{\alpha}^{b_{k}}_{i+1} = \hat{\alpha}^{b_{k}}_{i} + \hat{\beta}_{i}^{b_{k}}\delta t+ \frac{1}{2} \bar{\hat{a}}_{i}\delta t^{2} β^i+1bk=β^ibk+a^ˉiδt\hat{\beta}^{b_{k}}_{i+1} = \hat{\beta}^{b_{k}}_{i} + \bar{\hat{a}}_{i}\delta t γ^i+1bk=γ^ibkγ^i+1i=γ^ibk[112w^ˉiδt]\hat{\gamma}^{b_{k}}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \hat{\gamma}^{i}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \begin{bmatrix} 1\\ \frac{1}{2} \bar{\hat{w}}_{i}\delta t \end{bmatrix} 其中 a^ˉt=12[γ^ibk(a^ibi)+γ^i+1bk(a^i+1bai)]\bar{\hat{a}}_{t} = \frac{1}{2}[\hat{\gamma}^{b_{k}}_{i}(\hat{a}_{i}-b_{i}) + \hat{\gamma}^{b_{k}}_{i+1}(\hat{a}_{i+1}-b_{ai})] w^ˉi=12(w^i+w^i+1)bwi\bar{\hat{w}}_{i} = \frac{1}{2}(\hat w_{i} + \hat w_{i+1})-b_{wi}
    初始状态下αbkbk\alpha_{b_{k}}^{b_{k}}βbkbk\beta_{b_{k}}^{b_{k}}为0, γbkbk\gamma_{b_{k}}^{b_{k}}为单位四元素,nan_{a}nwn_{w}被视为0,ii为在[k,k+1][k, k+1]中IMU测量值的某一时刻,δt\delta t为IMU测量值iii+1i+1之间的时间间隔。

    5. 非线性方程的误差递推方程

    5.1 一段时间内多个IMU数据积分形成的预积分量的协方差计算

    一个 IMU 数据作为测量值的噪声方差我们能够标定。现在,一段时间内多个 IMU 数据积分形成的预积分量的方差呢?

    要推导预积分量的协方差,需要知道IMU噪声和预积分量之间的线性递推关系。

    假设已知了相邻时刻误差的线性传递方程:
    ηik+1=Fkηik+Gknk\eta_{ik+1} = F_{k}\eta_{ik} + G_{k}n_{k}其中ηik\eta_{ik}为状态量误差且ηik=[δθik,δvik,δpik]\eta_{ik}=[\delta\theta_{ik}, \delta v_{ik}, \delta p_{ik}]nkn_{k}为测量噪声且nk=[nkg,nkg]n_{k}=[n_{k}^{g}, n_{k}^{g}]
    可以看出误差的传递由倆部分组成:当前时刻的误差传递给下一时刻,当前时刻测量噪声传递给下一时刻

    5.2 非线性方程的误差递推方程推导

    通常对于状态量之间的递推关系是非线性的方程如xk+1=f(xk,uk)x_{k+1} = f(x_{k}, u_{k}),其中状态量xxuu为系统的输入量。

    可以用俩种方法来推导状态误差传递的线性递推关系:

    • 一种是基于一阶泰勒展开的误差递推方程
    • 一种是基于误差随时间变化的递推方程 (论文是基于误差随时间变化来推导的)

    5.2.1 基于一阶泰勒展开的误差递推方程

    令状态量为x^=x+δx\hat{x} = x +\delta x,其中x^\hat{x}表示计算得到的值,带有误差,真值为xx,误差为δx\delta x。另外,输入量uu的噪声为nn
    应用一阶泰勒展开(EKF的协方差预测也是用了泰勒展开),非线性系统x^k+1=f(x^k,u^k)\hat{x}_{k+1}=f(\hat{x}_{k},\hat{u}_{k})的状态误差的线性递推关系为:
    δxk+1=Fδxk+Gnk\delta x_{k+1} = F\delta x_{k}+Gn_{k} 其中,F是状态量xk+1x_{k+1}对状态量xkx_{k}的雅克比矩阵,G是状态量xk+1x_{k+1}对输入量uku_{k}的雅克比矩阵。

    证明:对非线性状态方程进行一阶泰勒展开有:
    x^k+1=f(x^k,u^k)\hat{x}_{k+1} = f(\hat{x}_{k}, \hat{u}_{k}) xk+1+δxk+1=f(xk+δxk,uk+nk)x_{k+1}+\delta x_{k+1} = f(x_{k} + \delta x_{k}, u_{k} + n_{k}) xk+1+δxk+1=f(xk,uk)+Fδxk+Gnk(3)\underline{x_{k+1}}+\delta x_{k+1} = \underline{f(x_{k}, u_{k})} + F \delta x_{k} + Gn_{k} \tag{3}

    5.2.2 基于误差随时间变化(误差导数)的递推方程

    如果我们能够推导状态误差随时间变化的导数关系,如:
    δ˙x=Aδx+Bn\dot {\delta}_{x} = A\delta x + Bn 则误差状态的传递方程为:
    δxk+1=δxk+δ˙xkΔt\delta x_{k+1} = \delta x_{k} + \dot{\delta} x_{k} \Delta t δxk+1=(I+AΔt)δxk+BΔtnk(4)\rightarrow \delta x_{k+1} = (I+A\Delta t)\delta x_{k} + B\Delta tn_{k} \tag{4}

    由公式(3)(4)(3)(4)F=I+AΔtF=I+A\Delta t, G=BΔtG=B\Delta t

    5.2.3 基于泰勒展开和误差随时间变化的方法对比

    基于一阶泰勒展开的误差递推方程不香吗,为什么会用误差随时间的变化来进行误差递推方程的构建呢?

    在VIO系统中,我们已经知道了状态的导数和状态之间的转移矩阵,如我们已经知道速度和状态量之间的关系:
    v˙w=Rbwab+gw\dot{v}^{w} = R^{w}_{b}a^{b}+g^{w} 那我们就可以推导速度的误差导数和状态误差之间的关系,在每一项上都加上各自的误差就有:
    v˙w+δ˙vw=Rbwexp([δθ]×)(ab+δab)+g+δg\dot{v}^{w}+\dot{\delta }v^{w} = R_{b}^{w}exp([\delta \theta]_{\times})(a^{b} + \delta a^{b})+g+\delta g v˙w+δ˙vw=Rbw(I+[δθ]×)(ab+δab)+g+δg\dot{v}^{w}+\dot{\delta }v^{w} = R_{b}^{w}(I+[\delta \theta]_{\times})(a^{b} + \delta a^{b})+g+\delta g v˙w+δ˙vw=Rbwab+g+Rbwδab+Rbw[δθ]×(ab+δab)+δg\underline{\dot{v}}^{w} + \dot{\delta} v^{w}= \underline{R_{b}^{w} a^{b}+ g} +R_{b}^{w}\delta a^{b} + R_{b}^{w} [\delta \theta]_{\times} (a^{b} + \delta a^{b}) + \delta gδ˙vw=Rbwδab+Rbw[δθ]×(ab+δab)+δg\dot{\delta}{v}^{w} = R_{b}^{w}\delta a^{b} + R_{b}^{w} [\delta \theta]_{\times} (a^{b} + \delta a^{b}) + \delta g δ˙vw=RbwδabRbw[ab]×δθ+δg(5)\dot{\delta} v^{w} = R_{b}^{w}\delta a^{b} - R_{b}^{w} [a^{b}]_{\times} \delta \theta + \delta g \tag{5} 由此就能以此类推,轻易写出整个 A 和 B 其他方程了。

    考虑到公式的编辑篇幅,为了对一些求导公式进行简化,对求导公式进行了简化 (下同):
    awδθ=limδθ0Rbwexp([δθ]×)abRbwabδθawδθ=Rbwexp([δθ]×)abδθ\frac{\partial a^{w}}{\partial \delta \theta } = \underset{\delta \theta \rightarrow 0}{lim} \frac{R_{b}^{w}exp([\delta \theta]_{\times})a^{b}-R_{b}^{w}a^{b}}{\delta \theta} \rightarrow \frac{\partial a^{w}}{\partial \delta \theta} = \frac{\partial R_{b}^{w}exp([\delta \theta]_{\times}) a^{b}}{\partial \delta \theta}

    6. PVQ增量误差、协方差及雅克比矩阵的递推方程

    6.1 连续形式下的误差、协方差及雅克比矩阵的递推方程

    在第4部分我们已经完成了IMU预积分测量值的推导,而要将IMU预积分运用到非线性优化中,需要建立线性高斯误差状态递推方程 (可参考第5部分),由线性高斯系统的协方差,推到方程协方差矩阵,并纠结对应的雅克比矩阵。
    首先由于四元素γtbk\gamma_{t}^{b_{k}}被参数化过,我们将其误差定义为围绕其均值的扰动:【论文式(8)】
    γtbkγ^tbk[112δθtbk]\gamma_{t}^{b_{k}} \approx \hat{\gamma}_{t}^{b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \delta \theta_{t}^{b_{k}}\end{bmatrix}
    由预积分的连续形式建立的运动模型,由公式(2)可得一段时间内IMU构建的预积分量作为测量值,对俩时刻之间的状态量进行约束,可得到其在kkk+1k+1时刻下的误差项为:【论文公式(24)】
    [δαbk+1bkδβbk+1bkδθbk+1bkδbaδbw]=[Rwbk(pbk+1wpbkwvkwΔt+12gwΔt2)αbk+1bkRwbk(vbk+1wvbkw+gwΔt)βbk+1bk2[qwbkqbk+1wγbkbk+1]xyzbabk+1babkbwbk+1bwbk]\begin{bmatrix} \delta \alpha_{b_{k+1}}^{b_{k}} \\ \delta \beta_{b_{k+1}}^{b_{k}} \\ \delta \theta_{b_{k+1}}^{b_{k}} \\ \delta b_{a} \\ \delta b_{w} \end{bmatrix} = \begin{bmatrix} R^{b_{k}}_{w}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{k}^{w} \Delta t + \frac{1}{2}g^{w} \Delta t ^{2}) - \alpha_{b_{k+1}}^{b_{k}}\\ R^{b_{k}}_{w}(v_{b_{k+1}}^{w}-v_{b_{k}}^{w}+g^{w}\Delta t) - \beta ^{b_{k}}_{b_{k+1}} \\ 2[ q_{w}^{b_{k}} \otimes q^{w}_{b_{k+1}} \otimes \gamma_{b_{k}}^{b_{k+1}} ]_{xyz} \\ b_{ab_{k+1}}-b_{ab_{k}} \\ b_{wb_{k+1}}-b_{wb_{k}}\end{bmatrix}

    我们给出在tt时刻误差项的线性化递推方程为:【论文式(9)】:
    [δα˙tbkδβ˙tbkδθ˙tbkδb˙atδb˙wt]=[0I00000Rtbk[a^tbat]×Rtbk000[w^bwt]×0I0000000000][δαtbkδβtbkδθtbkδbatδbwt]+[0000Rtbk0000I0000I0000I][nanwnbanbw]=Ftδztbk+Gtnt(6)\begin{bmatrix} \delta \dot{\alpha}_{t}^{b_{k}} \\ \delta \dot{\beta}_{t}^{b_{k}} \\ \delta \dot{\theta}_{t}^{b_{k}} \\ \delta \dot{b}_{at} \\ \delta \dot{b}_{wt} \end{bmatrix} = \begin{bmatrix} 0 & I & 0 & 0 & 0\\ 0 & 0 & -R_{t}^{b_{k}}[\hat{a}_{t}-b_{at}]_{\times} & -R_{t}^{b_{k}} & 0\\ 0 & 0 & -[\hat{w}-b_{wt}]_{\times} & 0 & -I\\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \delta \alpha_{t}^{b_{k}} \\ \delta \beta_{t}^{b_{k}} \\ \delta \theta_{t}^{b_{k}} \\ \delta b_{at} \\ \delta b_{wt} \end{bmatrix} + \begin{bmatrix} 0 & 0 & 0 & 0\\ -R_{t}^{b_{k}} & 0 & 0 & 0\\ 0& -I & 0 & 0 \\ 0 & 0 & I & 0 \\ 0 & 0 & 0 & I \end{bmatrix} \begin{bmatrix} n_{a} \\ n_{w} \\ n_{ba} \\ n_{bw} \end{bmatrix} \tag{6} \\ = F_{t}\delta z_{t}^{b_{k}} + G_{t}n_{t} 可以简写为:
    δz˙tbk=Ftδztbk+Gtnt\delta \dot{z}_{t}^{b_{k}} = F_{t}\delta z_{t}^{b_{k}} + G_{t}n_{t} 其中FtF_{t}是15x15,GtG_{t}是12x12,δztbk\delta z_{t}^{b_{k}}是15x1,ntn_{t}是12x1。
    根据导数的定义有:
    δz˙tbk=limδt0δzt+δtbkδztbkδt\delta \dot{z}_{t}^{b_{k}} = \underset{\delta t\rightarrow 0}{lim} \frac{\delta z_{t+\delta t}^{b_{k}}-\delta z_{t}^{b_{k}}}{\delta t} 即:δzt+δtbk=δztbk+δz˙tbkδt=δztbk+(Ftδztbk+Gtnt)δt=(I+Ftδt)δztbk+Gtδtnt\delta z_{t+\delta t}^{b_{k}} = \delta z_{t}^{b_{k}} + \delta \dot{z}_{t}^{b_{k}} \delta t = \delta z_{t}^{b_{k}} + (F_{t}\delta z_{t}^{b_{k}} + G_{t}n_{t})\delta t = (I+F_{t} \delta t) \delta z_{t}^{b_{k}} + G_{t}\delta t n_{t} 这个形式与公式(4)不谋而合,一般连续形式下的微小时间用δt\delta t表示,离散形式下的时间区间用Δt\Delta t表示,其实都是一个意思。进一步的,令F=(I+Ftδt),V=GtδtF=(I+F_{t} \delta t), V=G_{t}\delta t, 有:
    δzt+δtbk=Fδztbk+Vnt(7)\delta z_{t+\delta t}^{b_{k}} = F \delta z_{t}^{b_{k}} + Vn_{t} \tag{7} 由此我们已经知道了该非线性系统的线性化递推方程。

    至于下一时刻的协方差,首先对于一个高斯分布的变量线性变换后得到新的变量的协方差计算,我们有以下推导:
    已知一个变量y=Axy=Axx(0,Σx)x \sim(0, \Sigma_{x}),则有 Σy=AΣxAT\Sigma_{y} = A \Sigma_{x} A^{T},即:
    Σy=E((Ax)(Ax)T)=E(AxxTAT)=AΣxAT\Sigma_{y} = E((Ax)(Ax)^{T}) = E(Axx^{T}A^{T}) = A \Sigma_{x} A^{T}

    得到了递推方程(7)(7)之后,根据高斯分布线性变换协方差的传递规律,我们可以预测下一时刻的协方差:【论文式(10)】
    Pt+δtbk=(I+Ftδt)Ptbk(I+Ftδt)T+(Gδt)Q(Gtδt)TP_{t+\delta t}^{b_{k}} = (I + F_{t} \delta t) P_{t}^{b_{k}} (I + F_{t} \delta t)^{T} + (G \delta t) Q (G_{t} \delta t)^{T}
    其中初始协方差 Pbkbk=0P_{b_{k}}^{b_{k}} = 0, QQ代表噪声项的对角协方差矩阵:
    Q12×12=[σa20000σw20000σba20000σbw2]Q^{12 \times 12} = \begin{bmatrix} \sigma _{a}^{2} & 0 & 0 &0 \\ 0 & \sigma _{w}^{2} &0 & 0 \\ 0 & 0 &\sigma_{ba}^{2} & 0 \\ 0 & 0 &0 & \sigma_{bw} ^{2} \end{bmatrix}

    同时也可以根据递推方程得到对应的雅克比矩阵迭代公式:【论文式[11]】
    Jt+δt=(I+Ftδt)JtJ_{t+\delta t} = (I+F_{t} \delta t) J_{t} 其中初始雅克比矩阵为:Jbk=IJ_{b_{k}} = I


    关于公式(6)(6)的推导:
    先把之前的预积分再写一遍:
    αbk+1bk=t[tk,tk+1]Rtbk(a^tbatna)dt2\alpha^{b_{k}}_{b_{k+1}} = \int \int _{t\in [t_{k}, t_{k+1}]} R_{t}^{b_{k}} (\hat{a}_{t}-b_{at}-n_{a}) dt^{2} βbk+1bk=t[tk,tk+1]Rtbk(a^tbatna)dt\beta_{b_{k+1}}^{b_{k}} = \int _{t\in [t_{k}, t_{k+1}]}R_{t}^{b_{k}}(\hat{a}_{t}-b_{at}-n_{a})dt γbk+1=t[tk,tk+1]12Ω(w^tbwtnw)γtbkdt\gamma_{b_{k+1}} = \int _{t \in [t_{k}, t_{k+1}]} \frac{1}{2} \Omega(\hat{w}_{t}-b_{wt}-n_{w}) \gamma_{t}^{b_{k}}dt 1)首先对于δα˙tbk\delta \dot{\alpha}_{t}^{b_{k}}δb˙at\delta \dot {b}_{at}δb˙wt\delta \dot{b}_{wt}, 显然根据定义有:
    δα˙tbk=α˙^tbkα˙tbk=β^tbkβtbk=δβtbk\delta \dot{\alpha}_{t}^{b_{k}} = \hat{\dot {\alpha}}_{t}^{b_{k}} - \dot {\alpha}_{t}^{b_{k}} = \hat{\beta}_{t}^{b_{k}} - \beta_{t}^{b_{k}} = \delta \beta_{t}^{b_{k}} δb˙at=b˙at0=nba\delta \dot{b}_{at} = \dot{b}_{at} - 0 = n_{b_{a}}