VINS 预积分 雅各比矩阵和协方差矩阵推导过程

VINS 预积分 雅各比矩阵和协方差矩阵推导过程

根据中值积分

$${\omega }_{k+1}=\frac{{\omega }_{k+1}+{\omega }_k}{2}-b_{{\omega }_{k+1}}$$
$$q_{k+1}=q_k\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\omega }_k\delta t\end{array}\right]$$
$$a_{k+1}=\frac{q_k\left(a_k-b_{a_k}+n_{a_0}\right)+q_{k+1}\left(a_{k+1}-b_{a_{k+1}}+n_{a_1}\right)}{2}$$

旋转角度误差推导过程

  在误差状态方程式最重要的部分是对$\delta {\theta }_{k+1}$部分的推导。

由泰勒公式可得：
$$\delta {\theta }_{k+1}\approx \delta {\theta }_k+\dot{\delta {\theta }_k}\delta t$$
四元数导数的定义和一般形式定义分别为：
$$\dot{q_t}=\frac{1}{2}{q}_t\otimes {\omega }_t$$
$$\dot{q}=\frac{1}{2}q\otimes \omega$$
为了清晰起见，将大信号项和小信号项按角速率分组：
$$\omega \triangleq {\omega }_m-{\omega }_b$$
$$\delta \omega \triangleq -\delta {\omega }_b-{\omega }_n$$
注：${\omega }_m$为观测到的角速度值，${\omega }_b$角速度的偏置，${\omega }_n$ 角速度的噪声。
对于${\omega }_t$可以写成两部分：
$${\omega }_t=\omega +\delta \omega ={\omega }_m-{\omega }_b-\delta {\omega }_b-{\omega }_n$$
计算$\dot{q_t}$通过左展开和右展开两种不同的方法：
$$\dot{q_t}=\dot{\left(q_{t-1}\otimes \delta q\right)}=\frac{1}{2}{q}_t\otimes {\omega }_t$$
$$\dot{q_t}=\dot{q_{t-1}}\otimes \delta q+q_{t-1}\otimes \dot{\delta q}=\frac{1}{2}{q}_{t-1}\otimes \delta q\otimes {\omega }_t$$
$$\Rightarrow \frac{1}{2}{q}_{t-1}\otimes {\omega }_{t-1}\otimes \delta q+q_{t-1}\otimes \dot{\delta q}=\frac{1}{2}{q}_{t-1}\otimes \delta q\otimes {\omega }_t$$
$$\Rightarrow 2\dot{\delta q}=\delta q\otimes {\omega }_t-{\omega }_{t-1}\otimes \delta q$$

$$2\delta q\approx \left[\begin{array}{c}1\\ \delta \theta \end{array}\right]$$对该公式进行求导得
$$\left[\begin{array}{c}0\\ \dot{\delta \theta }\end{array}\right]=2\dot{\delta q}$$

$$\left[\begin{array}{c}0\\ \dot{\delta {\theta }_t}\end{array}\right]=2\dot{\delta q}=\delta q\otimes {\omega }_t-{\omega }_{t-1}\otimes \delta q$$
$$={\left[q\right]}_R\left({\omega }_t\right)\delta q-{\left[q\right]}_L\left({\omega }_{t-1}\right)\delta q$$
$$=\left[\begin{array}{cc}0& -{\left({\omega }_t-{\omega }_{t-1}\right)}^T\\ \left({\omega }_t-{\omega }_{t-1}\right)& -{\left[{\omega }_t+{\omega }_{t-1}\right]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_t\end{array}\right]$$
$$=\left[\begin{array}{cc}0& -{\delta \omega }^T\\ \delta \omega & -{\left[2{\omega }_t+\delta \omega \right]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_t\end{array}\right]$$
根据上式整理得：
$$\dot{\delta {\theta }_t}=\delta \omega -{\left[{\omega }_t\right]}_{\times }\delta {\theta }_t-\frac{1}{2}{\left[\delta \omega \right]}_{\times }\delta {\theta }_t\approx -{\left[{\omega }_t\right]}_{\times }\delta {\theta }_t+\delta \omega$$
$$\Rightarrow \dot{\delta {\theta }_t}=-{\left[{\omega }_t\right]}_{\times }\delta {\theta }_t+\delta \omega$$
$$\Rightarrow \dot{\delta \theta }=-{\left[{\omega }_m-{\omega }_b\right]}_{\times }\delta \theta -\delta {\omega }_b-{\omega }_n$$
$$\Rightarrow \dot{\delta {\theta }_k}=-{\left[\frac{{\omega }_{k+1}+{\omega }_k}{2}-b_{g_k}\right]}_{\times }\delta {\theta }_k-\delta b_{g_k}+\frac{n_{{\omega }_0}+n_{{\omega }_1}}{2}$$
将上式代入$\delta {\theta }_{k+1}\approx \delta {\theta }_k+\dot{\delta {\theta }_k}\delta t$得：
$$\delta {\theta }_{k+1}=\left(I-{\left[\frac{{\omega }_{k+1}+{\omega }_k}{2}-b_{g_k}\right]}_{\times }\delta t\right)\delta {\theta }_k-\delta b_{g_k}\delta t+\frac{n_{{\omega }_0}+n_{{\omega }_1}}{2}\delta t$$

速度误差推导过程

$$\delta {\beta }_{k+1}\approx \delta {\beta }_k+a_{k+1}\delta t=\delta {\beta }_k+\frac{q_k\left(a_k-b_{a_k}+n_{a_0}\right)+q_{k+1}\left(a_{k+1}-b_{a_{k+1}}+n_{a_1}\right)}{2}\delta t$$
即：
$$\delta {\beta }_{k+1}\approx \delta {\beta }_k+\dot{\delta \beta }\delta t$$
已知有如下关系成立：
$$R_k=R\left(I+{\left[\delta \theta \right]}_{\times }\right)+O\left({||\delta \theta ||}^2\right)$$
$$\dot{\delta \beta }=\dot{v}=R{a}_B+g$$
对于a（加速度）可以写成以下两部分：
$$a_B\triangleq a_m-b_a$$
$$\delta a_B\triangleq -\delta b_a-n_a$$
在惯性系中可以把加速度写成两部分的组合：
$$a_k=R_k\left(a_B+\delta a_B\right)+g$$
$$\dot{v_k}=\dot{v}+\dot{\delta v}=R\left(I+{\left[\delta \theta \right]}_{\times }\right)\left(a_B+\delta a_B\right)+g$$
$$\Rightarrow \dot{v_k}=R{a}_B+g+\dot{\delta v}=R{a}_B+R\delta a_B+R{\left[\delta \theta \right]}_{\times }{a}_B+R{\left[\delta \theta \right]}_{\times }\delta a_B+g$$
$$\Rightarrow \dot{\delta v}=R\left(\delta a_B+{\left[\delta \theta \right]}_{\times }{a}_B\right)+R{\left[\delta \theta \right]}_{\times }\delta a_B$$
消除二阶项，并重新组织叉乘（${\left[a\right]}_{\times }b=-{\left[b\right]}_{\times }a$）：
$$\dot{\delta v}=R\left(\delta a_B-{\left[a_B\right]}_{\times }\delta \theta \right)$$
把已知的公式代入上式中得到：
$$\dot{\delta v}=R\left(-{\left[a_m-b_a\right]}_{\times }\delta \theta -\delta b_a-n_a\right)=-R{\left[a_m-b_a\right]}_{\times }\delta \theta -R\delta b_a-R{n}_a$$
  为了简化表达，通常假设加速度计噪声是白色的，不相关的，各向同性的
$$E\left[n_a\right]=0$$
$$E\left[n_a{n_a}^T\right]={{\sigma }_a}^{2}I$$
  协方差椭球是以原点为中心的球面，意味着其均值和协方差在旋转时是不变的。

$$E\left[R{n}_a\right]=RE\left[n_a\right]=0$$
$$E\left[\left(R{n}_a\right){\left(R{n}_a\right)}^T\right]=RE\left[n_a{n_a}^T\right]R^T=R{{\sigma }_a}^{2}I{R}^T={{\sigma }_a}^{2}I$$

重新定义加速度计的噪声：
$$R{n}_a\to n_a$$
则有：
$$\dot{\delta v}=R\left(-{\left[a_m-b_a\right]}_{\times }\delta \theta -\delta b_a-n_a\right)=-R{\left[a_m-b_a\right]}_{\times }\delta \theta -R\delta b_a-n_a$$
$$\dot{\delta {\beta }_k}=-\frac{1}{2}{q}_k{\left[a_k-b_{a_k}\right]}_{\times }\delta {\theta }_k-\frac{1}{2}{q}_{k+1}{\left[a_{k+1}-b_{a_{k+1}}\right]}_{\times }\delta {\theta }_{k+1}$$
$$-\frac{1}{2}\left(q_k+q_{k+1}\right)\delta b_{a_k}-\frac{1}{2}\left(q_k{n}_{a_0}+q_{k+1}{n}_{a_1}\right)$$
把$\delta {\theta }_{k+1}$代入上式得：
$$\dot{\delta {\beta }_k}=-\frac{1}{2}{q}_k{\left[a_k-b_{a_k}\right]}_{\times }\delta {\theta }_k-\frac{1}{2}{q}_{k+1}{\left[a_{k+1}-b_{a_{k+1}}\right]}_{\times }$$$$\left(\left(I-{\left[\frac{{\omega }_{k+1}+{\omega }_k}{2}-b_{g_k}\right]}_{\times }\delta t\right)\delta {\theta }_k-\delta b_{g_k}\delta t+\frac{n_{{\omega }_0}+n_{{\omega }_1}}{2}\delta t\right)$$
$$-\frac{1}{2}\left(q_k+q_{k+1}\right)\delta b_{a_k}-\frac{1}{2}\left(q_k{n}_{a_0}+q_{k+1}{n}_{a_1}\right)$$
把上式代入
$$\delta {\beta }_{k+1}\approx \delta {\beta }_k+\dot{\delta {\beta }_k}\delta t$$

位置误差推导过程

$$\delta {\alpha }_{k+1}=\delta {\alpha }_k+\dot{\delta {\alpha }_k}\delta t$$
$\dot{\delta \beta }\delta t$表示的是速度的增量，那么在该速度的增量下的位置的增量为：
$$\dot{\delta a_k}=\frac{1}{2}\dot{\delta {\beta }_k}\delta t$$
$$=\frac{1}{4}{q}_k{\left[a_k-b_{a_k}\right]}_{\times }\delta \theta \delta t$$
$$-\frac{1}{4}{q}_{k+1}{\left[a_{k+1}-b_{a_{k+1}}\right]}_{\times }\left(\left(I-{\left[\frac{{\omega }_{k+1}+{\omega }_k}{2}\right]}_{\times }\delta t\right)\delta {\theta }_k-\delta b_{g_k}\delta t+\frac{n_{w_0}+n_{w_1}}{2}\delta t\right)\delta t$$
$$-\frac{1}{4}\left(q_k+q_{k+1}\right)\delta b_{a_k}\delta t-\frac{1}{4}\left(q_k{n}_0+q_{k+1}{n}_{a_1}\right)\delta t$$
根据上式可以得到：$\delta {\alpha }_{k+1}$

根据以上的计算结果整理成矩阵形式

$$\left[\begin{array}{c}\delta {\alpha }_{k+1}\\ \delta {\theta }_{k+1}\\ \delta {\beta }_{k+1}\\ \delta b_{a_{k+1}}\\ \delta b_{g_{k+1}}\end{array}\right]=\left[\begin{array}{ccccc}I& f_{01}& \delta t& -\frac{1}{4}\left(q_k+q_{k+1}\right){\delta t}^2& f_{04}\\ 0& I-{\left[\frac{{\omega }_{k+1}+{\omega }_k}{2}-b_{w_k}\right]}_{\times }\delta t& 0& 0& -\delta t\\ 0& f_{21}& I& -\frac{1}{2}\left(q_k+q_{k+1}\right)\delta t& f_{24}\\ 0& 0& 0& I& 0\\ 0& 0& 0& 0& I\end{array}\right]\left[\begin{array}{c}\delta {\alpha }_k\\ \delta {\theta }_k\\ \delta {\beta }_k\\ \delta b_{a_k}\\ \delta b_{g_k}\end{array}\right]$$
$$+\left[\begin{array}{cccccc}\frac{1}{4}{q}_k{\delta t}^2& v_{01}& \frac{1}{4}{q}_{k+1}{\delta t}^2& v_{03}& 0& 0\\ 0& \frac{1}{2}\delta t& 0& \frac{1}{2}\delta t& 0& 0\\ \frac{1}{2}{q}_k\delta t& v_{21}& \frac{1}{2}{q}_{k+1}\delta t& v_{23}& 0& 0\\ 0& 0& 0& 0& \delta t& 0\\ 0& 0& 0& 0& 0& \delta t\end{array}\right]\left[\begin{array}{c}{n}_{a_0}\\ n_{w_0}\\ n_{a_1}\\ n_{w_1}\\ n_{b_a}\\ n_{b_g}\end{array}\right]$$
其中：
$$f_{01}=-\frac{1}{4}{q}_k{\left[a_k-b_{a_k}\right]}_{\times }{\delta t}^2-\frac{1}{4}{q}_{k+1}{\left[a_{k+1}-b_{a_k}\right]}_{\times }\left(I-{\left[\frac{{\omega }_k+{\omega }_{k+1}}{2}-b_{g_k}\right]}_{\times }\delta t\right){\delta t}^2$$
$$f_{21}=-\frac{1}{2}{q}_k{\left[a_k-b_{a_k}\right]}_{\times }\delta t-\frac{1}{2}{q}_{k+1}{\left[a_{k+1}-b_{a_k}\right]}_{\times }\left(I-{\left[\frac{{\omega }_k+{\omega }_{k+1}}{2}\right]}_{\times }\delta t\right)\delta t$$
$$f_{04}=\frac{1}{4}\left(q_{k+1}{\left[a_{k+1}-b_{a_k}\right]}_{\times }{\delta t}^2\right)\delta t$$
$$f_{24}=\frac{1}{2}\left(q_{k+1}{\left[a_{k+1}-b_{a_k}\right]}_{\times }\delta t\right)\delta t$$
$$v_{01}=-\frac{1}{4}\left(q_{k+1}{\left[a_{k+1}-b_{a_k}\right]}_{\times }{\delta t}^2\right)\frac{1}{2}\delta t$$
$$v_{03}=-\frac{1}{4}\left(q_{k+1}{\left[a_{k+1}-b_{a_k}\right]}_{\times }{\delta t}^2\right)\frac{1}{2}\delta t$$
$$v_{21}=-\frac{1}{2}\left(q_{k+1}{\left[a_{k+1}-b_{a_k}\right]}_{\times }{\delta t}^2\right)\frac{1}{2}\delta t$$
$$v_{23}=-\frac{1}{2}\left(q_{k+1}{\left[a_{k+1}-b_{a_k}\right]}_{\times }{\delta t}^2\right)\frac{1}{2}\delta t$$
将上个矩阵简写为：
$${\delta }_{k+1}=F{\delta }_k+VQ$$
最后得到系统的雅各比矩阵$J_{k+1}$和协方差矩阵$P_{k+1}$
初始值为：
$$J_k=I$$
$$P_k=0$$
$$J_{k+1}=F{J}_k$$
$$P_{k+1}=F{P}_k{F}^T+VQ{V}^T$$