[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,x∈N(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=Fk−1ηik−1+Gk−1nk−1
其中：
- η 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=Fk−1Σik−1Fk−1T+Gk−1ΣnGk−1T
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(xk−1,uk−1)
其状态的误差的线性递推关系为：
δ x k = F δ x k − 1 + G n k − 1 \delta x_{k} =F \delta x_{k-1}+Gn_{k-1} δxk=Fδxk−1+Gnk−1
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=δxk−1+δxk−1˙Δ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)δk−1+BΔtnk−1
两种推导得到：
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^t−b^at−na)γ^˙tbk=21Ω(ωt−b^ωt−nω)γ^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^t−bat)γ˙tbk=21Ω(ωt−bω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˙at−0=nba=b˙wt−0=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^t−b^at−na)=Rtbkexp([δθ]×)(a^t−na−bat−δbat)=Rtbk(1+[δθ]×)(a^t−na−bat−δbat)=Rtbk(a^t−na−bat−δbat+[δθ]×(a^t−bat))=Rtbk(a^t−na−bat−δbat−[a^t−bat]×δθ)
而标称状态为：
β ˙ 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^t−bat)
因此：
δ β ˙ 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^t−bat]×δθ−Rtbkδbat−Rtbkna
对于 δ θ ˙ 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[−w∧−wTw0]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^−bwt−nw−δ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^t−bwt0]
根据导数的性质，可以得到：
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^t−bwt0]⊗δ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^t−bwt−nw−δbwt0]=21qt⊗[w^t−bwt0]⊗δq+qt⊗δq˙⇔21δq⊗[w^t−bwt−nw−δbwt0]=21[w^t−bwt0]⊗δq+δq˙⇔2δ˙q=δq⊗[w^t−bwt−nw−δbwt0]−[w^t−bwt0]⊗δq⇔2δ˙q=R([w^t−bwt−nw−δbwt0])δq−L([w^t−bwt0])δq⇔2δ˙q=([−(w^t−bwt−nw−δbwt)∧−(w^t−bwt−nw−δbwt)Tw^t−bwt−nw−δbwt0]−[(w^t−bwt)∧−(w^t−bwt)Tw^t−bwt0])[2δθ1]⇔2δq˙=[−(2w^t−2bwt−nw−δbwt)∧(nw+δbwt)T−nw−δ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^t−2bwt−nw−δbwt)∧(nw+δbwt)T−nw−δ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^t−2bwt−nw−δbwt)∧2δθ−nw−δbwt−(ω^t−bwt)∧δθ−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 = 00000I00000−Rtbk[a^t−bat]∧−[w^t−bwt]∧000−Rtbk00000−I00 δαtbkδβtbkδθtbkδbatδbwt + 0−Rtbk00000−I00000I00000I 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