VIO残差函数的构建以及IMU预积分和协方差传递

基于滑动窗口的 VIO Bundle Adjustment，为了节约计算量采用滑动窗口形式的 Bundle Adjustment，在 i 时刻，
滑动窗口内待优化的系统状态量定义如下：
χ=[Xn,Xn+1,...,Xn+N,λm,λm+1,...,λm+M]\chi = [X_n,X_{n+1},...,X_{n+N},\lambda{m},\lambda{m+1},...,\lambda{m+M}]χ=[Xn,Xn+1,...,Xn+N,λm,λm+1,...,λm+M]Xi=[pwbi,qwbi,viw,babi,bgbi],i∈[n,n+N]X_i=[p_{wb_i},q_{wb_i},v_i^w,b_a^{b_i},b_g^b{_i}],i \in [n,n+N]Xi=[pwbi,qwbi,viw,babi,bgbi],i∈[n,n+N]
其中:

xix_ixi包含iii 时刻 IMU 机体的在惯性坐标系中的位置，速度，姿态，以及 IMU 机体坐标系中的加速度和角速度的偏置量估计。
m,nm,nm,n分别是机体状态量，路标在滑动窗口里的起始时刻。
NNN 滑动窗口中关键帧数量。
MMM 是被滑动窗口内所有关键帧观测到的路标数量。
λ\lambdaλ 是逆深度。

IMU的真实值为w,aw,aw,a，测量值为w~,a~\tilde{w},\tilde{a}w~,a~，则有：
w~b=wb+bg+ng\tilde{w}^b=w^b+b^g+n^gw~b=wb+bg+nga~b=qbw(aw+gw)+ba+na\tilde{a}^b=q_{bw}(a^w+g^w)+b^a+n^aa~b=qbw(aw+gw)+ba+na

P(位置)，V(速度)，Q(位姿) 对时间的导数可写成：
p˙wbt=vtw,v˙tw=atw,q˙wbt=qwbt⊗[012wbt]\dot{p}_{wb_t}=v^w_t,\dot{v}^w_t=a^w_t,\dot{q}_{wb_t}=q_{wb_t}⊗ \begin{bmatrix} 0 \\ {1\over 2}w^{b_t} \\ \end{bmatrix} p˙wbt=vtw,v˙tw=atw,q˙wbt=qwbt⊗[021wbt]
从第 iii 时刻的 PVQ 对 IMU 的测量值进行积分得到第 jjj 时刻的 PVQ：
pwbj=pwbi+viwΔt+∬t∈[i,j](qwbtabt−gw)δt2p_{wb_j}=p_{wb_i}+v_i^w\Delta t +\iint_{t \in [i,j]}(q_{wb_t}a^{b_t}-g^w)\delta t^2pwbj=pwbi+viwΔt+∬t∈[i,j](qwbtabt−gw)δt2viw=viw+∫t∈[i,j](qwbtabt−gw)δtv_i^w=v_i^w+\int_{t \in [i,j]}(q_{wb_t}a^{b_t}-g^w)\delta tviw=viw+∫t∈[i,j](qwbtabt−gw)δtqwbj=∫t∈[i,j]qwbt⊗[012wbt]δtq_{wb_j}=\int_{t \in [i,j]}q_{wb_t}⊗ \begin{bmatrix} 0 \\ {1\over 2}w^{b_t} \\ \end{bmatrix}\delta tqwbj=∫t∈[i,j]qwbt⊗[021wbt]δt
每次 qwbtq_{wb_t}qwbt 优化更新后，都需要重新进行积分，运算量较大。因此提出IMU预积分：
qwbt=qwbi⊗qbibtq_{wb_t}=q_{wb_i}⊗q_{b_ib_t}qwbt=qwbi⊗qbibt
那么，PVQ 积分公式中的积分项则变成相对于第 iii 时刻的姿态，而不是相对于世界坐标系的姿态：
pwbj=pwbi+viwΔt+∬t∈[i,j](qwbtabt−gw)δt2=pwbi+viwΔt−12gwΔt2+∬t∈[i,j](qwbtabt)δt2p_{wb_j}=p_{wb_i}+v_i^w\Delta t +\iint_{t \in [i,j]}(q_{wb_t}a^{b_t}-g^w)\delta t^2 =p_{wb_i}+v_i^w\Delta t- {1\over 2}g^w \Delta t^2+\iint_{t \in [i,j]}(q_{wb_t}a^{b_t})\delta t^2 pwbj=pwbi+viwΔt+∬t∈[i,j](qwbtabt−gw)δt2=pwbi+viwΔt−21gwΔt2+∬t∈[i,j](qwbtabt)δt2⟹pwbj=pwbi+viwΔt−12gwδt2+qwbi∬t∈[i,j](qbibtabt)δt2\implies \color{red}{p_{wb_j}=p_{wb_i}+v_i^w\Delta t- {1\over 2}g^w \delta t^2+q_{wb_i}\iint_{t \in [i,j]}(q_{b_ib_t}a^{b_t})\delta t^2}⟹pwbj=pwbi+viwΔt−21gwδt2+qwbi∬t∈[i,j](qbibtabt)δt2viw=viw+∫t∈[i,j](qwbtabt−gw)δt⟹viw=viw−gwΔt+qwbi∫t∈[i,j](qbibtabt)δtv_i^w=v_i^w+\int_{t \in [i,j]}(q_{wb_t}a^{b_t}-g^w)\delta t \implies \color{red}{ v_i^w=v_i^w-g^w\Delta t+q_{wb_i}\int_{t \in [i,j]}(q_{b_ib_t}a^{b_t})\delta t}viw=viw+∫t∈[i,j](qwbtabt−gw)δt⟹viw=viw−gwΔt+qwbi∫t∈[i,j](qbibtabt)δtqwbj=∫t∈[i,j]qwbt⊗[012wbt]δt⟹qwbj=qwbi∫t∈[i,j]qbibt⊗[012wbt]δtq_{wb_j}=\int_{t \in [i,j]}q_{wb_t}⊗ \begin{bmatrix} 0 \\ {1\over 2}w^{b_t} \\ \end{bmatrix}\delta t \implies \color{red}{q_{wb_j}=q_{wb_i}\int_{t \in [i,j]}q_{b_ib_t}⊗ \begin{bmatrix} 0 \\ {1\over 2}w^{b_t} \\ \end{bmatrix}\delta t}qwbj=∫t∈[i,j]qwbt⊗[021wbt]δt⟹qwbj=qwbi∫t∈[i,j]qbibt⊗[021wbt]δt

预积分量仅仅跟 IMU 测量值有关，它将一段时间内的 IMU 数据直接积分起来就得到了预积分量：
αbibj=∬t∈[i,j](qbibtabt)δt2\alpha_{b_ib_j}=\iint_{t \in [i,j]}(q_{b_ib_t}a^{b_t})\delta t^2αbibj=∬t∈[i,j](qbibtabt)δt2βbibj=∫t∈[i,j](qbibtabt)δt\beta_{b_ib_j}=\int_{t \in [i,j]}(q_{b_ib_t}a^{b_t})\delta tβbibj=∫t∈[i,j](qbibtabt)δtqbibj=∫t∈[i,j]qbibt⊗[012wbt]δtq_{b_ib_j}=\int_{t \in [i,j]}q_{b_ib_t}⊗ \begin{bmatrix} 0 \\ {1\over 2}w^{b_t} \\ \end{bmatrix}\delta tqbibj=∫t∈[i,j]qbibt⊗[021wbt]δt将αbibj,βbibj,qbibj\alpha_{b_ib_j},\beta_{b_ib_j},q_{b_ib_j}αbibj,βbibj,qbibj代入整理可得到最终的PVQ积分公式：
[pwbjvjwqwbjbjabjg]=[pwbi+viwΔt−12gwΔt2+qwbiαbibjviw−gwΔt+qwbiβbibjqwbiqbibjbiabig]\begin{bmatrix} p_{wb_j}\\v_j^w\\q_{wb_j}\\b_j^a\\b_j^g\\ \end{bmatrix}=\begin{bmatrix} p_{wb_i}+v_i^w\Delta t- {1\over 2}g^w \Delta t^2+q_{wb_i}\color{red}{\alpha_{b_ib_j}}\\v_i^w-g^w\Delta t+q_{wb_i}\color{red}{\beta_{b_ib_j}} \\q_{wb_i}\color{red}{q_{b_ib_j}}\\b_i^a\\b_i^g\\ \end{bmatrix}⎣⎢⎢⎢⎢⎡pwbjvjwqwbjbjabjg⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡pwbi+viwΔt−21gwΔt2+qwbiαbibjviw−gwΔt+qwbiβbibjqwbiqbibjbiabig⎦⎥⎥⎥⎥⎤

预积分误差：一段时间内 IMU 构建的预积分量作为测量值，对两时刻之间的状态量进行约束
[rprqrvrbarbg]=[pwbj−pwbi−viwΔt+12gwΔt2−qwbiαbibj2[qbjbi⊗(qbiw⊗qwbj)]xyzvjw−viw+gwΔt−qwbiβbibjbja−biabjg−big]=[qbiw(pwbj−pwbi−viwΔt+12gwΔt2)−αbibj2[qbjbi⊗(qbiw⊗qwbj)]xyzqbiw(vjw−viw+gwΔt)−βbibjbja−biabjg−big]\begin{bmatrix} r_p\\r_q\\r_v\\r_{ba}\\r_{bg}\\ \end{bmatrix}=\begin{bmatrix} p_{wb_j} - p_{wb_i}-v_i^w\Delta t+ {1\over 2}g^w \Delta t^2-q_{wb_i}\color{red}{\alpha_{b_ib_j}}\\ 2[{\color{red}{q_{b_jb_i}}}⊗(q_{b_iw} ⊗ q_{wb_j})]_{xyz} \\v_j^w - v_i^w+g^w\Delta t-q_{wb_i}\color{red}{\beta_{b_ib_j}} \\b_j^a-b_i^a\\b_j^g-b_i^g\\ \end{bmatrix}=\begin{bmatrix} q_{b_iw}(p_{wb_j} - p_{wb_i}-v_i^w\Delta t+ {1\over 2}g^w \Delta t^2)-\color{red}{\alpha_{b_ib_j}}\\ 2[{\color{red}{q_{b_jb_i}}}⊗(q_{b_iw} ⊗ q_{wb_j})]_{xyz} \\q_{b_iw}(v_j^w - v_i^w+g^w\Delta t)-\color{red}{\beta_{b_ib_j}} \\b_j^a-b_i^a\\b_j^g-b_i^g\\ \end{bmatrix}⎣⎢⎢⎢⎢⎡rprqrvrbarbg⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡pwbj−pwbi−viwΔt+21gwΔt2−qwbiαbibj2[qbjbi⊗(qbiw⊗qwbj)]xyzvjw−viw+gwΔt−qwbiβbibjbja−biabjg−big⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡qbiw(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj2[qbjbi⊗(qbiw⊗qwbj)]xyzqbiw(vjw−viw+gwΔt)−βbibjbja−biabjg−big⎦⎥⎥⎥⎥⎤
上面误差中位移，速度，偏置都是直接相减得到。第二项是关于四元数的旋转误差，其中 [⋅]xyz[\cdot]_{xyz}[⋅]xyz 表示只取四元数的虚部 (x,y,z)(x, y, z)(x,y,z) 组成的三维向量。

预积分的离散形式
以上是预积分的连续形式，也可以使用离散形式，这里使用 mid−pointmid-pointmid−point 方法，即两个相邻时刻 kkk 到 k+1k+1k+1 的位姿是用两个时刻的测量值 a,ωa, ωa,ω 的平均值来计算：w=12((wbk−bkg)+(wbk+1−bkg))w={1 \over 2}((w^{b_k}-b_k^g)+(w^{b_{k+1}}-b_k^g))w=21((wbk−bkg)+(wbk+1−bkg))a=12(qbibk(abk−bka)+qbibk+1(abk+1−bka))a={1\over 2}(q_{b_ib_k}(a^{b_k}-b_k^a)+q_{b_ib_{k+1}}(a^{b_{k+1}}-b_k^a))a=21(qbibk(abk−bka)+qbibk+1(abk+1−bka))

预积分量的方差
协方差传递性质：在一个线性系统中，已知一个变量 y=Ax,x∈N(0,Σx)y = Ax, x ∈ \mathcal N (0, Σ_x)y=Ax,x∈N(0,Σx), 则有Σy=AΣxA⊤Σ_y = AΣ_xA^⊤Σy=AΣxA⊤
Σy=E((Ax)(Ax)⊤)=E(Axx⊤A⊤)=AΣxA⊤Σ_y = E((Ax)(Ax)^⊤) = E(Axx^⊤A^⊤) = AΣ_xA^⊤Σy=E((Ax)(Ax)⊤)=E(Axx⊤A⊤)=AΣxA⊤要推导预积分量的协方差，我们需要知道 imu 噪声和预积分量之间的线性递推关系。

假设已知了相邻时刻误差的线性传递方程：ηik=Fk−1ηik−1+Gk−1nk−1η_{ik} = {\color{red}F_{k−1}η_{ik−1}} + {\color{blue}G_{k−1}n_{k−1}}ηik=Fk−1ηik−1+Gk−1nk−1比如：状态量误差为 ηik=[δθik,δvik,δpik]η_{ik} = [δθ_{ik}, δv_{ik}, δp_{ik}]ηik=[δθik,δvik,δpik]，测量噪声为nk=[nkg,nka]n_k = [n^g_k, n^a_k]nk=[nkg,nka]。
误差的传递由两部分组成：1.当前时刻的误差传递给下一时刻，2.当前时刻测量噪声传递给下一时刻。
协方差矩阵可以通过递推Σy=AΣxA⊤Σ_y = AΣ_xA^⊤Σy=AΣxA⊤计算得到：Σik=Fk−1Σik−1Fk−1⊤+Gk−1ΣnGk−1⊤Σ_{ik} = {\color{red} F_{k−1}Σ_{ik−1}F^⊤_{k−1}} +{\color{blue} G_{k−1}Σ_nG^⊤_{k−1}}Σik=Fk−1Σik−1Fk−1⊤+Gk−1ΣnGk−1⊤其中，ΣnΣ_nΣn 是测量噪声的协方差矩阵，方差从iii 时刻开始进行递推，Σii=0Σ_{ii} = 0Σii=0。

状态误差线性递推公式的推导(线性化)
协方差传递性质只对于线性化系统，通常对于状态量之间的递推关系是非线性的方程如xk=f(xk−1,uk−1)x_k = f(x_{k−1}, u_{k−1})xk=f(xk−1,uk−1)，其中状态量为 x，ux，ux，u为系统的输入量。

我们可以用两种方法来推导状态误差传递的线性递推关系：

一种是基于一阶泰勒展开的误差递推方程（应用于 EKF 的协方差预测）。
令状态量为 x=x^+δxx = \hat{x} + δxx=x^+δx，其中，真值为 x^\hat{x}x^，误差为 δxδxδx。另外，输入量uuu 的噪声为 nnn。
非线性系统 xk=f(xk−1,uk−1)x_k = f(x_{k−1}, u_{k−1})xk=f(xk−1,uk−1) 方程进行一阶泰勒展开有：xk=f(xk−1,uk−1)x_k = f(x_{k−1}, u_{k−1})xk=f(xk−1,uk−1)x^k+δxk=f(x^k−1+δxk−1,u^k−1+nk−1)\hat{x}_k +δx_k= f(\hat{x}_{k−1}+δx_{k-1}, \hat{u}_{k−1}+n_{k-1})x^k+δxk=f(x^k−1+δxk−1,u^k−1+nk−1)x^k+δxk=f(x^k−1,u^k−1)+Fδxk−1+Gnk−1\hat{x}_k +δx_k= f(\hat{x}_{k−1}, \hat{u}_{k−1})+Fδx_{k-1}+Gn_{k-1}x^k+δxk=f(x^k−1,u^k−1)+Fδxk−1+Gnk−1δxk=Fδxk−1+Gnk−1{\color{red}δx_k= Fδx_{k-1}+Gn_{k-1}}δxk=Fδxk−1+Gnk−1其中，F 是状态量 xkx_kxk 对状态量 xk−1x_{k−1}xk−1 的雅克比矩阵，G 是状态量 xkx_kxk对输入量 uk−1u_{k−1}uk−1 的雅克比矩阵。
一种是基于误差随时间变化的递推方程。
如果我们能够推导状态误差随时间变化的导数关系，比如：δx˙=Aδx+Bnδ\dot{x} = Aδx + Bn δx˙=Aδx+Bn则误差状态的传递方程为：
δxk=δxk−1+δx˙k−1∆t→δxk=(I+A∆t)δxk−1+B∆tnk−1δx_k = δx_{k−1} + δ\dot{x}_{k−1}∆t → δx_k = (I + A∆t)δx_{k−1} + B∆tn_{k−1}δxk=δxk−1+δx˙k−1∆t→δxk=(I+A∆t)δxk−1+B∆tnk−1

这两种推导方式的可以看出有：
F=I+A∆t,G=B∆tF = I + A∆t, G = B∆tF=I+A∆t,G=B∆t

回顾预积分(离散形式)的误差递推公式，将测量噪声也考虑进模型：
w=12((wbk+nkg−bkg)+(wbk+1+nk+1g−bkg))w={1 \over 2}((w^{b_k}+ {\color{red}n_k^g}-b_k^g)+(w^{b_{k+1}}+ {\color{red}n_{k+1}^g}-b_k^g))w=21((wbk+nkg−bkg)+(wbk+1+nk+1g−bkg))a=12(qbibk(abk+nka−bka)+qbibk+1(abk+1+nk+1a−bka))a={1\over 2}(q_{b_ib_k}(a^{b_k}+ {\color{red}n_k^a}-b_k^a)+q_{b_ib_{k+1}}(a^{b_{k+1}}+ {\color{red}n_{k+1}^a}-b_k^a))a=21(qbibk(abk+nka−bka)+qbibk+1(abk+1+nk+1a−bka))确定误差传递的状态量，噪声量，然后开始构建传递方程。用前面一阶泰勒展开的推导方式，我们能推导出如下的形式：[δαbk+1b′k+1δθbk+1b′k+1δβbk+1b′k+1δbk+1aδbk+1g]=F[δαbkb′kδθbkb′kδβbkb′kδbkaδbkg]+G[nkankgnk+1ank+1gnbaanbkg]\begin{bmatrix} δα_{b_{k+1}b′_{k+1}} \\ δθ_{b_{k+1}b′_{k+1}} \\ δβ_{b_{k+1}b′_{k+1}} \\δb^a_{k+1}\\ δb^g_{k+1}\\ \end{bmatrix}=F\begin{bmatrix} δα_{b_{k}b′_{k}} \\ δθ_{b_{k}b′_{k}} \\ δβ_{b_{k}b′_{k}} \\δb^a_{k}\\ δb^g_{k}\\ \end{bmatrix}+G\begin{bmatrix} n^a_k\\n^g_k\\n^a_{k+1}\\n^g_{k+1}\\n_{b^a_a}\\n_{b^g_k}\\ \end{bmatrix}⎣⎢⎢⎢⎢⎡δαbk+1b′k+1δθbk+1b′k+1δβbk+1b′k+1δbk+1aδbk+1g⎦⎥⎥⎥⎥⎤=F⎣⎢⎢⎢⎢⎡δαbkb′kδθbkb′kδβbkb′kδbkaδbkg⎦⎥⎥⎥⎥⎤+G⎣⎢⎢⎢⎢⎢⎢⎡nkankgnk+1ank+1gnbaanbkg⎦⎥⎥⎥⎥⎥⎥⎤F, G 为两个时刻间的协方差传递矩阵。

对VIO残差函数的构建的理解：

对于TTT时刻的状态量，如位置、速度、变换、偏置。可通过IMU测量值(w,a)(w,a)(w,a)积分得到T+1T+1T+1时刻的状态。

由于状态量中的参考系都在全局坐标系下，对于每一时刻的状态优化完成之后，需要重新积分得到下一时刻的状态，因此提出预积分的概念，将积分转换成相对于上一时刻的量。

通过对上一时刻TTT预积分得到当前时刻T+1T+1T+1的状态作为测量值，通过当前时刻的真实值作为约束，得到残差函数。

以上过程再考虑到噪声、误差的影响，通过线性系统的协方差传递性质，将状态量之间的递推关系线性化，即利用F,G，考虑到上一时刻的误差以及当前时刻的观测误差。最终所得到的残差函数即为，上一时刻的预积分加上传递误差，与当前时刻的约束。

VIO残差函数的构建以及IMU预积分和协方差传递相关推荐

《视觉SLAM进阶：从零开始手写VIO》第三讲基于优化的IMU预积分与视觉信息融合作业
<视觉SLAM进阶:从零开始手写VIO>第三讲基于优化的IMU预积分与视觉信息融合作业文章目录 <视觉SLAM进阶:从零开始手写VIO>第三讲基于优化的IMU预积分与视 ...
关于DSO直接法与IMU预积分联合VIO/SLAM一些思路
本文不适合初学者:干货多没写具体方法,目前还在数论分解和思考中,估计得2个月后完成. 必要性: 1.常规VIO系统如VINS-MONO建立的地图质量太差,稀疏且不便认知 2.假设并入D相机,无论紧耦合 ...
[学习SLAM]VINS中IMU预积分的误差推到公式与代码雅克比(协防差/信息矩阵)构建
//todo /** *IMU预积分中采用中值积分地推jacobian和covariance **/ void midPointIntegration(double _dt, const Eigen: ...
VINS-Mono 代码详细解读——IMU预积分的残差、Jacobian和协方差
前言: 对第k帧和第k+1帧之间所有的IMU进行积分,可得到第K+1帧的PVQ(位置.速度.旋转),作为视觉估计的初始值. 每次qwbt优化更新后,都要重新进行积分,运算量较大.将积分模型转为预积分模 ...
IMU预积分及残差雅克比计算
前段时间推了泡泡机器人邱笑晨博士的IMU预积分公式,收获很大,再看到VIORB的时候,大都用的这一套公式. 后再看VINS的时候,VINS用的是连续时间的预积分,看一些大佬的博客,说代码里写的是离散时 ...
VINS-mono 论文解读：IMU预积分+Marg边缘化
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 VINS-mono 论文解读(IMU预积分+Marg边缘化) 前面 ...
VINS-Mono之IMU预积分，预积分误差、协方差及误差对状态量雅克比矩阵的递推方程的推导
文章目录 1. 前言 2. IMU模型 3. 基于世界坐标系下的IMU运动模型 3.1 连续形式下的IMU运动模型 3.2 离散形式下的IMU运动模型 3.2.1 欧拉法离散形式 3.2.2 中值法离 ...
十七.激光和惯导LIO-SLAM框架学习之IMU和IMU预积分
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
VINS-Mono理论学习——IMU预积分 Pre-integration （Jacobian 协方差）
引言 VINS论文的IV-B. IMU Pre-integration介绍了IMU预积分模型,Foster的两篇论文对IMU预积分理论进行详细分析. 传统传统捷联惯性导航的递推算法,是在已知上一时刻的 ...

VIO残差函数的构建以及IMU预积分和协方差传递

对VIO残差函数的构建的理解：

VIO残差函数的构建以及IMU预积分和协方差传递相关推荐

最新文章

热门文章