VINS 预积分 雅各比矩阵和协方差矩阵推导过程
根据中值积分
ω k + 1 = ω k + 1 + ω k 2 − b ω k + 1 \omega_{k+1} = \frac{\omega_{k+1} + \omega_k}{2} - b_{\omega_{k+1}} ωk+1=2ωk+1+ωk−bωk+1
q k + 1 = q k ⊗ [ 1 1 2 ω k δ t ] q_{k+1} = q_k \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \omega_k \delta t \end{bmatrix} qk+1=qk⊗[121ωkδt]
a k + 1 = q k ( a k − b a k + n a 0 ) + q k + 1 ( a k + 1 − b a k + 1 + n a 1 ) 2 a_{k+1} = \frac{q_k\left(a_k - b_{a_k} + n_{a_0}\right) + q_{k+1} \left(a_{k+1} - b_{a_{k+1}} + n_{a_1}\right)}{2} ak+1=2qk(ak−bak+na0)+qk+1(ak+1−bak+1+na1)
旋转角度误差推导过程
在误差状态方程式最重要的部分是对 δ θ k + 1 \delta \theta_{k+1} δθk+1部分的推导。
由泰勒公式可得：
δ θ k + 1 ≈ δ θ k + δ θ k ˙ δ t \delta \theta_{k+1} \approx \delta \theta_k + \dot{\delta \theta_k} \delta t δθk+1≈δθk+δθk˙δt
四元数导数的定义和一般形式定义分别为：
q t ˙ = 1 2 q t ⊗ ω t \dot{q_t} = \frac{1}{2} q_t \otimes \omega_t qt˙=21qt⊗ωt
q ˙ = 1 2 q ⊗ ω \dot{q} = \frac{1}{2} q \otimes \omega q˙=21q⊗ω
为了清晰起见，将大信号项和小信号项按角速率分组：
ω ≜ ω m − ω b \omega \triangleq \omega_m - \omega_b ω≜ωm−ωb
δ ω ≜ − δ ω b − ω n \delta \omega \triangleq -\delta \omega_b - \omega_n δω≜−δωb−ωn
注： ω m \omega_m ωm为观测到的角速度值， ω b \omega_b ωb角速度的偏置， ω n \omega_n ωn 角速度的噪声。
对于 ω t \omega_t ωt可以写成两部分：
ω t = ω + δ ω = ω m − ω b − δ ω b − ω n \omega_t = \omega + \delta \omega = \omega_m - \omega_b - \delta \omega_b - \omega_n ωt=ω+δω=ωm−ωb−δωb−ωn
计算 q t ˙ \dot{q_t} qt˙通过左展开和右展开两种不同的方法：
q t ˙ = ( q t − 1 ⊗ δ q ) ˙ = 1 2 q t ⊗ ω t \dot{q_t} = \dot{\left(q_{t-1} \otimes \delta q \right)} = \frac{1}{2}q_t \otimes \omega_t qt˙=(qt−1⊗δq)˙=21qt⊗ωt
q t ˙ = q t − 1 ˙ ⊗ δ q + q t − 1 ⊗ δ q ˙ = 1 2 q t − 1 ⊗ δ q ⊗ ω t \dot{q_t} = \dot{q_{t-1}} \otimes \delta q + q_{t-1} \otimes \dot{\delta q} = \frac{1}{2} q_{t-1} \otimes \delta q \otimes \omega_t qt˙=qt−1˙⊗δq+qt−1⊗δq˙=21qt−1⊗δq⊗ωt
⇒ 1 2 q t − 1 ⊗ ω t − 1 ⊗ δ q + q t − 1 ⊗ δ q ˙ = 1 2 q t − 1 ⊗ δ q ⊗ ω t \Rightarrow \frac{1}{2} q_{t-1} \otimes \omega_{t-1} \otimes \delta q + q_{t-1} \otimes \dot{\delta q} = \frac{1}{2} q_{t-1}\otimes \delta q \otimes \omega_t ⇒21qt−1⊗ωt−1⊗δq+qt−1⊗δq˙=21qt−1⊗δq⊗ωt
⇒ 2 δ q ˙ = δ q ⊗ ω t − ω t − 1 ⊗ δ q \Rightarrow 2 \dot{\delta q} = \delta q \otimes \omega_t - \omega_{t-1}\otimes \delta q ⇒2δq˙=δq⊗ωt−ωt−1⊗δq
2 δ q ≈ [ 1 δ θ ] 2\delta q \approx \begin{bmatrix} 1 \\ \delta \theta \end{bmatrix} 2δq≈[1δθ]对该公式进行求导得
[ 0 δ θ ˙ ] = 2 δ q ˙ \begin{bmatrix} 0 \\ \dot{\delta \theta} \end{bmatrix} = 2 \dot{\delta q} [0δθ˙]=2δq˙
[ 0 δ θ t ˙ ] = 2 δ q ˙ = δ q ⊗ ω t − ω t − 1 ⊗ δ q \begin{bmatrix} 0 \\ \dot{\delta \theta_t} \end{bmatrix} = 2 \dot{\delta q} = \delta q \otimes \omega_t - \omega_{t-1} \otimes \delta q [0δθt˙]=2δq˙=δq⊗ωt−ωt−1⊗δq
= [ q ] R ( ω t ) δ q − [ q ] L ( ω t − 1 ) δ q =\left[q\right]_R\left(\omega_t\right) \delta q - \left[q\right]_L\left(\omega_{t-1}\right)\delta q =[q]R(ωt)δq−[q]L(ωt−1)δq
= [ 0 − ( ω t − ω t − 1 ) T ( ω t − ω t − 1 ) − [ ω t + ω t − 1 ] × ] [ 1 1 2 δ θ t ] =\begin{bmatrix} 0 & -\left(\omega_t - \omega_{t-1}\right)^T \\ \left(\omega_t - \omega_{t-1}\right) & -\left[\omega_t + \omega_{t-1}\right]_{\times} \end{bmatrix}\begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_t \end{bmatrix} =[0(ωt−ωt−1)−(ωt−ωt−1)T−[ωt+ωt−1]×][121δθt]
= [ 0 − δ ω T δ ω − [ 2 ω t + δ ω ] × ] [ 1 1 2 δ θ t ] =\begin{bmatrix} 0 & -{\delta \omega}^T \\ \delta \omega & -\left[2 \omega_t + \delta \omega\right]_{\times} \end{bmatrix}\begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_t \end{bmatrix} =[0δω−δωT−[2ωt+δω]×][121δθt]
根据上式整理得：
δ θ t ˙ = δ ω − [ ω t ] × δ θ t − 1 2 [ δ ω ] × δ θ t ≈ − [ ω t ] × δ θ t + δ ω \dot{\delta \theta_t} = \delta \omega - \left[\omega_t\right]_{\times} \delta \theta_t - \frac{1}{2}\left[\delta \omega \right]_{\times}\delta \theta_t\approx-\left[\omega_t\right]_{\times}\delta \theta_t + \delta \omega δθt˙=δω−[ωt]×δθt−21[δω]×δθt≈−[ωt]×δθt+δω
⇒ δ θ t ˙ = − [ ω t ] × δ θ t + δ ω \Rightarrow\dot{\delta \theta_t} = -\left[\omega_t\right]_{\times}\delta \theta_t + \delta \omega ⇒δθt˙=−[ωt]×δθt+δω
⇒ δ θ ˙ = − [ ω m − ω b ] × δ θ − δ ω b − ω n \Rightarrow \dot{\delta \theta}=-\left[\omega_m - \omega_b\right]_{\times}\delta \theta - \delta \omega_b - \omega_n ⇒δθ˙=−[ωm−ωb]×δθ−δωb−ωn
⇒ δ θ k ˙ = − [ ω k + 1 + ω k 2 − b g k ] × δ θ k − δ b g k + n ω 0 + n ω 1 2 \Rightarrow \dot{\delta \theta_k}= -\left[\frac{\omega_{k+1} + \omega_k }{2}-b_{g_k}\right]_{\times}\delta \theta_k - \delta b_{g_k} + \frac{n_{\omega_0} + n_{\omega_1}}{2} ⇒δθk˙=−[2ωk+1+ωk−bgk]×δθk−δbgk+2nω0+nω1
将上式代入 δ θ k + 1 ≈ δ θ k + δ θ k ˙ δ t \delta \theta_{k+1} \approx \delta \theta_k + \dot{\delta \theta_k} \delta t δθk+1≈δθk+δθk˙δt得：
δ θ k + 1 = ( I − [ ω k + 1 + ω k 2 − b g k ] × δ t ) δ θ k − δ b g k δ t + n ω 0 + n ω 1 2 δ t \delta \theta_{k+1} = \left(I - \left[\frac{\omega_{k+1} + \omega_k}{2} - b_{g_k}\right]_{\times}\delta t\right)\delta \theta_k - \delta b_{g_k}\delta t + \frac{n_{\omega_0} + n_{\omega_1}}{2}\delta t δθk+1=(I−[2ωk+1+ωk−bgk]×δt)δθk−δbgkδt+2nω0+nω1δt
速度误差推导过程
δ β k + 1 ≈ δ β k + a k + 1 δ t = δ β k + q k ( a k − b a k + n a 0 ) + q k + 1 ( a k + 1 − b a k + 1 + n a 1 ) 2 δ t \delta \beta_{k+1} \approx \delta \beta_k + a_{k+1}\delta t = \delta \beta_k + \frac{q_k\left(a_k -b_{a_k} + n_{a_0}\right) + q_{k+1}\left(a_{k+1} - b_{a_{k+1}} + n_{a_1}\right)}{2}\delta t δβk+1≈δβk+ak+1δt=δβk+2qk(ak−bak+na0)+qk+1(ak+1−bak+1+na1)δt
即：
δ β k + 1 ≈ δ β k + δ β ˙ δ t \delta \beta_{k+1} \approx \delta \beta_k + \dot{\delta \beta}\delta t δβk+1≈δβk+δβ˙δt
已知有如下关系成立：
R k = R ( I + [ δ θ ] × ) + O ( ∣ ∣ δ θ ∣ ∣ 2 ) R_k = R\left(I + \left[\delta \theta\right]_{\times}\right) + O\left({||\delta \theta||}^2\right) Rk=R(I+[δθ]×)+O(∣∣δθ∣∣2)
δ β ˙ = v ˙ = R a B + g \dot{\delta \beta} = \dot{v} = R a_B + g δβ˙=v˙=RaB+g
对于a（加速度）可以写成以下两部分：
a B ≜ a m − b a a_B \triangleq a_m - b_a aB≜am−ba
δ a B ≜ − δ b a − n a \delta a_B \triangleq -\delta b_a - n_a δaB≜−δba−na
在惯性系中可以把加速度写成两部分的组合：
a k = R k ( a B + δ a B ) + g a_k = R_k\left(a_B + \delta a_B\right) + g ak=Rk(aB+δaB)+g
v k ˙ = v ˙ + δ v ˙ = R ( I + [ δ θ ] × ) ( a B + δ a B ) + g \dot{v_k}=\dot{v} + \dot{\delta v}=R\left(I + \left[\delta \theta\right]_{\times}\right)\left(a_B + \delta a_B\right)+g vk˙=v˙+δv˙=R(I+[δθ]×)(aB+δaB)+g
⇒ v k ˙ = R a B + g + δ v ˙ = R a B + R δ a B + R [ δ θ ] × a B + R [ δ θ ] × δ a B + g \Rightarrow \dot{v_k}=Ra_B + g + \dot{\delta v} = Ra_B + R\delta a_B+R\left[\delta \theta\right]_{\times}a_B + R\left[\delta \theta\right]_{\times}\delta a_B + g ⇒vk˙=RaB+g+δv˙=RaB+RδaB+R[δθ]×aB+R[δθ]×δaB+g
⇒ δ v ˙ = R ( δ a B + [ δ θ ] × a B ) + R [ δ θ ] × δ a B \Rightarrow\dot{\delta v}=R\left(\delta a_B + \left[\delta \theta\right]_{\times}a_B\right)+R\left[\delta \theta\right]_{\times}\delta a_B ⇒δv˙=R(δaB+[δθ]×aB)+R[δθ]×δaB
消除二阶项，并重新组织叉乘（ [ a ] × b = − [ b ] × a \left[a\right]_{\times}b = -\left[b\right]_{\times}a [a]×b=−[b]×a）：
δ v ˙ = R ( δ a B − [ a B ] × δ θ ) \dot{\delta v} = R\left(\delta a_B - \left[a_B\right]_{\times}\delta\theta\right) δv˙=R(δaB−[aB]×δθ)
把已知的公式代入上式中得到：
δ v ˙ = R ( − [ a m − b a ] × δ θ − δ b a − n a ) = − R [ a m − b a ] × δ θ − R δ b a − R n a \dot{\delta v} = R\left(-\left[a_m - b_a\right]_{\times}\delta \theta - \delta b_a - n_a\right)=-R\left[a_m - b_a\right]_{\times}\delta \theta - R\delta b_a - R n_a δv˙=R(−[am−ba]×δθ−δba−na)=−R[am−ba]×δθ−Rδba−Rna
为了简化表达，通常假设加速度计噪声是白色的，不相关的，各向同性的
E [ n a ] = 0 E\left[n_a\right] = 0 E[na]=0
E [ n a n a T ] = σ a 2 I E\left[n_a{n_a}^T\right]={\sigma_a}^2I E[nanaT]=σa2I
协方差椭球是以原点为中心的球面，意味着其均值和协方差在旋转时是不变的。E [ R n a ] = R E [ n a ] = 0 E\left[R n_a\right]=RE\left[n_a\right] = 0 E[Rna]=RE[na]=0
E [ ( R n a ) ( R n a ) T ] = R E [ n a n a T ] R T = R σ a 2 I R T = σ a 2 I E\left[\left(R n_a\right)\left(R n_a\right)^T\right]=RE\left[n_a {n_a}^T\right]R^T = R {\sigma_a}^2IR^T = {\sigma_a}^2I E[(Rna)(Rna)T]=RE[nanaT]RT=Rσa2IRT=σa2I
重新定义加速度计的噪声：
R n a → n a Rn_a \to n_a Rna→na
则有：
δ v ˙ = R ( − [ a m − b a ] × δ θ − δ b a − n a ) = − R [ a m − b a ] × δ θ − R δ b a − n a \dot{\delta v} = R\left(-\left[a_m - b_a\right]_{\times}\delta \theta - \delta b_a - n_a\right)=-R\left[a_m - b_a\right]_{\times}\delta \theta - R\delta b_a - n_a δv˙=R(−[am−ba]×δθ−δba−na)=−R[am−ba]×δθ−Rδba−na
δ β k ˙ = − 1 2 q k [ a k − b a k ] × δ θ k − 1 2 q k + 1 [ a k + 1 − b a k + 1 ] × δ θ k + 1 \dot{\delta \beta_k}=-\frac{1}{2}q_k\left[a_k-b_{a_k}\right]_{\times}\delta\theta_k - \frac{1}{2}q_{k+1}\left[a_{k+1} - b_{a_{k+1}}\right]_{\times}\delta\theta_{k+1} δβk˙=−21qk[ak−bak]×δθk−21qk+1[ak+1−bak+1]×δθk+1
− 1 2 ( q k + q k + 1 ) δ b a k − 1 2 ( q k n a 0 + q k + 1 n a 1 ) - \frac{1}{2}\left(q_k + q_{k+1}\right)\delta b_{a_k}-\frac{1}{2}\left(q_k n_{a_0} + q_{k+1} n_{a_1}\right) −21(qk+qk+1)δbak−21(qkna0+qk+1na1)
把 δ θ k + 1 \delta \theta_{k+1} δθk+1代入上式得：
δ β k ˙ = − 1 2 q k [ a k − b a k ] × δ θ k − 1 2 q k + 1 [ a k + 1 − b a k + 1 ] × \dot{\delta \beta_k} = -\frac{1}{2}q_k\left[a_k-b_{a_k}\right]_{\times}\delta\theta_k - \frac{1}{2}q_{k+1}\left[a_{k+1} - b_{a_{k+1}}\right]_{\times} δβk˙=−21qk[ak−bak]×δθk−21qk+1[ak+1−bak+1]× ( ( I − [ ω k + 1 + ω k 2 − b g k ] × δ t ) δ θ k − δ b g k δ t + n ω 0 + n ω 1 2 δ t ) \left(\left(I - \left[\frac{\omega_{k+1} + \omega_k}{2} - b_{g_k}\right]_{\times}\delta t\right)\delta \theta_k - \delta b_{g_k}\delta t + \frac{n_{\omega_0} + n_{\omega_1}}{2}\delta t\right) ((I−[2ωk+1+ωk−bgk]×δt)δθk−δbgkδt+2nω0+nω1δt)
− 1 2 ( q k + q k + 1 ) δ b a k − 1 2 ( q k n a 0 + q k + 1 n a 1 ) - \frac{1}{2}\left(q_k + q_{k+1}\right)\delta b_{a_k}-\frac{1}{2}\left(q_k n_{a_0} + q_{k+1} n_{a_1}\right) −21(qk+qk+1)δbak−21(qkna0+qk+1na1)
把上式代入
δ β k + 1 ≈ δ β k + δ β k ˙ δ t \delta \beta_{k+1} \approx \delta \beta_k + \dot{\delta \beta_k}\delta t δβk+1≈δβk+δβk˙δt
位置误差推导过程
δ α k + 1 = δ α k + δ α k ˙ δ t \delta \alpha_{k+1} = \delta \alpha_k + \dot{\delta \alpha_k}\delta t δαk+1=δαk+δαk˙δt
δ β ˙ δ t \dot{\delta \beta}\delta t δβ˙δt表示的是速度的增量，那么在该速度的增量下的位置的增量为：
δ a k ˙ = 1 2 δ β k ˙ δ t \dot{\delta a_k} = \frac{1}{2}\dot{\delta \beta_k}\delta t δak˙=21δβk˙δt
= 1 4 q k [ a k − b a k ] × δ θ δ t =\frac{1}{4}q_k\left[a_k - b_{a_k}\right]_{\times}\delta \theta \delta t =41qk[ak−bak]×δθδt
− 1 4 q k + 1 [ a k + 1 − b a k + 1 ] × ( ( I − [ ω k + 1 + ω k 2 ] × δ t ) δ θ k − δ b g k δ t + n w 0 + n w 1 2 δ t ) δ t -\frac{1}{4}q_{k+1}\left[a_{k+1}-b_{a_{k+1}}\right]_{\times}\left(\left(I-\left[\frac{\omega_{k+1} + \omega_k}{2}\right]_{\times}\delta t\right)\delta \theta_k-\delta b_{g_k}\delta t + \frac{n_{w_0} + n_{w_1}}{2}\delta t\right)\delta t −41qk+1[ak+1−bak+1]×((I−[2ωk+1+ωk]×δt)δθk−δbgkδt+2nw0+nw1δt)δt
− 1 4 ( q k + q k + 1 ) δ b a k δ t − 1 4 ( q k n 0 + q k + 1 n a 1 ) δ t -\frac{1}{4}\left(q_k + q_{k+1}\right)\delta b_{a_k} \delta t - \frac{1}{4}\left(q_k n_0 + q_{k+1} n_{a_1}\right)\delta t −41(qk+qk+1)δbakδt−41(qkn0+qk+1na1)δt
根据上式可以得到： δ α k + 1 \delta \alpha_{k+1} δαk+1
根据以上的计算结果整理成矩阵形式
[ δ α k + 1 δ θ k + 1 δ β k + 1 δ b a k + 1 δ b g k + 1 ] = [ I f 01 δ t − 1 4 ( q k + q k + 1 ) δ t 2 f 04 0 I − [ ω k + 1 + ω k 2 − b w k ] × δ t 0 0 − δ t 0 f 21 I − 1 2 ( q k + q k + 1 ) δ t f 24 0 0 0 I 0 0 0 0 0 I ] [ δ α k δ θ k δ β k δ b a k δ b g k ] \begin{bmatrix} \delta \alpha_{k+1} \\ \delta \theta_{k+1} \\ \delta \beta_{k+1} \\ \delta b_{a_{k+1}} \\ \delta b_{g_{k+1}} \end{bmatrix}= \begin{bmatrix} I & f_{01} & \delta t & -\frac{1}{4}\left(q_k + q_{k+1}\right){\delta t}^2 & f_{04} \\ 0 & I - \left[\frac{\omega_{k+1} + \omega_k}{2} - b_{w_k}\right]_{\times}\delta t & 0 & 0 & -\delta t \\ 0 & f_{21} & I & -\frac{1}{2}\left(q_k + q_{k+1}\right)\delta t & f_{24} \\ 0 & 0 & 0 & I & 0 \\ 0 & 0 & 0 & 0 & I \end{bmatrix} \begin{bmatrix} \delta \alpha_k \\ \delta \theta_k \\ \delta \beta_k \\ \delta b_{a_k} \\ \delta b_{g_k} \end{bmatrix} ⎣⎢⎢⎢⎢⎡δαk+1δθk+1δβk+1δbak+1δbgk+1⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡I0000f01I−[2ωk+1+ωk−bwk]×δtf2100δt0I00−41(qk+qk+1)δt20−21(qk+qk+1)δtI0f04−δtf240I⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎡δαkδθkδβkδbakδbgk⎦⎥⎥⎥⎥⎤
+ [ 1 4 q k δ t 2 v 01 1 4 q k + 1 δ t 2 v 03 0 0 0 1 2 δ t 0 1 2 δ t 0 0 1 2 q k δ t v 21 1 2 q k + 1 δ t v 23 0 0 0 0 0 0 δ t 0 0 0 0 0 0 δ t ] [ n a 0 n w 0 n a 1 n w 1 n b a n b g ] +\begin{bmatrix} \frac{1}{4}q_k{\delta t}^2 & v_{01} & \frac{1}{4}q_{k+1}{\delta t}^2 & v_{03} & 0 & 0 \\ 0 & \frac{1}{2}\delta t & 0 & \frac{1}{2}\delta t & 0 & 0 \\ \frac{1}{2}q_k\delta t & v_{21} & \frac{1}{2}q_{k+1}\delta t & v_{23} & 0 & 0 \\ 0 & 0 & 0 & 0 & \delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & \delta t \end{bmatrix} \begin{bmatrix} n_{a_0} \\ n_{w_0} \\ n_{a_1} \\ n_{w_1} \\ n_{b_a} \\ n_{b_g} \end{bmatrix} +⎣⎢⎢⎢⎢⎡41qkδt2021qkδt00v0121δtv210041qk+1δt2021qk+1δt00v0321δtv2300000δt00000δt⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡na0nw0na1nw1nbanbg⎦⎥⎥⎥⎥⎥⎥⎤
其中：
f 01 = − 1 4 q k [ a k − b a k ] × δ t 2 − 1 4 q k + 1 [ a k + 1 − b a k ] × ( I − [ ω k + ω k + 1 2 − b g k ] × δ t ) δ t 2 f_{01}=-\frac{1}{4}q_k\left[a_k-b_{a_k}\right]_{\times}{\delta t}^2 - \frac{1}{4}q_{k+1}\left[a_{k+1} - b_{a_k}\right]_{\times}\left(I - \left[\frac{\omega_k + \omega_{k+1}}{2} - b_{g_k}\right]_{\times}\delta t\right){\delta t}^2 f01=−41qk[ak−bak]×δt2−41qk+1[ak+1−bak]×(I−[2ωk+ωk+1−bgk]×δt)δt2
f 21 = − 1 2 q k [ a k − b a k ] × δ t − 1 2 q k + 1 [ a k + 1 − b a k ] × ( I − [ ω k + ω k + 1 2 ] × δ t ) δ t f_{21}=-\frac{1}{2}q_k\left[a_k - b_{a_k}\right]_{\times}\delta t - \frac{1}{2}q_{k+1}\left[a_{k+1}-b_{a_k}\right]_{\times}\left(I-\left[\frac{\omega_k + \omega_{k+1}}{2}\right]_{\times}\delta t\right)\delta t f21=−21qk[ak−bak]×δt−21qk+1[ak+1−bak]×(I−[2ωk+ωk+1]×δt)δt
f 04 = 1 4 ( q k + 1 [ a k + 1 − b a k ] × δ t 2 ) δ t f_{04}=\frac{1}{4}\left(q_{k+1}\left[a_{k+1}-b_{a_k}\right]_{\times}{\delta t}^2\right)\delta t f04=41(qk+1[ak+1−bak]×δt2)δt
f 24 = 1 2 ( q k + 1 [ a k + 1 − b a k ] × δ t ) δ t f_{24} = \frac{1}{2}\left(q_{k+1}\left[a_{k+1} - b_{a_k}\right]_{\times}\delta t\right)\delta t f24=21(qk+1[ak+1−bak]×δt)δt
v 01 = − 1 4 ( q k + 1 [ a k + 1 − b a k ] × δ t 2 ) 1 2 δ t v_{01} = -\frac{1}{4}\left(q_{k+1}\left[a_{k+1} - b_{a_k}\right]_{\times}{\delta t}^2\right) \frac{1}{2}\delta t v01=−41(qk+1[ak+1−bak]×δt2)21δt
v 03 = − 1 4 ( q k + 1 [ a k + 1 − b a k ] × δ t 2 ) 1 2 δ t v_{03} = -\frac{1}{4}\left(q_{k+1}\left[a_{k+1} - b_{a_k}\right]_{\times}{\delta t}^2\right) \frac{1}{2}\delta t v03=−41(qk+1[ak+1−bak]×δt2)21δt
v 21 = − 1 2 ( q k + 1 [ a k + 1 − b a k ] × δ t 2 ) 1 2 δ t v_{21}=-\frac{1}{2}\left(q_{k+1}\left[a_{k+1} - b_{a_k}\right]_{\times}{\delta t}^2\right)\frac{1}{2}\delta t v21=−21(qk+1[ak+1−bak]×δt2)21δt
v 23 = − 1 2 ( q k + 1 [ a k + 1 − b a k ] × δ t 2 ) 1 2 δ t v_{23}=-\frac{1}{2}\left(q_{k+1}\left[a_{k+1} - b_{a_k}\right]_{\times}{\delta t}^2\right)\frac{1}{2}\delta t v23=−21(qk+1[ak+1−bak]×δt2)21δt
将上个矩阵简写为：
δ k + 1 = F δ k + V Q \delta_{k+1} = F \delta_k + VQ δk+1=Fδk+VQ
最后得到系统的雅各比矩阵 J k + 1 J_{k+1} Jk+1和协方差矩阵 P k + 1 P_{k+1} Pk+1
初始值为：
J k = I J_k = I Jk=I
P k = 0 P_k = 0 Pk=0
J k + 1 = F J k J_{k+1} = FJ_k Jk+1=FJk
P k + 1 = F P k F T + V Q V T P_{k+1} = FP_kF^T + VQV^T Pk+1=FPkFT+VQVT
290
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
