如何制作奶油怎么做_厦门seo排名公司_网络推广网站建设_seo快速排名服务

中值积分形式的预积分（一）：https://blog.csdn.net/ergevv/article/details/143165323?spm=1001.2014.3001.5501
中值积分形式的预积分（二）：https://blog.csdn.net/ergevv/article/details/143262065?spm=1001.2014.3001.5502
零偏更新以及预积分的更新：https://blog.csdn.net/ergevv/article/details/143274896?spm=1001.2014.3001.5501

接前文：中值积分形式的预积分（一）：https://blog.csdn.net/ergevv/article/details/143165323?spm=1001.2014.3001.5501

3.5、对$\delta b_k^g$，只跟$\mathbf{a}\delta t$里的$q_{b{i}_{b}k+1}({\bar{a}}^{bk+1}-b_k^a)$相关，则：
$$\begin{array}{rl}{f}_{35}& =\frac{\partial \delta {\beta }^{bibk+1}}{\partial \delta b_k^g}=\frac{\partial \mathbf{a}\delta t}{\partial \delta b_k^g}\\ & =\frac{\partial \frac{1}{2}{\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}(\mathbf{\omega }-\delta {\mathbf{b}}_k^g)\delta t\end{array}\right]}{\partial \delta {\mathbf{b}}_k^g}({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\delta t\\ & =\frac{1}{2}\frac{\partial {\mathbf{R}}_{b_i{b}_k}\exp ([(\omega -\delta {\mathbf{b}}_k^g)\delta t{]}_{\times })({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\delta t}{\partial \delta {\mathbf{b}}_k^g}\\ & =\frac{1}{2}\frac{\partial {\mathbf{R}}_{b_i{b}_k}\exp ([\omega \delta t{]}_{\times })\exp \left([-J_r(\omega \delta t)\delta {\mathbf{b}}_k^g\delta t{]}_{\times }\right)({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\delta t}{\partial \delta {\mathbf{b}}_k^g}\\ & =\frac{1}{2}\frac{\partial {\mathbf{R}}_{b_i{b}_k}\exp ([\omega \delta t{]}_{\times })(I+\left([-J_r(\omega \delta t)\delta {\mathbf{b}}_k^g\delta t{]}_{\times }\right))({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\delta t}{\partial \delta {\mathbf{b}}_k^g}\\ & =\frac{1}{2}\frac{\partial {\mathbf{R}}_{b_i{b}_{k+1}}\left([-J_r(\omega \delta t)\delta {\mathbf{b}}_k^g\delta t{]}_{\times }\right)({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\delta t}{\partial \delta {\mathbf{b}}_k^g}\\ & =-\frac{1}{2}\frac{\partial {\mathbf{R}}_{b_i{b}_{k+1}}[({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\delta t{]}_{\times }\left(-J_r(\omega \delta t)\delta {\mathbf{b}}_k^g\delta t\right)}{\partial \delta {\mathbf{b}}_k^g}\\ & =-\frac{1}{2}{\mathbf{R}}_{b_i{b}_{k+1}}[({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\delta t{]}_{\times }\left(-J_r(\omega \delta t)\delta t\right)\end{array}$$

4~5、$\delta b_{k+1}^a$、$\delta b_{k+1}^g$对其他状态量的求导：
根据：
$$\begin{array}{rl}{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{array}$$
易得：
$$\begin{array}{rl}{f}_{41}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta {\alpha }^{bibk}}=0\\ f_{42}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta {\theta }^{bibk}}=0\\ f_{43}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta {\beta }^{bibk}}=0\\ f_{44}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta b_k^a}=I\\ f_{45}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta b_k^g}=0\end{array}$$

$$\begin{array}{rl}{f}_{51}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta {\alpha }^{bibk}}=0\\ f_{52}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta {\theta }^{bibk}}=0\\ f_{53}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta {\beta }^{bibk}}=0\\ f_{54}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta b_k^a}=0\\ f_{55}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta b_k^g}=I\end{array}$$

$G矩阵的推导：$
1、$\alpha$对噪声量的求导：
1.1、对$\delta n_k^a$，只跟$\frac{1}{2}\mathbf{a}\delta t^2$里的$q_{bibk}({\bar{a}}^{bk}-b_k^a)$相关，则：
$$g_{11}=\frac{\partial \delta {\alpha }^{bibk+1}}{\partial \delta n_k^a}=\frac{\partial \frac{1}{2}\mathbf{a}\delta t^2}{\partial \delta n_k^a}=\frac{\partial \frac{1}{4}{q}_{bibk}({\bar{a}}^{bk}+\delta n_k^a-b_k^a)\delta t^2}{\partial \delta n_k^a}=\frac{1}{4}{q}_{bibk}\delta t^2$$

1.2、对$\delta n_k^g$，只跟$\frac{1}{2}\mathbf{a}\delta t^2$里的$q_{bibk+1}({\bar{a}}^{bk}-b_k^a)$相关，则：
$$\begin{array}{rl}{g}_{12}& =\frac{\partial \delta {\alpha }^{bibk+1}}{\partial \delta n_k^g}=\frac{\partial \frac{1}{2}\mathbf{a}\delta t^2}{\partial \delta n_k^g}\\ & =\frac{\partial \frac{1}{4}{q}_{bibk+1}({\bar{a}}^{bk+1}-b_k^a)\delta t^2}{\partial \delta n_k^g}\\ & =\frac{\partial \frac{1}{4}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1}+\delta n_k^g)-b_k^g)\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t^2}{\partial \delta n_k^g}\\ & =\frac{\partial \frac{1}{4}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1})-b_k^g)\delta t+\frac{1}{2}\delta n_k^g\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t^2}{\partial \delta n_k^g}\\ & =\frac{\partial \frac{1}{4}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1})-b_k^g)\delta t{]}_{\times })\exp ([\frac{1}{2}\delta n_k^g\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t^2}{\partial \delta n_k^g}\\ & =\frac{\partial -\frac{1}{4}{q}_{bibk+1}[({\bar{a}}^{bk+1}-b_k^a)\delta t^2{]}_{\times }\frac{1}{2}\delta n_k^g\delta t}{\partial \delta n_k^g}\\ & =-\frac{1}{4}{q}_{bibk+1}[({\bar{a}}^{bk+1}-b_k^a)\delta t^2{]}_{\times }\frac{1}{2}\delta t\end{array}$$
其中利用公式：$\exp \left([\varphi +\delta \varphi {]}_{\times }\right)\approx \exp ([\varphi {]}_{\times })\exp \left([J_r(\varphi )\delta \varphi {]}_{\times }\right)$，$J_r(\varphi )$ 称之为 SO3 的右雅克比。当 $\varphi$ 非常小时，$J_r(\varphi )\approx I$

1.3、对$\delta n_{k+1}^a$，与1.1推导一致：
$$g_{13}=\frac{\partial \delta {\alpha }^{bibk+1}}{\partial \delta n_{k+1}^a}=\frac{\partial \frac{1}{2}\mathbf{a}\delta t^2}{\partial \delta n_{k+1}^a}=\frac{\partial \frac{1}{4}{q}_{bibk+1}({\bar{a}}^{bk+1}+\delta n_{k+1}^a-b_k^a)\delta t^2}{\partial \delta n_{k+1}^a}=\frac{1}{4}{q}_{bibk+1}\delta t^2$$

1.4、对$\delta n_{k+1}^g$，与1.2推导一致：
$$\begin{array}{rl}{g}_{14}& =\frac{\partial \delta {\alpha }^{bibk+1}}{\partial \delta n_k^{g+1}}=\frac{\partial \frac{1}{2}\mathbf{a}\delta t^2}{\partial \delta n_k^{g+1}}\\ & =\frac{\partial \frac{1}{4}{q}_{bibk+1}({\bar{a}}^{bk+1}-b_k^a)\delta t^2}{\partial \delta n_k^{g+1}}\\ & =\frac{\partial \frac{1}{4}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1}+\delta n_k^{g+1})-b_k^g)\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t^2}{\partial \delta n_k^{g+1}}\\ & =\frac{\partial \frac{1}{4}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1})-b_k^g)\delta t+\frac{1}{2}\delta n_k^{g+1}\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t^2}{\partial \delta n_k^{g+1}}\\ & =\frac{\partial \frac{1}{4}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1})-b_k^g)\delta t{]}_{\times })\exp ([\frac{1}{2}\delta n_k^{g+1}\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t^2}{\partial \delta n_k^{g+1}}\\ & =\frac{\partial -\frac{1}{4}{q}_{bibk+1}[({\bar{a}}^{bk+1}-b_k^a)\delta t^2{]}_{\times }\frac{1}{2}\delta n_k^{g+1}\delta t}{\partial \delta n_k^{g+1}}\\ & =-\frac{1}{4}{q}_{bibk+1}[({\bar{a}}^{bk+1}-b_k^a)\delta t^2{]}_{\times }\frac{1}{2}\delta t\end{array}$$

1.5~1.6、对$\delta n_{b_k^a}、\delta n_{b_k^g}$，$\delta {\alpha }^{bibk+1}$与其无关，故：
$$\begin{array}{rl}{g}_{15}& =\frac{\partial \delta {\alpha }^{bibk+1}}{\partial \delta n_{b_k^a}}=0\\ g_{16}& =\frac{\partial \delta {\alpha }^{bibk+1}}{\partial \delta n_{b_k^g}}=0\end{array}$$

2、$\theta$对噪声量的求导：
2.1、2.3、2.5～2.6、对$\delta n_k^a、\delta n_{k+}^a、\delta n_{b_k^a}、\delta n_{b_k^g}$，$\delta {\theta }^{bibk+1}$与其无关，则：
$$\begin{array}{rl}{g}_{21}& =\frac{\partial \delta {\theta }^{bibk+1}}{\partial \delta n_{b_k^a}}=0\\ g_{22}& =\frac{\partial \delta {\theta }^{bibk+1}}{\partial \delta n_{b_k^g}}=0\\ g_{25}& =\frac{\partial \delta {\theta }^{bibk+1}}{\partial \delta n_{b_k^a}}=0\\ g_{26}& =\frac{\partial \delta {\theta }^{bibk+1}}{\partial \delta n_{b_k^g}}=0\end{array}$$

2.2、对$\delta n_k^g$，类似$f_{25}的推导：

$$\begin{array}{rl}{R}_{bibk+1}\exp ([\delta {\theta }^{bibk+1}{]}_{\times })& =R_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1}+\delta n_k^g)-b_k^g)\delta t{]}_{\times })\\ & =R_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1})-b_k^g)\delta t{]}_{\times })\exp ([\frac{1}{2}\delta n_k^g\delta t{]}_{\times })\\ & =R_{bibk+1}\exp ([\frac{1}{2}\delta n_k^g\delta t{]}_{\times })\end{array}$$
$$g_{22}=\frac{\partial \delta {\theta }^{bibk+1}}{\partial \delta n_k^g}=\frac{\partial \frac{1}{2}\delta n_k^g\delta t}{\partial \delta n_k^g}=\frac{1}{2}I\delta t$$

2.4、对$\delta n_k^g$，类似$f_{25}的推导：

$$\begin{array}{rl}{R}_{bibk+1}\exp ([\delta {\theta }^{bibk+1}{]}_{\times })& =R_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1}+\delta n_{k+1}^g)-b_k^g)\delta t{]}_{\times })\\ & =R_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1})-b_k^g)\delta t{]}_{\times })\exp ([\frac{1}{2}\delta n_{k+1}^g\delta t{]}_{\times })\\ & =R_{bibk+1}\exp ([\frac{1}{2}\delta n_{k+1}^g\delta t{]}_{\times })\end{array}$$
$$g_{24}=\frac{\partial \delta {\theta }^{bibk+1}}{\partial \delta n_{k+1}^g}=\frac{\partial \frac{1}{2}\delta n_{k+1}^g\delta t}{\partial \delta n_{k+1}^g}=\frac{1}{2}I\delta t$$

3、$\beta$对噪声量的求导：
3.1、对$\delta n_k^a$，只跟$\mathbf{a}\delta t$里的$q_{bibk}({\bar{a}}^{bk}-b_k^a)$相关，则：
$$g_{31}=\frac{\partial \delta {\beta }^{bibk+1}}{\partial \delta n_k^a}=\frac{\partial \mathbf{a}\delta t}{\partial \delta n_k^a}=\frac{\partial \frac{1}{2}{q}_{bibk}({\bar{a}}^{bk}+\delta n_k^a-b_k^a)\delta t}{\partial \delta n_k^a}=\frac{1}{2}{q}_{bibk}\delta t$$

3.2、对$\delta n_k^g$，只跟$\mathbf{a}\delta t$里的$q_{bibk+1}({\bar{a}}^{bk}-b_k^a)$相关，则：
$$\begin{array}{rl}{g}_{32}& =\frac{\partial \delta {\beta }^{bibk+1}}{\partial \delta n_k^g}=\frac{\partial \mathbf{a}\delta t}{\partial \delta n_k^g}\\ & =\frac{\partial \frac{1}{2}{q}_{bibk+1}({\bar{a}}^{bk+1}-b_k^a)\delta t}{\partial \delta n_k^g}\\ & =\frac{\partial \frac{1}{2}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1}+\delta n_k^g)-b_k^g)\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t}{\partial \delta n_k^g}\\ & =\frac{\partial \frac{1}{2}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1})-b_k^g)\delta t+\frac{1}{2}\delta n_k^g\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t}{\partial \delta n_k^g}\\ & =\frac{\partial \frac{1}{2}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1})-b_k^g)\delta t{]}_{\times })\exp ([\frac{1}{2}\delta n_k^g\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t}{\partial \delta n_k^g}\\ & =\frac{\partial -\frac{1}{2}{q}_{bibk+1}[({\bar{a}}^{bk+1}-b_k^a)\delta t{]}_{\times }\frac{1}{2}\delta n_k^g\delta t}{\partial \delta n_k^g}\\ & =-\frac{1}{2}{q}_{bibk+1}[({\bar{a}}^{bk+1}-b_k^a)\delta t{]}_{\times }\frac{1}{2}\delta t\end{array}$$

3.3、对$\delta n_{k+1}^a$，只跟$\mathbf{a}\delta t$里的$q_{bibk+1}({\bar{a}}^{bk}-b_k^a)$相关，则：
$$g_{31}=\frac{\partial \delta {\beta }^{bibk+1}}{\partial \delta n_{k+1}^a}=\frac{\partial \mathbf{a}\delta t}{\partial \delta n_{k+1}^a}=\frac{\partial \frac{1}{2}{q}_{bibk+1}({\bar{a}}^{bk}+\delta n_{k+1}^a-b_k^a)\delta t}{\partial \delta n_{k+1}^a}=\frac{1}{2}{q}_{bibk+1}\delta t$$

3.4、对$\delta n_{k+1}^g$，只跟$\mathbf{a}\delta t$里的$q_{bibk+1}({\bar{a}}^{bk}-b_k^a)$相关，则：
$$\begin{array}{rl}{g}_{32}& =\frac{\partial \delta {\beta }^{bibk+1}}{\partial \delta n_{k+1}^g}=\frac{\partial \mathbf{a}\delta t}{\partial \delta n_{k+1}^g}\\ & =\frac{\partial \frac{1}{2}{q}_{bibk+1}({\bar{a}}^{bk+1}-b_k^a)\delta t}{\partial \delta n_{k+1}^g}\\ & =\frac{\partial \frac{1}{2}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1}+\delta n_{k+1}^g)-b_k^g)\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t}{\partial \delta n_{k+1}^g}\\ & =\frac{\partial \frac{1}{2}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1})-b_k^g)\delta t+\frac{1}{2}\delta n_{k+1}^g\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t}{\partial \delta n_{k+1}^g}\\ & =\frac{\partial \frac{1}{2}{q}_{bibk}\exp ([(\frac{1}{2}({\omega }^{bk}+{\omega }^{bk+1})-b_k^g)\delta t{]}_{\times })\exp ([\frac{1}{2}\delta n_{k+1}^g\delta t{]}_{\times })({\bar{a}}^{bk+1}-b_k^a)\delta t}{\partial \delta n_{k+1}^g}\\ & =\frac{\partial -\frac{1}{2}{q}_{bibk+1}[({\bar{a}}^{bk+1}-b_k^a)\delta t{]}_{\times }\frac{1}{2}\delta n_{k+1}^g\delta t}{\partial \delta n_{k+1}^g}\\ & =-\frac{1}{2}{q}_{bibk+1}[({\bar{a}}^{bk+1}-b_k^a)\delta t{]}_{\times }\frac{1}{2}\delta t\end{array}$$

3.5~3.6、对$\delta n_{b_k^a}、\delta n_{b_k^g}$，$\delta {\beta }^{bibk+1}$与其无关，故：
$$\begin{array}{rl}{g}_{35}& =\frac{\partial \delta {\beta }^{bibk+1}}{\partial \delta n_{b_k^a}}=0\\ g_{36}& =\frac{\partial \delta {\beta }^{bibk+1}}{\partial \delta n_{b_k^g}}=0\end{array}$$

4~5、$\delta b_{k+1}^a$、$\delta b_{k+1}^g$对噪声量的求导：
根据：
$$\begin{array}{rl}{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{array}$$
易得：
$$\begin{array}{rl}{g}_{41}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta n_k^a}=0\\ g_{42}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta n_k^g}=0\\ g_{43}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta n_{k+1}^a}=0\\ g_{44}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta n_{k+1}^g}=0\\ g_{45}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta n_{b_k^a}}=I\delta t\\ g_{46}& =\frac{\partial \delta b_{k+1}^a}{\partial \delta n_{b_k^g}}=0\end{array}$$

$$\begin{array}{rl}{g}_{51}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta n_k^a}=0\\ g_{52}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta n_k^g}=0\\ g_{53}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta n_{k+1}^a}=0\\ g_{54}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta n_{k+1}^g}=0\\ g_{55}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta n_{b_k^a}}=0\\ g_{56}& =\frac{\partial \delta b_{k+1}^g}{\partial \delta n_{b_k^g}}=I\delta t\end{array}$$

至此，F和G矩阵推导完毕。

预积分的协方差递推公式：

当$y=Ax,x\in N(0,{\Sigma }_x)$，则有$\Sigma y=A{\Sigma }_x{A}^T$。
$${\Sigma }_y=E((Ax)x(Ax{)}^T)=E(Ax{x}^T{A}^T)=A{\Sigma }_x{A}^T$$

设 $\left[\begin{array}{c}\delta n_k^a\\ \delta n_k^g\\ \delta n_{k+1}^a\\ \delta n_{k+1}^g\\ \delta n_{b_k^a}\\ \delta n_{b_k^g}\end{array}\right]$的协方差矩阵为${\Sigma }_N$，这个矩阵需要标定或者自己定义参数。
所以，
$${\Sigma }_{k+1}=F\ast {\Sigma }_k\ast F^T+G\ast {\Sigma }_N\ast G^T$$

接后文：零偏更新以及预积分的更新：https://blog.csdn.net/ergevv/article/details/143274896?spm=1001.2014.3001.5501