[VINS-Mono]IMU预积分之PVQ 增量的误差、 协方差及 Jacobian(离散形式)

PVQ 增量的误差、 协方差及 Jacobian

5.离散形式

PVQ 增量误差在离散形式下的矩阵形式：
$$\begin{array}{l}\left[\begin{array}{c}\delta {\alpha }_{k+1}\\ \delta {\theta }_{k-1}\\ \delta {\beta }_{k+1}\\ \delta b_{a_{k+1}}\\ \delta b_{w_{k+1}}\end{array}\right]=\left[\begin{array}{ccccc}\mathbf{I}& f_{12}& \delta t& f_{14}& f_{15}\\ 0& f_{22}& 0& 0& -\delta t\\ 0& f_{32}& \mathbf{I}& f_{34}& f_{35}\\ 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}\delta {\alpha }_k\\ \delta {\theta }_k\\ \delta {\beta }_k\\ \delta b_{a_k}\\ \delta b_{w_k}\end{array}\right]+\left[\begin{array}{cccccc}{g}_{11}& g_{12}& g_{13}& g_{14}& 0& 0\\ 0& -\frac{1}{2}\delta t& 0& -\frac{1}{2}\delta t& 0& 0\\ -\frac{1}{2}{\mathbf{R}}_k\delta t& g_{32}& -\frac{1}{2}{\mathbf{R}}_k\delta t& g_{34}& 0& 0\\ 0& 0& 0& 0& \delta t& 0\\ 0& 0& 0& 0& 0& \delta t\end{array}\right]\left[\begin{array}{c}{\mathbf{n}}_{a_k}\\ {\mathbf{n}}_{w_k}\\ {\mathbf{n}}_{a_{k+1}}\\ {\mathbf{n}}_{w_{k+1}}\\ {\mathbf{n}}_{b_n}\\ {\mathbf{n}}_{b_w}\end{array}\right]\\ =\mathbf{F}\left[\begin{array}{c}\delta {\alpha }_k\\ \delta {\theta }_k\\ \delta {\beta }_k\\ \delta b_{a_k}\\ \delta b_{w_k}\end{array}\right]+\mathbf{G}\left[\begin{array}{c}{\mathbf{n}}_{a_k}\\ {\mathbf{n}}_{w_k}\\ {\mathbf{n}}_{a_{k+1}}\\ {\mathbf{n}}_{w_{k+1}}\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_w}\end{array}\right]\end{array}$$
对于$\delta {\theta }_{k+1}$，由连续形式得到：
$$\begin{array}{ccc}\dot{\delta \theta }& \approx & -{\left({\hat{\omega }}_t-b_{w_t}\right)}^{\wedge }\delta \theta -n_w-\delta b_{w_t}\end{array}$$
则中值积分的离散形式为：
$$\delta {\dot{\theta }}_k\approx -{\left(\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}\right)}^{\wedge }\delta {\theta }_k-\frac{n_{w_k}+n_{w_{k-1}}}{2}-\delta b_{w_k}$$
由导数的定义：
$$\begin{array}{rl}\delta {\theta }_{k+1}& =\delta {\theta }_k+\delta {\dot{\theta }}_k\delta t\\ & =f_{22}\delta {\theta }_k-\delta t\delta b_{w_k}-\frac{\delta t}{2}{n}_{w_k}-\frac{\delta t}{2}{n}_{w_{k+1}}\end{array}$$
其中：
$$f_{22}=I-{\left(\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}\right)}^{\wedge }\delta t$$
对于$\delta {\beta }_{k+1}$，由连续形式得到：
$$\begin{array}{rl}\delta {\dot{\beta }}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}& ={\hat{\dot{\beta }}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}-{\dot{\beta }}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}\\ & =-{\mathrm{R}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}{\left[{\hat{\mathrm{a}}}_{\mathrm{t}}-{\mathrm{b}}_{\mathrm{at}}\right]}_{\times }\delta \theta -{\mathrm{R}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}\delta {\mathrm{b}}_{\mathrm{at}}-{\mathrm{R}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}{\mathrm{n}}_{\mathrm{a}}\end{array}$$
则离散形式为：
$$\delta {\dot{\beta }}_t^{b_k}=-\frac{{\mathbf{R}}_k^{b_k}{\left({\hat{a}}_k-b_{a_k}\right)}^{\wedge }\delta {\theta }_k+{\mathbf{R}}_{k+1}^{b_k}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\delta {\theta }_{k+1}}{2}-\frac{{\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}}{2}\delta b_{a_k}-\frac{{\mathbf{R}}_k^{b_k}{n}_{a_k}+{\mathbf{R}}_{k+1}^{b_k}{n}_{a_{k+1}}}{2}$$
将$\delta {\theta }_{k+1}$带入：
$$\begin{array}{rl}\delta {\dot{\beta }}_t^{b_k}=& -\frac{1}{2}{\mathbf{R}}_k^{b_k}{\left({\hat{a}}_k-b_{a_k}\right)}^{\wedge }\delta {\theta }_k\\ & -\frac{1}{2}{\mathbf{R}}_{k+1}^{b_k}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\left\{\left[I-{\left(\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}\right)}^{\wedge }\delta t\right]\delta {\theta }_k-\frac{n_{w_k}+n_{w_{k+1}}}{2}\delta t-\delta b_{w_k}\delta t\right\}\\ & -\frac{1}{2}\left({\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}\right)\delta b_{a_k}-\frac{1}{2}{\mathbf{R}}_k^{b_k}{n}_{a_k}-\frac{1}{2}{\mathbf{R}}_{k+1}^{b_k}{n}_{a_{k+1}}\\ =& \left\{-\frac{1}{2}{\mathbf{R}}_k^{b_k}{\left({\hat{a}}_k-b_{a_k}\right)}^{\wedge }-\frac{1}{2}{\mathbf{R}}_{k+1}^{b_k}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\left[I-\left(\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}\right)\delta t\right]\right\}\delta {\theta }_k\\ & +\frac{\delta t}{4}{\mathbf{R}}_{k+1}^{b_k}\left({\hat{a}}_{k+1}-b_{a_k}\right)n_{w_k}+\frac{\delta t}{4}{\mathbf{R}}_{k+1}^{b_k}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }{n}_{w_{k-1}}+\frac{\delta t}{2}{\mathbf{R}}_{k+1}^{b_k}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\delta b_{w_k}\\ & -\frac{1}{2}\left({\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}\right)\delta b_{a_k}-\frac{1}{2}{\mathbf{R}}_k^{b_k}{n}_{a_k}-\frac{1}{2}{\mathbf{R}}_{k+1}^{b_k}{n}_{a_{k+1}}\end{array}$$
由导数的定义可知：
$$\begin{array}{l}\delta {\beta }_{k+1}^{b_k}=\delta {\beta }_k^{b_k}+\delta {\dot{\beta }}_t^{b_k}\delta t\\ =f_{32}\delta {\theta }_k+\delta {\beta }_k^{b_k}-\frac{1}{2}\left({\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}\right)\delta t\delta b_{a_k}+f_{35}\delta b_{w_k}\\ -\frac{1}{2}{\mathbf{R}}_k^{b_k}\delta t{n}_{a_k}+g_{32}{n}_{w_k}-\frac{1}{2}{\mathbf{R}}_{k+1}^{b_k}\delta t{n}_{a_{k+1}}+g_{34}{n}_{w_{k-1}}\end{array}$$
其中：
$$\begin{array}{rl}{f}_{32}& =-\frac{1}{2}{\mathbf{R}}_k^{b_k}{\left({\hat{a}}_k-b_{a_k}\right)}^{\wedge }\delta t-\frac{1}{2}{\mathbf{R}}_{k+1}^{b_k}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\left[I-{\left(\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}\right)}^{\wedge }\delta t\right]\delta t\\ f_{34}& =-\frac{1}{2}\left({\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}\right)\delta t\\ f_{35}& =\frac{1}{2}{\mathbf{R}}_{k+1}^{b_k}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\delta t^2\\ g_{32}& =\frac{1}{4}{\mathbf{R}}_{k+1}^{b_k}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\delta t^2\\ g_{34}& =\frac{1}{4}{\mathbf{R}}_{k+1}^{b_k}{\left({\hat{a}}_{k+1}-b_{a_k}\right)}^{\wedge }\delta t^2\end{array}$$
对于$\delta {\alpha }_{k+1}$，由连续形式可知：
$$\delta {\dot{\alpha }}_t^{b_k}=\delta {\beta }_t^{b_k}$$
则离散形式：
$$\begin{array}{rl}\delta {\dot{\alpha }}_k^{b_k}=& \frac{1}{2}\left(\delta {\beta }_k^{b_k}+\delta {\beta }_{k+1}^{b_k}\right)\\ =& \frac{1}{2}(\delta {\beta }_k^{b_k}+\delta {\beta }_k^{b_k}+f_{32}\delta {\theta }_k+f_{35}\delta b_{w_k}+g_{32}{n}_{w_k}+g_{34}{n}_{w_{k+1}}\\ & -\frac{1}{2}\left({\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}\right)\delta t\delta b_{a_k}-\frac{1}{2}{\mathbf{R}}_k^{b_k}\delta t{n}_{a_k}-\frac{1}{2}{\mathbf{R}}_{k+1}^{b_k}\delta t{n}_{a_{k+1}})\\ =& \delta {\beta }_k^{b_k}+\frac{1}{2}{f}_{32}\delta {\theta }_k+\frac{1}{2}{f}_{35}\delta b_{w_k}+\frac{1}{2}{g}_{32}{n}_{w_k}+\frac{1}{2}{g}_{34}{n}_{w_{k+1}}\\ & -\frac{1}{4}\left({\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}\right)\delta t\delta b_{a_k}-\frac{1}{4}{\mathbf{R}}_k^{b_k}\delta t{n}_{a_k}-\frac{1}{4}{\mathbf{R}}_{k+1}^{b_k}\delta t{n}_{a_{k+1}}\end{array}$$
由导数的定义可知：
$$\begin{array}{rl}\delta {\alpha }_{k+1}=& \delta {\alpha }_k+\delta {\dot{\alpha }}_k^{b_k}\delta t\\ =& \delta {\alpha }_k+(\delta {\beta }_k^{b_k}+\frac{1}{2}{f}_{32}\delta {\theta }_k+\frac{1}{2}{f}_{35}\delta b_{w_k}+\frac{1}{2}{g}_{32}{n}_{w_k}+\frac{1}{2}{g}_{34}{n}_{w_{k-1}}\\ & -\frac{1}{4}\left({\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}\right)\delta t\delta b_{a_k}-\frac{1}{4}{\mathbf{R}}_k^{b_k}\delta t{n}_{a_k}-\frac{1}{4}{\mathbf{R}}_{k-1}^{b_k}\delta t{n}_{a_{k+1}})\delta t\\ =& \delta {\alpha }_k+\delta {\beta }_k^{b_k}\delta t+\frac{\delta t}{2}{f}_{32}\delta {\theta }_k+\frac{\delta t}{2}{f}_{35}\delta b_{w_k}+\frac{\delta t}{2}{g}_{32}{n}_{w_k}+\frac{\delta t}{2}{g}_{34}{n}_{w_{k+1}}\\ & -\frac{\delta t}{4}\left({\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}\right)\delta t\delta b_{a_k}-\frac{\delta t}{4}{\mathbf{R}}_k^{b_k}\delta t{n}_{a_k}-\frac{\delta t}{4}{\mathbf{R}}_{k+1}^{b_k}\delta t{n}_{a_{k+1}}\\ =& \delta {\alpha }_k+\frac{\delta t}{2}{f}_{32}\delta {\theta }_k+\delta t\delta {\beta }_k^{b_k}-\frac{\delta t}{4}\left({\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}\right)\delta t\delta b_{a_k}+\frac{\delta t}{2}{f}_{35}\delta b_{w_k}\\ & -\frac{\delta t}{4}{\mathbf{R}}_k^{b_k}\delta t{n}_{a_k}+\frac{\delta t}{2}{g}_{32}{n}_{w_k}-\frac{\delta t}{4}{\mathbf{R}}_{k+1}^{b_k}\delta t{n}_{a_{k+1}}+\frac{\delta t}{2}{g}_{34}{n}_{w_{k-1}}\end{array}$$
其中：
$$\begin{array}{l}{f}_{12}=\frac{\delta t}{2}{f}_{32}\\ f_{14}=-\frac{\delta t}{4}\left({\mathbf{R}}_k^{b_k}+{\mathbf{R}}_{k+1}^{b_k}\right)\delta t\\ f_{15}=\frac{\delta t}{2}{f}_{35}\\ g_{11}=-\frac{\delta t}{4}{\mathbf{R}}_k^{b_k}\delta t\\ g_{12}=\frac{\delta t}{2}{g}_{32}\\ g_{13}=-\frac{\delta t}{4}{\mathbf{R}}_{k+1}^{b_k}\delta t\\ g_{14}=\frac{\delta t}{2}{g}_{34}\end{array}$$
协方差与雅可比矩阵：
$$\delta z_{k+1}^{15\times 1}=F^{15\times 15}\delta z_k^{15\times 1}+V^{15\times 18}{Q}^{18\times 1}$$
协方差迭代公式：
$${P_{k+1}^{b_k}}^{15\times 15}=F{P}_k^{b_k}{F}^T+VQ{V}^T$$
其中，初始值$P_{b_k}^{b_k}=0$，Q为噪声的对角协方差矩阵：
$${\mathbf{Q}}^{18\times 18}=\left[\begin{array}{cccccc}{\sigma }_a^2& 0& 0& 0& 0& 0\\ 0& {\sigma }_w^2& 0& 0& 0& 0\\ 0& 0& {\sigma }_a^2& 0& 0& 0\\ 0& 0& 0& {\sigma }_w^2& 0& 0\\ 0& 0& 0& 0& {\sigma }_{b_a}^2& 0\\ 0& 0& 0& 0& 0& {\sigma }_{b_w}^2\end{array}\right]$$
另外根据上式可获得误差项的jacobian迭代公式：
$$J_{k+1}^{15\times 15}=F{J}_k$$
其中，jacobian的初始值为$J_k=I$