IMU预积分--对状态变量雅可比推导

平移项对各状态变量雅可比

• $\frac{\partial ({\alpha }_{k+1}^i)}{\partial {\alpha }_k^i}$

  $$\begin{array}{rl}{f}_{\alpha ,\alpha }({\hat{\alpha }}_k^i+\delta {\alpha }_k^i)& =({\hat{\alpha }}_k^i+\delta {\alpha }_k^i)+{\hat{\beta }}_k^i\Delta t+\frac{1}{2}{\hat{\gamma }}_k^i{\hat{a}}_k^b\Delta t^2\\ & =f_{\alpha ,\alpha }({\hat{\alpha }}_k^i)+\delta {\alpha }_k^i\\ \frac{\partial ({\alpha }_{k+1}^i)}{\partial {\alpha }_k^i}& =\underset{\delta {\alpha }_k^i\to 0}{\lim }\frac{f_{\alpha ,\alpha }({\hat{\alpha }}_k^i+\delta {\alpha }_k^i)-f_{\alpha ,\alpha }({\hat{\alpha }}_k^i)}{\delta {\alpha }_k^i}\\ & =\underset{\delta {\alpha }_k^i\to 0}{\lim }\frac{\delta {\alpha }_k^i}{\delta {\alpha }_k^i}\\ & =I_{3\times 3}\end{array}$$

• $\frac{\partial ({\alpha }_{k+1}^i)}{\partial {\theta }_k^i}$

  $$\begin{array}{rl}{f}_{\alpha ,\theta }({\hat{\theta }}_k^i+\delta {\theta }_k^i)& ={\hat{\alpha }}_k^i+{\hat{\beta }}_k^i\Delta t+\frac{1}{2}{\hat{\gamma }}_k^{i}exp([\delta {\theta }_k^i{]}_{\times }){\hat{a}}_k^b\Delta t^2\\ & ={\hat{\alpha }}_k^i+{\hat{\beta }}_k^i\Delta t+\frac{1}{2}{\hat{\gamma }}_k^i(I+[\delta {\theta }_k^i{]}_{\times }){\hat{a}}_k^b\Delta t^2\\ & ={\hat{\alpha }}_k^i+{\hat{\beta }}_k^i\Delta t+\frac{1}{2}{\hat{\gamma }}_k^i{\hat{a}}_k^b\Delta t^2+\frac{1}{2}{\hat{\gamma }}_k^i[\delta {\theta }_k^i{]}_{\times }{\hat{a}}_k^b\Delta t^2\\ & ={\hat{\alpha }}_k^i+{\hat{\beta }}_k^i\Delta t+\frac{1}{2}{\hat{\gamma }}_k^i{\hat{a}}_k^b\Delta t^2-\frac{1}{2}{\hat{\gamma }}_k^i[{\hat{a}}_k^b\Delta t^2{]}_{\times }\delta {\theta }_k^i\\ & =f_{\alpha ,\theta }({\hat{\theta }}_k^i)-\frac{1}{2}{\hat{\gamma }}_k^i[{\hat{a}}_k^b\Delta t^2{]}_{\times }\delta {\theta }_k^i\\ \frac{\partial ({\alpha }_{k+1}^i)}{\partial {\theta }_k^i}& =\underset{\delta {\theta }_k^i\to 0}{\lim }\frac{f_{\alpha ,\theta }({\hat{\theta }}_k^i+\delta {\theta }_k^i)-f_{\alpha ,\theta }({\hat{\theta }}_k^i)}{\delta {\theta }_k^i}\\ & =-\frac{1}{2}{\hat{\gamma }}_k^i[{\hat{a}}_k^b\Delta t^2{]}_{\times }\end{array}$$

• $\frac{\partial ({\alpha }_{k+1}^i)}{\partial {\beta }_k^i}$

  $$\begin{array}{rl}{f}_{\alpha ,\beta }({\hat{\beta }}_k^i+\delta {\beta }_k^i)& ={\hat{\alpha }}_k^i+({\hat{\beta }}_k^i+\delta {\beta }_k^i)\Delta t+\frac{1}{2}{\hat{\gamma }}_k^i{\hat{a}}_k^b\Delta t^2\\ & =f_{\alpha ,\beta }({\hat{\beta }}_k^i)+\delta {\beta }_k^i\Delta t\\ \frac{\partial ({\alpha }_{k+1}^i)}{\partial {\beta }_k^i}& =\underset{\delta {\beta }_k^i\to 0}{\lim }\frac{f_{\alpha ,\beta }({\hat{\beta }}_k^i+\delta {\beta }_k^i)-f_{\alpha ,\beta }({\hat{\beta }}_k^i)}{\delta {\beta }_k^i}\\ & =I\cdot \Delta t\end{array}$$

• $\frac{\partial ({\alpha }_{k+1}^i)}{\partial b_k^a}$

  $$\begin{array}{rl}{a}_k^b& =\frac{1}{2}\left[\left({\tilde{a}}_k^b-({\hat{b}}_k^a+\delta b_k^a)\right)+Exp({\hat{\omega }}_k^b\Delta t)\left({\tilde{a}}_{k+1}^b-({\hat{b}}_k^a+\delta b_k^a)\right)\right]\\ & =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a)+Exp({\hat{\omega }}_k^b\Delta t)({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right]-\frac{1}{2}\left(I+Exp({\hat{\omega }}_k^b\Delta t)\right)\delta b_k^a\\ & ={\hat{a}}_k^b-\frac{1}{2}\left(I+Exp({\hat{\omega }}_k^b\Delta t)\right)\delta b_k^a\\ f_{\alpha ,ba}({\hat{b}}_k^a+\delta b_k^a)& ={\hat{\alpha }}_k^i+{\hat{\beta }}_k^i\Delta t+\frac{1}{2}{\hat{\gamma }}_k^i\left({\hat{a}}_k^b-\frac{1}{2}\left(I+Exp({\hat{\omega }}_k^b\Delta t)\right)\delta b_k^a\right)\Delta t^2\\ & =f_{\alpha ,ba}({\hat{b}}_k^a)-\frac{1}{4}{\hat{\gamma }}_k^i\left(I+Exp({\hat{\omega }}_k^b\Delta t)\right)\delta {\widehat{b^a}}_k\Delta t^2\\ \frac{\partial ({\alpha }_{k+1}^i)}{\partial b_k^a}& =\underset{\delta b_k^a\to 0}{\lim }\frac{f_{\alpha ,ba}({\hat{b}}_k^a+\delta b_k^a)-f_{\alpha ,ba}({\hat{b}}_k^a)}{\delta b_k^a}\\ & =-\frac{1}{4}{\hat{\gamma }}_k^i\left(I+Exp({\hat{\omega }}_k^b\Delta t)\right)\Delta t^2\\ & =-\frac{1}{4}({\hat{\gamma }}_k^i+{\hat{\gamma }}_{k+1}^i)\Delta t^2\end{array}$$

• $\frac{\partial ({\alpha }_{k+1}^i)}{\partial b_k^g}$

  $$\begin{array}{rl}{\omega }_k^b& =\frac{1}{2}[({\tilde{\omega }}_k^b-{\hat{b}}_k^g-\delta b_k^g)+({\tilde{\omega }}_{k+1}^b-{\hat{b}}_k^g-\delta b_k^g)]\\ & ={\hat{\omega }}_k^b-\delta b_k^g\\ a_k^b& =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a)+Exp(({\hat{\omega }}_k^b-\delta b_k^g)\Delta t)({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right]\\ & =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a)+Exp({\hat{\omega }}_k^b\Delta t)Exp(-\underset{I}{\underbrace{J_r({\hat{\omega }}_k^b\Delta t)}}\delta b_k^g\Delta t)({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right]\\ & =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a)+Exp({\hat{\omega }}_k^b\Delta t)(I-[\delta b_k^g\Delta t{]}_{\times })({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right]\\ & =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a)+Exp({\hat{\omega }}_k^b\Delta t)({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right]-\frac{1}{2}Exp({\hat{\omega }}_k^b\Delta t)[\delta b_k^g\Delta t{]}_{\times }({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\\ & =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a)+Exp({\hat{\omega }}_k^b\Delta t)({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right]+\frac{1}{2}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\delta b_k^g\Delta t\\ & ={\hat{a}}_k^b+\frac{1}{2}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\delta b_k^g\Delta t\end{array}$$

  $$\begin{array}{rl}{f}_{\alpha ,bg}({\hat{b}}_k^g+\delta b_k^g)& ={\hat{\alpha }}_k^i+{\hat{\beta }}_k^i\Delta t+\frac{1}{2}{\hat{\gamma }}_k^i\left({\hat{a}}_k^b+\frac{1}{2}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\delta b_k^g\Delta t\right)\Delta t^2\\ & ={\hat{\alpha }}_k^i+{\hat{\beta }}_k^i\Delta t+\frac{1}{2}{\hat{\gamma }}_k^i{\hat{a}}_k^b\Delta t^2+\frac{1}{4}{\gamma }_k^{i}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\delta b_k^g\Delta t^3\\ & ={\hat{\alpha }}_k^i+{\hat{\beta }}_k^i\Delta t+\frac{1}{2}{\hat{\gamma }}_k^i{\hat{a}}_k^b\Delta t^2+\frac{1}{4}{\hat{\gamma }}_{k+1}^i[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\delta b_k^g\Delta t^3\\ \frac{\partial ({\alpha }_{k+1}^i)}{\partial b_k^g}& =\underset{\delta b_k^g\to 0}{\lim }\frac{f_{\alpha ,bg}({\hat{b}}_k^g+\delta b_k^g)-f_{\alpha ,bg}({\hat{b}}_k^g)}{\delta b_k^g}\\ & =\frac{1}{4}{\hat{\gamma }}_{k+1}^i[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\Delta t^3\end{array}$$

角度项对各状态变量雅可比

$$\begin{array}{rl}{\hat{\gamma }}_{k+1}^i& ={\hat{\gamma }}_k^{i}Exp({\hat{\omega }}_k^b\Delta t)\\ {\hat{\gamma }}_{k+1}^i\oplus \delta {\theta }_{k+1}^i& =({\hat{\gamma }}_k^i\oplus \delta {\theta }_k^i)Exp({\hat{\omega }}_k^b\Delta t)\end{array}$$

• $\frac{\partial ({\theta }_{k+1}^i)}{\partial {\theta }_k^i}$

  $$\begin{array}{rl}\frac{\partial ({\theta }_{k+1}^i)}{\partial {\theta }_k^i}& =\underset{\delta {\theta }_k^i\to 0}{\lim }\frac{Exp({\hat{\theta }}_k^i)Exp(\delta {\theta }_k^i)Exp({\hat{\omega }}_k^b\Delta t)\ominus Exp({\hat{\theta }}_k^i)Exp({\hat{\omega }}_k^b\Delta t)}{\delta {\theta }_k^i}\\ & =\underset{\delta {\theta }_k^i\to 0}{\lim }\frac{Log\left(Ex{p}^{-1}({\hat{\theta }}_{k+1}^i)Exp({\hat{\theta }}_k^i)Exp(\delta {\theta }_k^i)Exp({\hat{\omega }}_k^b\Delta t)\right)}{\delta {\theta }_k^i}\\ & =\underset{\delta {\theta }_k^i\to 0}{\lim }\frac{Log\left(Exp(-{\hat{\omega }}_k^b\Delta t)Exp(\delta {\theta }_k^i)Exp({\hat{\omega }}_k^b\Delta t)\right)}{\delta {\theta }_k^i}\\ & =\underset{\delta {\theta }_k^i\to 0}{\lim }\frac{Log\left(Exp(Exp(-{\hat{\omega }}_k^b\Delta t)\delta {\theta }_k^i)\right)}{\delta {\theta }_k^i}\\ & =\underset{\delta {\theta }_k^i\to 0}{\lim }\frac{Exp(-{\hat{\omega }}_k^b\Delta t)\delta {\theta }_k^i}{\delta {\theta }_k^i}\\ & =Exp(-{\hat{\omega }}_k^b\Delta t)\\ & =I-[{\hat{\omega }}_k^b\Delta t{]}_{\times }\end{array}$$

• $\frac{\partial ({\theta }_{k+1}^i)}{\partial b_k^g}$

  $$\begin{array}{rl}\frac{\partial ({\theta }_{k+1}^i)}{\partial b_k^g}& =\underset{\delta b_k^g\to 0}{\lim }\frac{Exp({\hat{\theta }}_k^i)Exp\left(({\hat{\omega }}_k^b-\delta b_k^g)\Delta t\right)\ominus Exp({\hat{\theta }}_k^i)Exp({\hat{\omega }}_k^b\Delta t)}{\delta b_k^g}\\ & =\underset{\delta b_k^g\to 0}{\lim }\frac{Exp({\hat{\theta }}_k^i)Exp({\hat{\omega }}_k^b\Delta t)Exp\left(-J_r({\hat{\omega }}_k^b\Delta t)\delta b_i^g\Delta t\right)\ominus Exp({\hat{\theta }}_k^i)Exp({\hat{\omega }}_k^b\Delta t)}{\delta b_k^g}\\ & =\underset{\delta b_k^g\to 0}{\lim }\frac{Log\left(Exp(-\delta b_i^g\Delta t)\right)}{\delta b_k^g}\\ & =\underset{\delta b_k^g\to 0}{\lim }\frac{-\delta b_i^g\Delta t}{\delta b_k^g}\\ & =-I\cdot \Delta t\end{array}$$

速度项对各状态变量雅可比

$$\begin{array}{rl}{\hat{\beta }}_{k+1}^i& ={\hat{\beta }}_k^i+{\hat{\gamma }}_k^i{\hat{a}}_k^b\Delta t\end{array}$$

• $\frac{\partial ({\beta }_{k+1}^i)}{\partial {\theta }_k^i}$

  $$\begin{array}{rl}{f}_{\beta ,\theta }({\hat{\theta }}_k^i+\delta {\theta }_k^i)& ={\hat{\beta }}_k^i+{\hat{\gamma }}_k^{i}exp([\delta {\theta }_k^i{]}_{\times }){\hat{a}}_k^b\Delta t\\ & ={\hat{\beta }}_k^i+{\hat{\gamma }}_k^i(I+[\delta {\theta }_k^i{]}_{\times }){\hat{a}}_k^b\Delta t\\ & =f_{\beta ,\theta }({\hat{\theta }}_k^i)+{\hat{\gamma }}_k^i[\delta {\theta }_k^i{]}_{\times }{\hat{a}}_k^b\Delta t\\ & =f_{\beta ,\theta }({\hat{\theta }}_k^i)-{\hat{\gamma }}_k^i[{\hat{a}}_k^b\Delta t{]}_{\times }\delta {\theta }_k^i\\ \frac{\partial ({\beta }_{k+1}^i)}{\partial {\theta }_k^i}& =\underset{\delta {\theta }_k^i\to 0}{\lim }\frac{f_{\beta ,\theta }({\hat{\theta }}_k^i+\delta {\theta }_k^i)-f_{\beta ,\theta }({\hat{\theta }}_k^i)}{\delta {\theta }_k^i}\\ & =-{\hat{\gamma }}_k^i[{\hat{a}}_k^b\Delta t{]}_{\times }\end{array}$$

• $\frac{\partial ({\beta }_{k+1}^i)}{\partial {\beta }_k^i}$

  $$\begin{array}{rl}{f}_{\beta ,\beta }({\hat{\beta }}_k^i+\delta {\beta }_k^i)& ={\hat{\beta }}_k^i+{\hat{\gamma }}_k^i{\hat{a}}_k^b\Delta t+\delta {\beta }_k^i\\ & =f_{\beta ,\beta }({\hat{\beta }}_k^i)+\delta {\beta }_k^i\\ \frac{\partial ({\beta }_{k+1}^i)}{\partial {\beta }_k^i}& =\underset{\delta {\beta }_k^i\to 0}{\lim }\frac{f_{\beta ,\beta }({\hat{\beta }}_k^i+\delta {\beta }_k^i)-f_{\beta ,\beta }({\hat{\beta }}_k^i)}{\delta {\beta }_k^i}\\ & =I\end{array}$$

• $\frac{\partial ({\beta }_{k+1}^i)}{\partial b_k^a}$

  $$\begin{array}{rl}{a}_k^b& =\frac{1}{2}\left[\left({\tilde{a}}_k^b-({\hat{b}}_k^a+\delta b_k^a)\right)+Exp({\hat{\omega }}_k^b\Delta t)\left({\tilde{a}}_{k+1}^b-({\hat{b}}_k^a+\delta b_k^a)\right)\right]\\ & =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a)+Exp({\hat{\omega }}_k^b\Delta t)({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right]-\frac{1}{2}\left(I+Exp({\hat{\omega }}_k^b\Delta t)\right)\delta b_k^a\\ & ={\hat{a}}_k^b-\frac{1}{2}\left(I+Exp({\hat{\omega }}_k^b\Delta t)\right)\delta b_k^a\\ f_{\beta ,ba}({\hat{b}}_k^a+\delta b_k^a)& ={\hat{\beta }}_k^i+{\hat{\gamma }}_k^i\left({\hat{a}}_k^b-\frac{1}{2}\left(I+Exp({\hat{\omega }}_k^b\Delta t)\right)\delta b_k^a\right)\Delta t\\ & =f_{\beta ,ba}({\hat{b}}_k^a)-\frac{1}{2}{\hat{\gamma }}_k^i\left(I+Exp({\hat{\omega }}_k^b\Delta t)\right)\delta b_k^a\Delta t\\ \frac{\partial ({\beta }_{k+1}^i)}{\partial b_k^a}& =\underset{\delta b_k^a\to 0}{\lim }\frac{f_{\beta ,ba}({\hat{b}}_k^a+\delta b_k^a)-f_{\beta ,ba}({\hat{b}}_k^a)}{\delta b_k^a}\\ & =-\frac{1}{2}{\hat{\gamma }}_k^i\left(I+Exp({\hat{\omega }}_k^b\Delta t)\right)\Delta t\\ & =-\frac{1}{2}({\hat{\gamma }}_k^i+{\hat{\gamma }}_{k+1}^i)\Delta t\end{array}$$

• $\frac{\partial ({\beta }_{k+1}^i)}{\partial b_k^g}$

  $$\begin{array}{rl}{\omega }_k^b& ={\hat{\omega }}_k^b-\delta b_k^g\\ a_k^b& ={\hat{a}}_k^b+\frac{1}{2}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\delta b_k^g\Delta t\\ f_{\beta ,bg}({\hat{\beta }}_k^i+\delta {\beta }_k^i)& ={\hat{\beta }}_k^i+{\hat{\gamma }}_k^i\left({\hat{a}}_k^b+\frac{1}{2}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\delta b_k^g\Delta t\right)\Delta t\\ & =f_{\beta ,bg}({\hat{\beta }}_k^i)+\frac{1}{2}{\hat{\gamma }}_k^{i}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\delta b_k^g\Delta t^2\\ \frac{\partial ({\beta }_{k+1}^i)}{\partial b_k^g}& =\underset{\delta b_k^g\to 0}{\lim }\frac{f_{\beta ,bg}({\hat{\beta }}_k^i+\delta {\beta }_k^i)-f_{\beta ,bg}({\hat{\beta }}_k^i)}{\delta b_k^g}\\ & =\frac{1}{2}{\hat{\gamma }}_k^{i}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\Delta t^2\\ & =\frac{1}{2}{\hat{\gamma }}_{k+1}^i[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\Delta t^2\end{array}$$

偏置项对各状态变量雅可比

因为：
$$\begin{array}{rl}{\hat{b}}_{k+1}^a& ={\hat{b}}_k^a\\ {\hat{b}}_{k+1}^g& ={\hat{b}}_k^g\end{array}$$
所以：
$$\begin{array}{rl}\frac{\partial (b_{k+1}^a)}{\partial b_k^a}& =I\\ \frac{\partial (b_{k+1}^g)}{\partial b_k^g}& =I\end{array}$$