根据IMU残差函数及雅可比公式推导（一）已知：
α b i b k + 1 = α b i b k + β b i b k δ t + 1 2 a δ t 2 q b i b k + 1 = q b i b k ⊗ [ 1 1 2 ω δ t ] β b i b k + 1 = β b i b k + a δ t \begin{aligned} \alpha_{b_ib_{k+1}} &= \alpha_{b_ib_{k}} + \beta_{b_ib_k}\delta t +\frac{1}{2}a\delta t^2 \\ q_{b_ib_{k+1}} &= q_{b_ib_{k}}\otimes \begin{bmatrix} 1 \\ \frac{1}{2}\omega \delta t \end{bmatrix} \\ \beta_{b_ib_{k+1}} &= \beta_{b_ib_{k}} +a\delta t \end{aligned} αbibk+1qbibk+1βbibk+1=αbibk+βbibkδt+21aδt2=qbibk⊗[121ωδt]=βbibk+aδt
b k + 1 a = b k a + n b k a δ t b k + 1 g = b k g + n b k g δ t b^a_{k+1}=b^a_k+n_{b^a_k}\delta t \\ b^g_{k+1}=b^g_k+n_{b^g_k}\delta t bk+1a=bka+nbkaδtbk+1g=bkg+nbkgδt
ω = 1 2 [ ( ω ~ k b k − b k g + n k g ) + ( ω ~ k + 1 b k + 1 − b k g + n k + 1 g ) ] a = 1 2 [ q b i b k ( a ~ k b k − b k a + n k a ) + q b i b k + 1 ( a ~ k + 1 b k + 1 − b k a + n k + 1 a ) ] \begin{aligned} \omega &= \frac{1}{2}[(\tilde{\omega}^{b_k}_k -b^g_k +n^g_k)+(\tilde{\omega}^{b_{k+1}}_{k+1}-b^g_{k} +n^g_{k+1})]\\ a &=\frac{1}{2}[q_{b_ib_k} (\tilde{a}^{b_k}_k - b^a_k+n^a_k)+q_{b_ib_{k+1}} (\tilde{a}^{b_{k+1}}_{k+1}- b^a_{k}+n^a_{k+1})] \end{aligned} ωa=21[(ω~kbk−bkg+nkg)+(ω~k+1bk+1−bkg+nk+1g)]=21[qbibk(a~kbk−bka+nka)+qbibk+1(a~k+1bk+1−bka+nk+1a)]
求
F = ∂ [ α b i b k + 1 ′ , θ b i b k + 1 ′ , β b i b k + 1 ′ , b b k + 1 a , b b k + 1 g ] T ∂ [ δ α b k b k ′ , δ θ b k b k ′ , δ β b k b k ′ , δ b b k a , δ b b k g ] T = [ I f 12 f 13 f 14 f 15 0 f 22 0 0 f 25 0 f 32 I f 34 f 35 0 0 0 I 0 0 0 0 0 I ] \begin{aligned} F &= \frac{ \partial [\alpha_{b_{i}b'_{k+1}},\theta_ {b_{i}b'_{k+1}},\beta_{b_{i}b'_{k+1}},b^a_{b_{k+1}},b^g_{b_{k+1}}]^T} { \partial [\delta \alpha_{b_{k}b'_{k}},\delta \theta_ {b_{k}b'_{k}},\delta \beta_{b_{k}b'_{k}},\delta b^a_{b_{k}},\delta b^g_{b_{k}}]^T} \\ &=\begin{bmatrix} I & f_{12} & f_{13} & f_{14} & f_{15} \\ 0 & f_{22} & 0 & 0 & f_{25} \\ 0 & f_{32} & I & f_{34} & f_{35} \\ 0 & 0 & 0 & I & 0\\ 0 & 0 & 0 & 0 & I\\ \end{bmatrix} \end{aligned} F=∂[δαbkbk′,δθbkbk′,δβbkbk′,δbbka,δbbkg]T∂[αbibk+1′,θbibk+1′,βbibk+1′,bbk+1a,bbk+1g]T=⎣⎢⎢⎢⎢⎡I0000f12f22f3200f130I00f140f34I0f15f25f350I⎦⎥⎥⎥⎥⎤
求F.1.x
α b i b k + 1 = α b i b k + β b i b k δ t + 1 2 a δ t 2 a = 1 2 [ q b i b k ( a ~ k b k − b k a + n k a ) + q b i b k + 1 ( a ~ k + 1 b k + 1 − b k a + n k + 1 a ) ] \begin{aligned} \alpha_{b_ib_{k+1}} &= \alpha_{b_ib_{k}} + \beta_{b_ib_k}\delta t +\frac{1}{2}a\delta t^2 \\ a &=\frac{1}{2}[q_{b_ib_k} (\tilde{a}^{b_k}_k - b^a_k+n^a_k)+q_{b_ib_{k+1}} (\tilde{a}^{b_{k+1}}_{k+1}- b^a_{k}+n^a_{k+1})] \end{aligned} αbibk+1a=αbibk+βbibkδt+21aδt2=21[qbibk(a~kbk−bka+nka)+qbibk+1(a~k+1bk+1−bka+nk+1a)]
求F.1.2：
【注】：该项的求解和 f 32 f_{32} f32几乎类似，在最终的结果乘上 1 2 δ t \frac{1}{2}\delta t 21δt即可。
故，最终的结果：
∂ α b i b k + 1 ∂ δ θ b k b k ′ = − 1 4 R b i b k [ ( a ~ k b k − b k a + n k a ) ] × δ t 2 − 1 4 R b i b k + 1 [ ( a ~ k b k − b k a + n k a ) ] × ( I − [ ω δ t ] × ) δ t 2 \begin{aligned} \frac{\partial \alpha_{b_ib_{k+1}} }{\partial \delta\theta_{b_kb'_k} } &=- \frac{1}{4}R_{b_ib_k}[(\tilde{a}^{b_k}_k - b^a_k+n^a_k) ]_\times \delta t^2-\frac{1}{4}R_{b_ib_{k+1}} [(\tilde{a}^{b_k}_k - b^a_k+n^a_k)]_\times (I-[\omega\delta t]_\times) \delta t^2 \end{aligned} ∂δθbkbk′∂αbibk+1=−41Rbibk[(a~kbk−bka+nka)]×δt2−41Rbibk+1[(a~kbk−bka+nka)]×(I−[ωδ
最低0.47元/天 解锁文章
扫一扫
2388
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
