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

根据IMU残差函数及雅可比公式推导（一）已知：
$$\begin{array}{rl}{\alpha }_{b_i{b}_{k+1}}& ={\alpha }_{b_i{b}_k}+{\beta }_{b_i{b}_k}\delta t+\frac{1}{2}a\delta t^2\\ q_{b_i{b}_{k+1}}& =q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\\ {\beta }_{b_i{b}_{k+1}}& ={\beta }_{b_i{b}_k}+a\delta t\end{array}$$

$$\begin{gathered} b_{k+1}^a=b_k^a+n_{b_k^a}\delta t \\ {b}_{k+1}^g=b_k^g+n_{b_k^g}\delta t \end{gathered}$$

$$\begin{array}{rl}\omega & =\frac{1}{2}[({\tilde{\omega }}_k^{b_k}-b_k^g+n_k^g)+({\tilde{\omega }}_{k+1}^{b_{k+1}}-b_k^g+n_{k+1}^g)]\\ a& =\frac{1}{2}[q_{b_i{b}_k}({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)+q_{b_i{b}_{k+1}}({\tilde{a}}_{k+1}^{b_{k+1}}-b_k^a+n_{k+1}^a)]\end{array}$$

---

求
$$\begin{array}{rl}F& =\frac{\partial [{\alpha }_{b_i{b}_{k+1}^{\prime }},{\theta }_{b_i{b}_{k+1}^{\prime }},{\beta }_{b_i{b}_{k+1}^{\prime }},b_{b_{k+1}}^a,b_{b_{k+1}}^g{]}^T}{\partial [\delta {\alpha }_{b_k{b}_k^{\prime }},\delta {\theta }_{b_k{b}_k^{\prime }},\delta {\beta }_{b_k{b}_k^{\prime }},\delta b_{b_k}^a,\delta b_{b_k}^g{]}^T}\\ & =\left[\begin{array}{ccccc}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{array}\right]\end{array}$$

---

求F.1.x

$$\begin{array}{rl}{\alpha }_{b_i{b}_{k+1}}& ={\alpha }_{b_i{b}_k}+{\beta }_{b_i{b}_k}\delta t+\frac{1}{2}a\delta t^2\\ a& =\frac{1}{2}[q_{b_i{b}_k}({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)+q_{b_i{b}_{k+1}}({\tilde{a}}_{k+1}^{b_{k+1}}-b_k^a+n_{k+1}^a)]\end{array}$$

求F.1.2：

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

故，最终的结果：
$$\begin{array}{rl}\frac{\partial {\alpha }_{b_i{b}_{k+1}}}{\partial \delta {\theta }_{b_k{b}_k^{\prime }}}& =-\frac{1}{4}{R}_{b_i{b}_k}[({\tilde{a}}_k^{b_k}-b_k^a+n_k^a){]}_{\times }\delta t^2-\frac{1}{4}{R}_{b_i{b}_{k+1}}[({\tilde{a}}_k^{b_k}-b_k^a+n_k^a){]}_{\times }(I-[\omega \delta t{]}_{\times })\delta t^2\end{array}$$
推导过程请跳转。

求F.1.3：

$f_{13}=\delta tI$

求F.1.4：

$$\begin{array}{rl}\frac{\partial {\alpha }_{b_i{b}_{k+1}}}{\partial \delta {\theta }_{b_k{b}_k^{\prime }}}& =\frac{\partial \frac{1}{2}\frac{1}{2}[q_{b_i{b}_k}(-(b_k^a+\delta b_k^a))+q_{b_i{b}_{k+1}}(-(b_k^a+\delta b_k^a))]\delta t^2+(...)}{\partial \delta {\theta }_{b_k{b}_k^{\prime }}}\\ & =-\frac{1}{4}(q_{b_i{b}_k}+q_{b_i{b}_{k+1}})\delta t^2\end{array}$$

求F.1.5：

---

求F.2.x

$$\begin{array}{rl}{q}_{b_i{b}_{k+1}}& =q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\\ \omega & =\frac{1}{2}({\tilde{\omega }}_k^{b_k}+n_k^g+{\tilde{\omega }}_{k+1}^{b_{k+1}}+n_{k+1}^g)-b_k^g\end{array}$$

求F.2.2：

我们欲求取$\delta {\theta }_{b_{k+1}{b}_{k+1}^{\prime }}=f_{22}\delta {\theta }_{b_k{b}_k^{\prime }}$中的$f_{22}$，考虑二者之间的联系：
$$\begin{array}{rl}{q}_{b_i{b}_{k+1}}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_{k+1}{b}_{k+1}^{\prime }}\end{array}\right]& =q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}\right]\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\\ \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_{k+1}{b}_{k+1}^{\prime }}\end{array}\right]& =q_{b_{k+1}{b}_i}\otimes q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}\right]\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\\ & =q_{b_{k+1}{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}\right]\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\\ & \approx q_{b_{k+1}{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}\right]\otimes q_{b_{k+1}{b}_k}^{\ast }\\ & =\left[\begin{array}{c}1\\ \frac{1}{2}{R}_{b_{k+1}{b}_k}\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}\right]\end{array}$$
【注】这里用到一条性质：
$q\otimes p\otimes q^{\ast }=q\otimes \left[\begin{array}{c}{p}_w\\ p_v\end{array}\right]\otimes q^{\ast }=\left[\begin{array}{c}{p}_w\\ R{p}_v\end{array}\right]$
其中，$R$是$q$对应的旋转矩阵，$p_w,p_v$分别为$p$的实部和虚部。
故有：
$$\begin{array}{rl}\delta {\theta }_{b_{k+1}{b}_{k+1}^{\prime }}& =R_{b_{k+1}{b}_k}\delta {\theta }_{b_k{b}_k^{\prime }}\\ & =exp([-\omega \delta t{]}_{\times })\delta {\theta }_{b_k{b}_k^{\prime }}\\ & =(I-[\omega \delta t{]}_{\times })\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}$$
则，$f_{22}=I-[\omega \delta t{]}_{\times }$

求F.2.5：

我们欲求取$\delta {\theta }_{b_{k+1}{b}_{k+1}^{\prime }}=f_{25}\delta b_k^g$中的$f_{25}$，考虑二者之间的联系：
$$\omega =\frac{1}{2}({\tilde{\omega }}_k^{b_k}+n_k^g+{\tilde{\omega }}_{k+1}^{b_{k+1}}+n_{k+1}^g)-(b_k^g+\delta b_k^g)$$

$$\begin{array}{rl}{q}_{b_i{b}_{k+1}}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_{k+1}{b}_{k+1}^{\prime }}\end{array}\right]& =q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\otimes \left[\begin{array}{c}1\\ -\frac{1}{2}\delta b_k^g\delta t\end{array}\right]\\ \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_{k+1}{b}_{k+1}^{\prime }}\end{array}\right]& =q_{b_{k+1}{b}_i}\otimes q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\otimes \left[\begin{array}{c}1\\ -\frac{1}{2}\delta b_k^g\delta t\end{array}\right]\\ & =\left[\begin{array}{c}1\\ -\frac{1}{2}\delta b_k^g\delta t\end{array}\right]\end{array}$$
故有：
$$\begin{array}{rl}\delta {\theta }_{b_{k+1}{b}_{k+1}^{\prime }}& =-\delta tI\delta b_k^g\end{array}$$
则，$f_{25}=-\delta tI$

---

求F.3.x

$$\begin{array}{rl}{\beta }_{b_i{b}_{k+1}}& ={\beta }_{b_i{b}_k}+a\delta t\\ & ={\beta }_{b_i{b}_k}+\frac{1}{2}[q_{b_i{b}_k}({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)+q_{b_i{b}_{k+1}}({\tilde{a}}_{k+1}^{b_{k+1}}-b_k^a+n_{k+1}^a)]\delta t\end{array}$$

求F.3.2：

即$q_{b_i{b}_k}:=q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}\right]$，$q_{b_i{b}_{k+1}}:=q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}\right]\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]$，${\beta }_{b_i{b}_{k+1}}$中两部分与该项有关系：

$$\begin{array}{rl}\frac{\partial {\beta }_{b_i{b}_{k+1}}}{\partial \delta {\theta }_{b_k{b}_k^{\prime }}}& =\frac{\partial \frac{1}{2}{q}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}\right]({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t}{\partial \delta {\theta }_{b_k{b}_k^{\prime }}}\\ & +\frac{\partial \frac{1}{2}{q}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}\right]\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]({\tilde{a}}_{k+1}^{b_{k+1}}-b_k^a+n_{k+1}^a)\delta t}{\partial \delta {\theta }_{b_k{b}_k^{\prime }}}\end{array}$$
第一部分分子：
$$\begin{array}{rl}part1& =\frac{1}{2}{R}_{b_i{b}_k}exp([\delta {\theta }_{b_k{b}_k^{\prime }}{]}_{\times })({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t\\ & =\frac{1}{2}{R}_{b_i{b}_k}(I+[\delta {\theta }_{b_k{b}_k^{\prime }}{]}_{\times })({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t\\ & =-\frac{1}{2}{R}_{b_i{b}_k}[({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t{]}_{\times }\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}$$
第二部分分子：
$$\begin{array}{rl}part2& =\frac{1}{2}{R}_{b_i{b}_k}exp([\delta {\theta }_{b_k{b}_k^{\prime }}{]}_{\times })exp([\omega \delta t{]}_{\times })({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t\\ & =\frac{1}{2}{R}_{b_i{b}_k}(I+[\delta {\theta }_{b_k{b}_k^{\prime }}{]}_{\times })exp([\omega \delta t{]}_{\times })({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t\\ & =-\frac{1}{2}{R}_{b_i{b}_k}[exp([\omega \delta t{]}_{\times })({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t{]}_{\times }\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}$$
第二部分还可以做一点化简：
$$\begin{array}{rl}part2& =-\frac{1}{2}{R}_{b_i{b}_k}exp([\omega \delta t{]}_{\times })[({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t{]}_{\times }exp([-\omega \delta t{]}_{\times })\delta {\theta }_{b_k{b}_k^{\prime }}\\ & =-\frac{1}{2}{R}_{b_i{b}_{k+1}}[({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t{]}_{\times }(I-[\omega \delta t{]}_{\times })\delta {\theta }_{b_k{b}_k^{\prime }}\end{array}$$
故，最终的结果：
$$\begin{array}{rl}\frac{\partial {\beta }_{b_i{b}_{k+1}}}{\partial \delta {\theta }_{b_k{b}_k^{\prime }}}& =-\frac{1}{2}{R}_{b_i{b}_k}[({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t{]}_{\times }-\frac{1}{2}{R}_{b_i{b}_{k+1}}[({\tilde{a}}_k^{b_k}-b_k^a+n_k^a)\delta t{]}_{\times }(I-[\omega \delta t{]}_{\times })\end{array}$$

求F.3.4：

即$b_k^a:=b_k^a+\delta b_k^a$
$$\begin{array}{rl}\frac{\partial {\beta }_{b_i{b}_{k+1}}}{\partial \delta b_k^a}& =\frac{\frac{1}{2}[q_{b_i{b}_k}({\tilde{a}}_k^{b_k}-(b_k^a+\delta b_k^a)+n_k^a)+q_{b_i{b}_{k+1}}({\tilde{a}}_{k+1}^{b_{k+1}}-(b_k^a+\delta b_k^a)+n_{k+1}^a)]\delta t+(...)}{\partial \delta b_k^a}\\ & =\frac{\frac{1}{2}[q_{b_i{b}_k}(-\delta b_k^a)+q_{b_i{b}_{k+1}}(-b_k^a)]\delta t+(...)}{\partial \delta b_k^a}\\ & =-\frac{1}{2}[q_{b_i{b}_k}+q_{b_i{b}_{k+1}}]\delta t\end{array}$$

求F.3.5：

即$b_k^g:=b_k^g+\delta b_k^g$，其影响体现在$\omega$中：
$$\begin{array}{rl}\omega & :=\frac{1}{2}[({\tilde{\omega }}_k^{b_k}-(b_k^g+\delta b_k^g)+n_k^g)+({\tilde{\omega }}_{k+1}^{b_{k+1}}-(b_k^g+\delta b_k^g)+n_{k+1}^g)]\\ & =\frac{1}{2}[({\tilde{\omega }}_k^{b_k}+n_k^g)+({\tilde{\omega }}_{k+1}^{b_{k+1}}+n_{k+1}^g)]-(b_k^g+\delta b_k^g)\end{array}$$
进而体现在$q_{b_i{b}_{k+1}}$：
$$q_{b_i{b}_{k+1}}:=q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]=q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\otimes \left[\begin{array}{c}1\\ -\frac{1}{2}\delta b_t^g\delta t\end{array}\right]$$
则：
$$\begin{array}{rl}\frac{\partial {\beta }_{b_i{b}_{k+1}}}{\partial \delta b_k^g}& =\frac{\frac{1}{2}{q}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\otimes \left[\begin{array}{c}1\\ -\frac{1}{2}\delta b_t^g\delta t\end{array}\right]({\tilde{a}}_{k+1}^{b_{k+1}}-b_k^a+n_{k+1}^a)\delta t+(...)}{\partial \delta b_k^g}\\ & =\frac{\frac{1}{2}{R}_{b_i{b}_{k+1}}exp([-\delta b_k^g\delta t{]}_{\times })({\tilde{a}}_{k+1}^{b_{k+1}}-b_k^a+n_{k+1}^a)\delta t+(...)}{\partial \delta b_k^g}\\ & =\frac{\frac{1}{2}{R}_{b_i{b}_{k+1}}(I+[-\delta b_k^g\delta t{]}_{\times })({\tilde{a}}_{k+1}^{b_{k+1}}-b_k^a+n_{k+1}^a)\delta t+(...)}{\partial \delta b_k^g}\\ & =\frac{\frac{1}{2}{R}_{b_i{b}_{k+1}}([-\delta b_k^g\delta t{]}_{\times })({\tilde{a}}_{k+1}^{b_{k+1}}-b_k^a+n_{k+1}^a)\delta t+(...)}{\partial \delta b_k^g}\\ & =\frac{\frac{1}{2}{R}_{b_i{b}_{k+1}}[({\tilde{a}}_{k+1}^{b_{k+1}}-b_k^a+n_{k+1}^a){]}_{\times }\delta t^2\delta b_k^g+(...)}{\partial \delta b_k^g}\\ & =\frac{1}{2}{R}_{b_i{b}_{k+1}}[({\tilde{a}}_{k+1}^{b_{k+1}}-b_k^a+n_{k+1}^a){]}_{\times }\delta t^2\end{array}$$