[VINS-Mono]IMU预积分之PVQ 增量的误差、 协方差及 Jacobian

PVQ 增量的误差、 协方差及 Jacobian

根据协方差矩阵的性质:
y = A x , x ∈ N ( 0 , Σ x ) Σ y = A Σ x A T \begin{array}{c} y = Ax, x\in N\left(0, \Sigma _{x}\right) \\ \Sigma_y = A \Sigma_{x}A^{T} \end{array} y=Ax,xN(0,Σx)Σy=AΣxAT
则相邻时刻误差的线性传递方程:
η i k = F k − 1 η i k − 1 + G k − 1 n k − 1 \eta_{ik} = F_{k-1} \eta_{ik-1}+G_{k-1}n_{k-1} ηik=Fk1ηik1+Gk1nk1
其中:

  • η i k = [ δ θ i k , δ v i k , δ p i k ] \eta_{ik}=\left[\delta \theta _{ik}, \delta v_{ik}, \delta p_{ik}\right] ηik=[δθik,δvik,δpik]:状态量误差
  • n k = [ n k g , n k a ] n_{k} = \left[n_{k}^g, n_{k}^a\right] nk=[nkg,nka]:测量噪声

误差的传递由两部分组成:

  • 当前时刻的误差传递给下一时刻
  • 当前时刻测量噪声传递给下一时刻

因此协方差矩阵可以递推得到:
Σ i k = F k − 1 Σ i k − 1 F k − 1 T + G k − 1 Σ n G k − 1 T \Sigma_{ik} = F_{k-1}\Sigma_{ik-1}F_{k-1}^T + G_{k-1}\Sigma_{n}G_{k-1}^{T} Σik=Fk1Σik1Fk1T+Gk1ΣnGk1T

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

令状态量: x = x ^ + δ x x= \hat{x}+\delta x x=x^+δx,其中 x ^ \hat{x} x^为真值,误差为 δ x \delta x δx,输入量u的噪声为n
非线性系统:
x k = f ( x k − 1 , u k − 1 ) x_{k} = f\left(x_{k-1}, u_{k-1}\right) xk=f(xk1,uk1)
其状态的误差的线性递推关系为:
δ x k = F δ x k − 1 + G n k − 1 \delta x_{k} =F \delta x_{k-1}+Gn_{k-1} δxk=Fδxk1+Gnk1

2.基于误差随时间变化的递推方程

状态误差随时间变化的导数关系为:
δ x ˙ = A δ x + B n \dot{\delta_{x}} = A\delta x+Bn δx˙=Aδx+Bn
则误差状态的传递方程为:
δ x k = δ x k − 1 + δ x k − 1 ˙ Δ t \delta x_{k} = \delta x_{k-1} + \dot{\delta x_{k-1}}\Delta t δxk=δxk1+δxk1˙Δt
得到:
δ x k = ( I + A Δ t ) δ k − 1 + B Δ t n k − 1 \delta x_{k} = \left(I+A\Delta t\right)\delta_{k-1}+B\Delta tn_{k-1} δxk=(I+AΔt)δk1+BΔtnk1
两种推导得到:
F = I + A Δ t , G = B Δ t F=I+A\Delta t, G=B\Delta t F=I+AΔt,G=BΔt

3.PVQ 增量的误差、 协方差及 Jacobian的连续形式

  • 真实状态:去除噪声后的理想状态
  • 标称状态:未去除噪声的大信号
  • 误差状态:噪声小信号

真实状态等于标称状态误差状态
α ^ t b k = α t b k + δ α t b k β ^ t b k = β t b k + δ β t b k γ ^ t b k = γ t b k ⊗ [ 1 1 2 δ θ t b k ] b ^ a t = b a t + δ b a t b ^ ω t = b ω t + δ b ω t \begin{array}{c} \hat{\alpha}_{t}^{b_{k}}=\alpha_{t}^{b_{k}}+\delta \alpha_{t}^{b_{k}} \\ \hat{\beta}_{t}^{b_{k}}=\beta_{t}^{b_{k}}+\delta \beta_{t}^{b_{k}} \\ \hat{\gamma}_{t}^{b_{k}}=\gamma_{t}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \theta_{t}^{b_{k}} \end{array}\right] \\ \hat{b}_{a_{t}}=b_{a_{t}}+\delta b_{a_{t}} \\ \hat{b}_{\omega_{t}}=b_{\omega_{t}}+\delta b_{\omega_{t}} \end{array} α^tbk=αtbk+δαtbkβ^tbk=βtbk+δβtbkγ^tbk=γtbk[121δθtbk]b^at=bat+δbatb^ωt=bωt+δbωt
真实状态的状态方程:
{ α ^ ˙ t b k = β ^ t b k β ^ ˙ t b k = R ^ t b k ( a ^ t − b ^ a t − n a ) γ ^ ˙ t b k = 1 2 Ω ( ω t − b ^ ω t − n ω ) γ ^ t b k b ^ ˙ a t = n b a b ^ ˙ ω t = n b ω \left\{\begin{array}{l} \dot{\hat{\alpha}}_{t}^{b_{k}}=\hat{\beta}_{t}^{b_{k}} \\ \dot{\hat{\beta}}_{t}^{b_{k}}=\hat{R}_{t}^{b_{k}}\left(\hat{a}_{t}-\hat{b}_{a_{t}}-n_{a}\right) \\ \dot{\hat{\gamma}}_{t}^{b_{k}}=\frac{1}{2} \Omega\left(\omega_{t}-\hat{b}_{\omega_{t}}-n_{\omega}\right) \hat{\gamma}_{t}^{b_{k}} \\ \dot{\hat{b}}_{a_{t}}=n_{b_{a}} \\ \dot{\hat{b}}_{\omega_{t}}=n_{b_{\omega}} \end{array}\right. α^˙tbk=β^tbkβ^˙tbk=R^tbk(a^tb^atna)γ^˙tbk=21Ω(ωtb^ωtnω)γ^tbkb^˙at=nbab^˙ωt=nbω
标称状态的状态方程:没有噪声和扰动
{ α ˙ t b k = β t b k β ˙ t b k = R t b k ( a ^ t − b a t ) γ ˙ t b k = 1 2 Ω ( ω t − b ω t ) γ t b k b ˙ a t = 0 b ˙ ω t = 0 \left\{\begin{array}{l} \dot{\alpha}_{t}^{b_{k}}=\beta_{t}^{b_{k}} \\ \dot{\beta}_{t}^{b_{k}}=R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right) \\ \dot{\gamma}_{t}^{b_{k}}=\frac{1}{2} \Omega\left(\omega_{t}-b_{\omega_{t}}\right) \gamma_{t}^{b_{k}} \\ \dot{b}_{a_{t}}=0 \\ \dot{b}_{\omega_{t}}=0 \end{array}\right. α˙tbk=βtbkβ˙tbk=Rtbk(a^tbat)γ˙tbk=21Ω(ωtbωt)γtbkb˙at=0b˙ωt=0
根据定义得到 δ α ˙ t b k \delta \dot{\alpha}_{t}^{b_{k}} δα˙tbk, δ b ˙ a t \delta \dot{b}_{a_{t}} δb˙at, δ b ˙ w t \delta \dot{b}_{w_{t}} δb˙wt
δ α ˙ t b k = α ^ ˙ t b k − α ˙ t b k = β ^ t b k − β t b k = δ β t b k δ b ˙ a t = b ˙ a t − 0 = n b a δ b ˙ w t = b ˙ w t − 0 = n b w \begin{align} \delta \dot{\alpha}_{t}^{b_{k}} & = \dot{\hat{\alpha}}_{t}^{b_{k}} -\dot{\alpha}_{t}^{b_{k}} \\ & = \hat{\beta}_{t}^{b_{k}}-\beta_{t}^{b_{k}} \\ & = \delta \beta_{t}^{b_k} \\ \delta \dot{b}_{a_{t}} & = \dot{b}_{a_{t}} - 0 \\ & = n_{b_a} \\ \delta \dot{b}_{w_{t}} & = \dot{b}_{w_{t}} - 0 \\ & = n_{b_w} \end{align} δα˙tbkδb˙atδb˙wt=α^˙tbkα˙tbk=β^tbkβtbk=δβtbk=b˙at0=nba=b˙wt0=nbw
对于 δ β ˙ t b k \delta \dot{\beta}_{t}^{b_{k}} δβ˙tbk真值状态为:
β ˙ ^ t b k = R ^ t b k ( a ^ t − b ^ a t − n a ) = R t b k exp ⁡ ( [ δ θ ] × ) ( a ^ t − n a − b a t − δ b a t ) = R t b k ( 1 + [ δ θ ] × ) ( a ^ t − n a − b a t − δ b a t ) = R t b k ( a ^ t − n a − b a t − δ b a t + [ δ θ ] × ( a ^ t − b a t ) ) = R t b k ( a ^ t − n a − b a t − δ b a t − [ a ^ t − b a t ] × δ θ ) \begin{align} \hat{\dot{\beta}}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}} & = \hat{\mathrm{R}}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}}\left(\hat{\mathrm{a}}_{\mathrm{t}}-\hat{\mathrm{b}}_{\mathrm{a}_{\mathrm{t}}}-\mathrm{n}_{\mathrm{a}}\right) \\ & = \mathrm{R}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}} \exp \left([\delta \theta]_{\times}\right)\left(\hat{\mathrm{a}}_{\mathrm{t}}-\mathrm{n}_{\mathrm{a}}-\mathrm{b}_{\mathrm{a}_{\mathrm{t}}}-\delta \mathrm{b}_{\mathrm{a}_{\mathrm{t}}}\right) \\ & = \mathrm{R}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}}\left(1+[\delta \theta]_{\times}\right)\left(\hat{\mathrm{a}}_{\mathrm{t}}-\mathrm{n}_{\mathrm{a}}-\mathrm{b}_{\mathrm{a}_{\mathrm{t}}}-\delta \mathrm{b}_{\mathrm{a}_{\mathrm{t}}}\right) \\ & = \mathrm{R}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}}\left(\hat{\mathrm{a}}_{\mathrm{t}}-\mathrm{n}_{\mathrm{a}}-\mathrm{b}_{\mathrm{a}_{\mathrm{t}}}-\delta \mathrm{b}_{\mathrm{a}_{\mathrm{t}}}+[\delta \theta]_{\times}\left(\hat{\mathrm{a}}_{\mathrm{t}}-\mathrm{b}_{\mathrm{a}_{\mathrm{t}}}\right)\right) \\ & = \mathrm{R}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}}\left(\hat{\mathrm{a}}_{\mathrm{t}}-\mathrm{n}_{\mathrm{a}}-\mathrm{b}_{\mathrm{at}}-\delta \mathrm{b}_{\mathrm{at}}-\left[\hat{\mathrm{a}}_{\mathrm{t}}-\mathrm{b}_{\mathrm{at}}\right]_{\times} \delta \theta\right) \end{align} β˙^tbk=R^tbk(a^tb^atna)=Rtbkexp([δθ]×)(a^tnabatδbat)=Rtbk(1+[δθ]×)(a^tnabatδbat)=Rtbk(a^tnabatδbat+[δθ]×(a^tbat))=Rtbk(a^tnabatδbat[a^tbat]×δθ)
标称状态为:
β ˙ t b k = R t b k ( a ^ t − b a t ) \dot{\beta}_{t}^{b_{k}}=R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right) β˙tbk=Rtbk(a^tbat)
因此:
δ β ˙ t b k = β ˙ ^ t b k − β ˙ t b k = − R t b k [ a ^ t − b a t ] × δ θ − R t b k δ b a t − R t b k n a \begin{align} \delta \dot{\beta}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}} & = \hat{\dot{\beta}}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}}-\dot{\beta}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}}\\ & = -\mathrm{R}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}}\left[\hat{\mathrm{a}}_{\mathrm{t}}-\mathrm{b}_{\mathrm{at}}\right]_{\times} \delta \theta-\mathrm{R}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}} \delta \mathrm{b}_{\mathrm{at}}-\mathrm{R}_{\mathrm{t}}^{\mathrm{b}_{\mathrm{k}}} \mathrm{n}_{\mathrm{a}} \end{align} δβ˙tbk=β˙^tbkβ˙tbk=Rtbk[a^tbat]×δθRtbkδbatRtbkna
对于 δ θ ˙ t b k \delta \dot{\theta}_{t}^{b_{k}} δθ˙tbk,根据四元数的导数得到:
q ˙ t = 1 2 [ − w ∧ w − w T 0 ] q t = 1 2 Ω ( w ) q t = 1 2 R ( [ w 0 ] ) q t = 1 2 q t ⊗ [ w 0 ] \dot{\mathbf{q}}_{t}=\frac{1}{2}\left[\begin{array}{ll} -w^{\wedge} & w \\ -w^{T} & 0 \end{array}\right] \mathbf{q}_{t}=\frac{1}{2} \Omega(w) \mathbf{q}_{t}=\frac{1}{2} \mathcal{R}\left(\left[\begin{array}{l} w \\ 0 \end{array}\right]\right) \mathbf{q}_{t}=\frac{1}{2} \mathbf{q}_{t} \otimes\left[\begin{array}{l} w \\ 0 \end{array}\right] q˙t=21[wwTw0]qt=21Ω(w)qt=21R([w0])qt=21qt[w0]
可以得到:

  • 真实值:
    q ˙ ^ = 1 2 q ^ ⊗ [ w ^ 0 ] = 1 2 q ^ ⊗ δ q ⊗ [ w ^ − b w t − n w − δ b w t 0 ] \begin{align} \hat{\dot{q}} & = \frac{1}{2}\hat{q}\otimes\begin{bmatrix} \hat{w} \\ 0 \end{bmatrix} \\ & = \frac{1}{2}\hat{q}\otimes\delta q \otimes \begin{bmatrix} \hat{w} - b_{w_{t}} -n_{w}-\delta b_{w_{t}} \\ 0 \end{bmatrix} \end{align} q˙^=21q^[w^0]=21q^δq[w^bwtnwδbwt0]
  • 标称值:
    q ˙ t = 1 2 q t ⊗ [ w ^ t − b w t 0 ] \dot{q}_t = \frac{1}{2}q_{t}\otimes \begin{bmatrix} \hat{w}_{t}-b_{w_{t}} \\0 \end{bmatrix} q˙t=21qt[w^tbwt0]
    根据导数的性质,可以得到:
    q ˙ ^ t = q t ⊗ δ q ⏞ ˙ = q ˙ t ⊗ δ q + q t ⊗ δ q ˙ = 1 2 q t ⊗ [ w ^ t − b w t 0 ] ⊗ δ q + q t ⊗ δ q ˙ \begin{align} \hat{\dot{q}}_{t} & = \dot{\overbrace{q_{t}\otimes \delta q} } \\ & = \dot{q}_{t}\otimes \delta q+q_{t}\otimes \delta \dot{q} \\ & = \frac{1}{2}q_{t}\otimes \begin{bmatrix} \hat{w}_{t}-b_{w_t} \\ 0 \end{bmatrix}\otimes \delta q+q_{t}\otimes \delta \dot{q} \end{align} q˙^t=qtδq ˙=q˙tδq+qtδq˙=21qt[w^tbwt0]δq+qtδq˙

因此:
1 2 q t ⊗ δ q ⊗ [ w ^ t − b w t − n w − δ b w t 0 ] = 1 2 q t ⊗ [ w ^ t − b w t 0 ] ⊗ δ q + q t ⊗ δ q ˙ ⇔ 1 2 δ q ⊗ [ w ^ t − b w t − n w − δ b w t 0 ] = 1 2 [ w ^ t − b w t 0 ] ⊗ δ q + δ q ˙ ⇔ 2 δ ˙ q = δ q ⊗ [ w ^ t − b w t − n w − δ b w t 0 ] − [ w ^ t − b w t 0 ] ⊗ δ q ⇔ 2 δ ˙ q = R ( [ w ^ t − b w t − n w − δ b w t 0 ] ) δ q − L ( [ w ^ t − b w t 0 ] ) δ q ⇔ 2 δ ˙ q = ( [ − ( w ^ t − b w t − n w − δ b w t ) ∧ w ^ t − b w t − n w − δ b w t − ( w ^ t − b w t − n w − δ b w t ) T 0 ] − [ ( w ^ t − b w t ) ∧ w ^ t − b w t − ( w ^ t − b w t ) T 0 ] ) [ δ θ 2 1 ] ⇔ 2 δ q ˙ = [ − ( 2 w ^ t − 2 b w t − n w − δ b w t ) ∧ − n w − δ b w t ( n w + δ b w t ) T 0 ] [ δ θ 2 1 ] \begin{array}{l} \frac{1}{2} \mathbf{q}_{t} \otimes \delta \mathbf{q} \otimes\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}} \\ 0 \end{array}\right]=\frac{1}{2} \mathbf{q}_{t} \otimes\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}} \\ 0 \end{array}\right] \otimes \delta \mathbf{q}+\mathbf{q}_{t} \otimes \dot{\delta \mathbf{q}} \\ \Leftrightarrow \frac{1}{2} \delta \mathbf{q} \otimes\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}} \\ 0 \end{array}\right]=\frac{1}{2}\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}} \\ 0 \end{array}\right] \otimes \delta \mathbf{q}+\dot{\delta \mathbf{q}} \\ \Leftrightarrow 2 \dot{\delta} \mathbf{q}=\delta \mathbf{q} \otimes\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}} \\ 0 \end{array}\right]-\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}} \\ 0 \end{array}\right] \otimes \delta \mathbf{q} \\ \Leftrightarrow 2 \dot{\delta} \mathbf{q}=\mathcal{R}\left(\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}} \\ 0 \end{array}\right]\right) \delta \mathbf{q}-\mathcal{L}\left(\left[\begin{array}{c} \hat{w}_{t}-b_{w_{t}} \\ 0 \end{array}\right]\right) \delta \mathbf{q} \\ \Leftrightarrow 2 \dot{\delta} \mathbf{q}=\left(\left[\begin{array}{cc} -\left(\hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}}\right)^{\wedge} & \hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}} \\ -\left(\hat{w}_{t}-b_{w_{t}}-n_{w}-\delta b_{w_{t}}\right)^{T} & 0 \end{array}\right]-\left[\begin{array}{cc} \left(\hat{w}_{t}-b_{w_{t}}\right)^{\wedge} & \hat{w}_{t}-b_{w_{t}} \\ -\left(\hat{w}_{t}-b_{w_{t}}\right)^{T} & 0 \end{array}\right]\right)\begin{bmatrix} \frac{\delta \theta}{2} \\ 1 \end{bmatrix} \\ \Leftrightarrow 2 \dot{\delta \mathbf{q}}=\left[\begin{array}{cc} -\left(2 \hat{w}_{t}-2 b_{w_{t}}-n_{w}-\delta b_{w_{t}}\right)^{\wedge} & -n_{w}-\delta b_{w_{t}} \\ \left(n_{w}+\delta b_{w_{t}}\right)^{T} & 0 \end{array}\right]\left[\begin{array}{c} \frac{\delta \theta}{2} \\ 1 \end{array}\right] \\ \end{array} 21qtδq[w^tbwtnwδbwt0]=21qt[w^tbwt0]δq+qtδq˙21δq[w^tbwtnwδbwt0]=21[w^tbwt0]δq+δq˙2δ˙q=δq[w^tbwtnwδbwt0][w^tbwt0]δq2δ˙q=R([w^tbwtnwδbwt0])δqL([w^tbwt0])δq2δ˙q=([(w^tbwtnwδbwt)(w^tbwtnwδbwt)Tw^tbwtnwδbwt0][(w^tbwt)(w^tbwt)Tw^tbwt0])[2δθ1]2δq˙=[(2w^t2bwtnwδbwt)(nw+δbwt)Tnwδbwt0][2δθ1]
上式左侧可以写成:
2 δ q ˙ = [ δ θ ˙ 0 ] = [ − ( 2 w ^ t − 2 b w t − n w − δ b w t ) ∧ − n w − δ b w t ( n w + δ b w t ) T 0 ] [ δ θ 2 1 ] 2 \dot{\delta \mathbf{q}}=\left[\begin{array}{c} \dot{\delta \theta} \\ 0 \end{array}\right]=\left[\begin{array}{cc} -\left(2 \hat{w}_{t}-2 b_{w_{t}}-n_{w}-\delta b_{w_{t}}\right)^{\wedge} & -n_{w}-\delta b_{w_{t}} \\ \left(n_{w}+\delta b_{w_{t}}\right)^{T} & 0 \end{array}\right]\left[\begin{array}{c} \frac{\delta \theta}{2} \\ 1 \end{array}\right] 2δq˙=[δθ˙0]=[(2w^t2bwtnwδbwt)(nw+δbwt)Tnwδbwt0][2δθ1]
最终可以得到:
δ θ ˙ = − ( 2 w ^ t − 2 b w t − n w − δ b w t ) ∧ δ θ 2 − n w − δ b w t ≈ − ( ω ^ t − b w t ) ∧ δ θ − n w − δ b w t \begin{array}{c} \dot{\delta \theta} &=& -\left(2\hat{w}_t-2b_{w_t}-n_w-\delta b_{w_t}\right)^{\wedge } \frac{\delta \theta}{2}-n_{w}-\delta b_{w_t} \\ & \approx & -\left ( \hat{\omega}_t-b_{w_t} \right ) ^{\wedge }\delta \theta-n_w-\delta b_{w_t} \end{array} δθ˙=(2w^t2bwtnwδbwt)2δθnwδbwt(ω^tbwt)δθnwδbwt

误差的递推方程
[ δ α ˙ t b k δ β ˙ t b k δ θ ˙ t b k δ b ˙ a t δ b ˙ w t ] = [ 0 I 0 0 0 0 0 − R t b k [ a ^ t − b a t ] ∧ − R t b k 0 0 0 − [ w ^ t − b w t ] ∧ 0 − I 0 0 0 0 0 0 0 0 0 0 ] [ δ α t b k δ β t b k δ θ t b k δ b a t δ b w t ] + [ 0 0 0 0 − R t b k 0 0 0 0 − I 0 0 0 0 I 0 0 0 0 I ] [ n a n w n b a n b w e ] = F t δ z t b k + G t n t \begin{array}{l} {\left[\begin{array}{c} \delta \dot{\alpha}_{t}^{b_{k}} \\ \delta \dot{\beta}_{t}^{b_{k}} \\ \delta \dot{\theta}_{t}^{b_{k}} \\ \delta \dot{b}_{a_{t}} \\ \delta \dot{b}_{w_{t}} \end{array}\right] = \left[\begin{array}{ccccc} 0 & \mathbf{I} & 0 & 0 & 0 \\ 0 & 0 & -\mathbf{R}_{t}^{b_{k}}\left[\hat{a}_{t}-b_{a_{t}}\right]^{\wedge} & -\mathbf{R}_{t}^{b_{k}} & 0 \\ 0 & 0 & -\left[\hat{w}_{t}-b_{w_{t}}\right]^{\wedge} & 0 & -\mathbf{I} \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \end{array}\right]\left[\begin{array}{c} \delta \alpha_{t}^{b_{k}} \\ \delta \beta_{t}^{b_{k}} \\ \delta \theta_{t}^{b_{k}} \\ \delta b_{a_{t}} \\ \delta b_{w_{t}} \end{array}\right]+\left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ -\mathbf{R}_{t}^{b_{k}} & 0 & 0 & 0 \\ 0 & -\mathbf{I} & 0 & 0 \\ 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & \mathbf{I} \end{array}\right]\left[\begin{array}{c} \mathbf{n}_{a} \\ \mathbf{n}_{w} \\ \mathbf{n}_{b_{a}} \\ \mathbf{n}_{b_{w e}} \end{array}\right]} \\ = \mathbf{F}_{t} \delta \mathbf{z}_{t}^{b_{k}}+\mathbf{G}_{t} \mathbf{n}_{t} \\ \end{array} δα˙tbkδβ˙tbkδθ˙tbkδb˙atδb˙wt = 00000I00000Rtbk[a^tbat][w^tbwt]000Rtbk00000I00 δαtbkδβtbkδθtbkδbatδbwt + 0Rtbk00000I00000I00000I nanwnbanbwe =Ftδztbk+Gtnt

  • 其中:
    • F t 15 × 15 F_{t}^{15 \times 15} Ft15×15
    • G t 15 × 12 G_{t}^{15 \times 12} Gt15×12
    • δ z t b k 15 × 1 \delta z_{t}^{b_{k} 15 \times 1} δztbk15×1
    • n t 12 × 1 n_{t}^{12 \times 1} nt12×1

4.协方差与雅可比

根据公式:
δ z ˙ t b k = F t δ z t b k + G t n t \delta \dot{z}_t^{b_{k}}=F_{t}\delta z_{t}^{b_{k}}+G_{t}n_{t} δz˙tbk=Ftδztbk+Gtnt
可得到:
δ z t + δ t b k = δ z t b k + δ z ˙ t b k δ t = ( I + F t δ t ) δ z t b k + ( G t δ t ) n t = F δ z t b k + V n t \begin{align} \delta z_{t+\delta t}^{b_{k}} & = \delta z_{t}^{b_{k}}+\delta \dot{z}_{t}^{b_{k}}\delta t \\ & = \left ( I+F_{t}\delta t \right )\delta z_{t}^{b_{k}}+\left ( G_{t}\delta t \right ) n_{t} \\ & = F\delta z_{t}^{b_{k}}+Vn_{t} \end{align} δzt+δtbk=δztbk+δz˙tbkδt=(I+Ftδt)δztbk+(Gtδt)nt=Fδztbk+Vnt
其中, F = I + F t δ t F = I+F_{t}\delta t F=I+Ftδt V = G t δ t V=G_{t}\delta t V=Gtδt
因此,根据当前时刻的值,预测下一时刻的协方差为:
P t + δ t b k = ( I + F t δ t ) P t b k ( I + F t δ t ) T + ( G t δ t ) Q t ( G t δ t ) T , t ∈ [ k , k + 1 ] P_{t+\delta t}^{b_{k}}=\left(I+F_{t}\delta t\right)P_{t}^{b_{k}}\left(I+F_{t}\delta t\right)^T + \left ( G_{t}\delta t \right ) Q_{t}\left ( G_{t}\delta t \right )^T,t\in \left [ k,k+1 \right ] Pt+δtbk=(I+Ftδt)Ptbk(I+Ftδt)T+(Gtδt)Qt(Gtδt)T,t[k,k+1]
其中,
Q 12 × 12 = [ δ a 2 0 0 0 0 δ w 2 0 0 0 0 δ b a 2 0 0 0 0 δ b w 2 ] Q^{12 \times 12}=\begin{bmatrix} \delta_{a}^2 & 0 & 0 & 0\\ 0 & \delta_{w}^2 & 0 & 0\\ 0 & 0 & \delta_{b_a}^2 & 0\\ 0 & 0 & 0 & \delta_{b_w}^2 \end{bmatrix} Q12×12= δa20000δw20000δba20000δbw2
雅可比矩阵更新公式为:
J t + δ t = ( I + F t δ t ) J t J_{t+\delta t}=\left(I+F_{t}\delta t\right)J_{t} Jt+δt=(I+Ftδt)Jt
其中,jacobian的初值为I

你可能感兴趣的:(VINS,SLAM,自动驾驶,算法,机器人)