IMU预积分--对噪声雅可比推导

平移项对噪声雅可比

$$\begin{array}{rl}{\hat{\omega }}_k^b& =\frac{1}{2}[({\tilde{\omega }}_k^b-{\hat{b}}_k^g)+({\tilde{\omega }}_{k+1}^b-{\hat{b}}_k^g)]\\ {\hat{a}}_k^b& =\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]\\ E[{\hat{n}}_k^a]& =0\\ E[{\hat{n}}_{k+1}^a]& =0\\ E[{\hat{n}}_k^g]& =0\\ E[{\hat{n}}_{k+1}^g]& =0\end{array}$$

• $\frac{\partial {\alpha }_{k+1}^i}{\partial \delta n_k^a},\frac{\partial {\alpha }_{k+1}^i}{\partial \delta n_{k+1}^a}$

  $$\begin{array}{rl}{a}_k^b& =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a+\delta n_k^a)+Exp({\hat{\omega }}_k^b\Delta t)({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a+\delta n_{k+1}^a)\right]\\ & ={\hat{a}}_k^b+\frac{1}{2}\delta n_k^a+\frac{1}{2}Exp({\hat{\omega }}_k^b\Delta t)\delta n_{k+1}^a\\ g_{\alpha ,na}(\delta n_k^a,\delta n_{k+1}^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}\delta n_k^a+\frac{1}{2}Exp({\hat{\omega }}_k^b\Delta t)\delta n_{k+1}^a\right]\Delta t^2\\ & =g_{\alpha ,na}(0,0)+\frac{1}{4}{\hat{\gamma }}_k^i\delta n_k^a\Delta t^2+\frac{1}{4}{\hat{\gamma }}_{k+1}^i\delta n_{k+1}^a\Delta t^2\end{array}$$

  $$\begin{array}{rl}\frac{\partial {\alpha }_{k+1}^i}{\partial \delta n_k^a}& =\underset{\delta n_k^a\to 0}{\lim }\frac{g_{\alpha ,na}(\delta n_k^a,0)-g_{\alpha ,na}(0,0)}{\delta n_k^a}\\ & =\frac{1}{4}{\hat{\gamma }}_k^i\Delta t^2\\ \frac{\partial {\alpha }_{k+1}^i}{\partial \delta n_{k+1}^a}& =\underset{\delta n_{k+1}^a\to 0}{\lim }\frac{g_{\alpha ,na}(0,\delta n_{k+1}^a)-g_{\alpha ,na}(0,0)}{\delta n_{k+1}^a}\\ & =\frac{1}{4}{\hat{\gamma }}_{k+1}^i\Delta t^2\end{array}$$

• $\frac{\partial {\alpha }_{k+1}^i}{\partial \delta n_k^g},\frac{\partial {\alpha }_{k+1}^i}{\partial \delta n_{k+1}^g}$

  $$\begin{array}{rl}{\omega }_k^b& =\frac{1}{2}[({\tilde{\omega }}_k^b-{\hat{b}}_k^g+\delta n_k^g)+({\tilde{\omega }}_{k+1}^b-{\hat{b}}_k^g+\delta n_{k+1}^g)]\\ & ={\hat{\omega }}_k^b+\frac{1}{2}\delta n_k^g+\frac{1}{2}\delta n_{k+1}^g\\ a_k^b& =\frac{1}{2}\left[({\tilde{a}}_k^b-{\hat{b}}_k^a)+Exp({\hat{\omega }}_k^b\Delta t+\frac{1}{2}\delta n_k^g\Delta t+\frac{1}{2}\delta n_{k+1}^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\left(\underset{I}{\underbrace{J_r({\hat{\omega }}_k^b\Delta t)}}\left(\frac{1}{2}\delta n_k^g\Delta t+\frac{1}{2}\delta n_{k+1}^g\Delta t\right)\right)({\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\left(\frac{1}{2}\delta n_k^g\Delta t+\frac{1}{2}\delta n_{k+1}^g\Delta t\right)({\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)\left(I+{\left[\frac{1}{2}\delta n_k^g\Delta t+\frac{1}{2}\delta n_{k+1}^g\Delta t\right]}_{\times }\right)({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right]\\ & ={\hat{a}}_k^b+\frac{1}{2}\left(Exp({\hat{\omega }}_k^b\Delta t){\left[\frac{1}{2}\delta n_k^g\Delta t+\frac{1}{2}\delta n_{k+1}^g\Delta t\right]}_{\times }({\tilde{a}}_{k+1}^b-{\hat{b}}_k^a)\right)\\ & ={\hat{a}}_k^b-\frac{1}{4}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\left(\delta n_k^g\Delta t+\delta n_{k+1}^g\Delta t\right)\\ g_{\alpha ,ng}(\delta n_k^g,\delta n_{k+1}^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}{4}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\left(\delta n_k^g\Delta t+\delta n_{k+1}^g\Delta t\right)\right]\Delta t^2\\ & =g_{\alpha ,ng}(0,0)-\frac{1}{8}{\hat{\gamma }}_{k+1}^i[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }(\delta n_k^g+\delta n_{k+1}^g)\Delta t^3\end{array}$$

  $$\begin{array}{rl}\frac{\partial {\alpha }_{k+1}^i}{\partial \delta n_k^g}& =\underset{\delta n_k^g\to 0}{\lim }\frac{g_{\alpha ,ng}(\delta n_k^g,0)-g_{\alpha ,na}(0,0)}{\delta n_k^a}\\ & =-\frac{1}{8}{\hat{\gamma }}_{k+1}^i[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\Delta t^3\\ \frac{\partial {\alpha }_{k+1}^i}{\partial \delta n_{k+1}^g}& =\underset{\delta n_{k+1}^g\to 0}{\lim }\frac{g_{\alpha ,ng}(0,\delta n_{k+1}^g)-g_{\alpha ,ng}(0,0)}{\delta n_{k+1}^g}\\ & =-\frac{1}{8}{\hat{\gamma }}_{k+1}^i[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\Delta t^3\end{array}$$

角度项对噪声雅可比

$$\begin{array}{rl}{\hat{\omega }}_k^b& =\frac{1}{2}[({\tilde{\omega }}_k^b-{\hat{b}}_k^g)+({\tilde{\omega }}_{k+1}^b-{\hat{b}}_k^g)]\\ {\hat{\gamma }}_{k+1}^i& ={\hat{\gamma }}_k^i\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\hat{\omega }}_k^b\Delta t\end{array}\right]\\ or& \\ Exp({\hat{\theta }}_{k+1}^i)& =Exp({\hat{\theta }}_k^i)Exp({\hat{\omega }}_k^b\Delta t)\end{array}$$

$$\begin{array}{rl}{\omega }_k^b& ={\hat{\omega }}_k^b+\frac{1}{2}\delta n_k^g+\frac{1}{2}\delta n_{k+1}^g\\ \frac{\partial {\theta }_k^i}{\delta n_k^g}& =\underset{\delta n_k^g\to 0}{\lim }\frac{Exp({\hat{\theta }}_k^i)Exp\left(({\hat{\omega }}_k^b+\frac{1}{2}\delta n_k^g)\Delta t\right)\ominus Exp({\hat{\theta }}_k^i)Exp({\hat{\omega }}_k^b\Delta t)}{\delta n_k^g}\\ & =\underset{\delta n_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)\frac{1}{2}\delta n_k^g\Delta t\right)\ominus Exp({\hat{\theta }}_k^i)Exp({\hat{\omega }}_k^b\Delta t)}{\delta n_k^g}\\ & =\underset{\delta n_k^g\to 0}{\lim }\frac{Exp({\hat{\theta }}_k^i)Exp({\hat{\omega }}_k^b\Delta t)Exp\left(\frac{1}{2}\delta n_k^g\Delta t\right)\ominus Exp({\hat{\theta }}_k^i)Exp({\hat{\omega }}_k^b\Delta t)}{\delta n_k^g}\\ & =\underset{\delta n_k^g\to 0}{\lim }\frac{Log(Exp\left(\frac{1}{2}\delta n_k^g\Delta t\right))}{\delta n_k^g}\\ & =\frac{1}{2}I\cdot \Delta t\\ \frac{\partial {\theta }_k^i}{\delta n_k^g}& =\frac{1}{2}I\cdot \Delta t\end{array}$$

速度项对噪声雅可比

• $\frac{\partial {\beta }_{k+1}^i}{\partial \delta n_k^a},\frac{\partial {\beta }_{k+1}^i}{\partial \delta n_{k+1}^a}$

  $$\begin{array}{rl}{a}_k^b& ={\hat{a}}_k^b+\frac{1}{2}\delta n_k^a+\frac{1}{2}Exp({\hat{\omega }}_k^b\Delta t)\delta n_{k+1}^a\\ g_{\beta ,na}(\delta n_k^a,\delta n_{k+1}^a)& ={\hat{\beta }}_k^i+{\hat{\gamma }}_k^i\left({\hat{a}}_k^b+\frac{1}{2}\delta n_k^a+\frac{1}{2}Exp({\hat{\omega }}_k^b\Delta t)\delta n_{k+1}^a\right)\Delta t\\ & =g_{\beta ,na}(0,0)+\frac{1}{2}{\hat{\gamma }}_k^i\delta n_k^a\Delta t+\frac{1}{2}{\hat{\gamma }}_{k+1}^i\delta n_{k+1}^a\Delta t\end{array}$$

  $$\begin{array}{rl}\frac{\partial {\beta }_{k+1}^i}{\partial \delta n_k^a}& =\frac{1}{2}{\hat{\gamma }}_k^i\Delta t\\ \frac{\partial {\beta }_{k+1}^i}{\partial \delta n_{k+1}^a}& =\frac{1}{2}{\hat{\gamma }}_{k+1}^i\Delta t\end{array}$$

• $\frac{\partial {\beta }_{k+1}^i}{\partial \delta n_k^g},\frac{\partial {\beta }_{k+1}^i}{\partial \delta n_{k+1}^g}$

  $$\begin{array}{rl}{\omega }_k^b& ={\hat{\omega }}_k^b+\frac{1}{2}\delta n_k^g+\frac{1}{2}\delta n_{k+1}^g\\ a_k^b& ={\hat{a}}_k^b-\frac{1}{4}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\left(\delta n_k^g\Delta t+\delta n_{k+1}^g\Delta t\right)\\ g_{\beta ,ng}(\delta n_k^g,\delta n_{k+1}^g)& ={\hat{\beta }}_k^i+{\hat{\gamma }}_k^i\left({\hat{a}}_k^b-\frac{1}{4}Exp({\hat{\omega }}_k^b\Delta t)[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\left(\delta n_k^g\Delta t+\delta n_{k+1}^g\Delta t\right)\right)\Delta t\\ & =g_{\beta ,ng}(0,0)-\frac{1}{4}{\hat{\gamma }}_{k+1}^i[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\left(\delta n_k^g+\delta n_{k+1}^g\right)\Delta t^2\end{array}$$

  $$\begin{array}{rl}\frac{\partial {\beta }_{k+1}^i}{\partial \delta n_k^g}& =-\frac{1}{4}{\hat{\gamma }}_{k+1}^i[{\tilde{a}}_{k+1}^b-{\hat{b}}_k^a{]}_{\times }\Delta t^2\\ \frac{\partial {\beta }_{k+1}^i}{\partial \delta n_{k+1}^g}& =-\frac{1}{4}{\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}^g& ={\hat{b}}_k^g+{\hat{n}}_k^{bg}\Delta t\\ g_{bg,nbg}(\delta n_k^{bg})& ={\hat{b}}_k^g+{\hat{n}}_k^{bg}\Delta t+\delta n_k^{bg}\Delta t\\ & =g_{bg,nbg}(0)+{\hat{n}}_k^{bg}\Delta t\\ {\hat{b}}_{k+1}^a& ={\hat{b}}_k^a+{\hat{n}}_k^{ba}\Delta t\\ g_{ba,nba}(\delta n_k^{ba})& ={\hat{b}}_k^a+{\hat{n}}_k^{ba}\Delta t+\delta n_k^{ba}\Delta t\\ & =g_{ba,nba}(0)+{\hat{n}}_k^{ba}\Delta t\end{array}$$

所以：
$$\begin{array}{rl}\frac{\partial n_{k+1}^{bg}}{\partial n_k^{bg}}& =\underset{\delta n_k^{bg}\to 0}{\lim }\frac{g_{bg,nbg}(\delta n_k^{bg})-g_{bg,nbg}(0)}{\delta n_k^{bg}}\\ & =I\cdot \Delta t\\ \frac{\partial n_{k+1}^{ba}}{\partial n_k^{ba}}& =\underset{\delta n_k^{ba}\to 0}{\lim }\frac{g_{ba,nba}(\delta n_k^{ba})-g_{ba,nba}(0)}{\delta n_k^{ba}}\\ & =I\cdot \Delta t\end{array}$$