IMU残差函数及雅可比公式推导（二）

IMU状态预测与雅可比矩阵推导

最新推荐文章于 2025-05-19 08:22:11 发布

原创

最新推荐文章于 2025-05-19 08:22:11 发布 · 962 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#VINS

根据IMU残差函数及雅可比公式推导（一）已知：
$αbibk+1=αbibk+βbibkδt+12aδt2qbibk+1=qbibk⊗[112ωδt]βbibk+1=βbibk+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}$

$bk+1a=bka+nbkaδtbk+1g=bkg+nbkgδtb^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$

$ω=12[(ω~kbk−bkg+nkg)+(ω~k+1bk+1−bkg+nk+1g)]a=12[qbibk(a~kbk−bka+nka)+qbibk+1(a~k+1bk+1−bka+nk+1a)]\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}$

求
$F=∂[αbibk+1′,θbibk+1′,βbibk+1′,bbk+1a,bbk+1g]T∂[δαbkbk′,δθbkbk′,δβbkbk′,δbbka,δbbkg]T=[If12f13f14f150f2200f250f32If34f35000I00000I]\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.1.x

$αbibk+1=αbibk+βbibkδt+12aδt2a=12[qbibk(a~kbk−bka+nka)+qbibk+1(a~k+1bk+1−bka+nk+1a)]\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}$

求F.1.2：

【注】：该项的求解和 $f_{32}$ 几乎类似，在最终的结果乘上 $12δt\frac{1}{2}\delta t$ 即可。

故，最终的结果：
$∂αbibk+1∂δθbkbk′=−14Rbibk[(a~kbk−bka+nka)]×δt2−14Rbibk+1[(a~kbk−bka+nka)]×(I−[ωδt]×)δt2\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}$

最低0.47元/天解锁文章