[VINS-Mono]IMU预积分之PVQ 增量的误差、 协方差及 Jacobian

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

根据协方差矩阵的性质：
$$\begin{array}{c}y=Ax,x\in N\left(0,{\Sigma }_x\right)\\ {\Sigma }_y=A{\Sigma }_x{A}^T\end{array}$$
则相邻时刻误差的线性传递方程：
$${\eta }_{ik}=F_{k-1}{\eta }_{ik-1}+G_{k-1}{n}_{k-1}$$
其中：

• ${\eta }_{ik}=\left[\delta {\theta }_{ik},\delta v_{ik},\delta p_{ik}\right]$：状态量误差
• $n_k=\left[n_k^g,n_k^a\right]$：测量噪声
  ​

误差的传递由两部分组成：

• 当前时刻的误差传递给下一时刻
• 当前时刻测量噪声传递给下一时刻

因此协方差矩阵可以递推得到：
$${\Sigma }_{ik}=F_{k-1}{\Sigma }_{ik-1}{F}_{k-1}^T+G_{k-1}{\Sigma }_n{G}_{k-1}^T$$

1.基于一阶泰勒展开的误差递推方程

令状态量：$x=\hat{x}+\delta x$，其中$\hat{x}$为真值，误差为$\delta x$，输入量u的噪声为n
非线性系统：
$$x_k=f\left(x_{k-1},u_{k-1}\right)$$
其状态的误差的线性递推关系为：
$$\delta x_k=F\delta x_{k-1}+G{n}_{k-1}$$

2.基于误差随时间变化的递推方程

状态误差随时间变化的导数关系为：
$$\dot{{\delta }_x}=A\delta x+Bn$$
则误差状态的传递方程为：
$$\delta x_k=\delta x_{k-1}+\dot{\delta x_{k-1}}\Delta t$$
得到：
$$\delta x_k=\left(I+A\Delta t\right){\delta }_{k-1}+B\Delta t{n}_{k-1}$$
两种推导得到：
$$F=I+A\Delta t,G=B\Delta t$$

3.PVQ 增量的误差、 协方差及 Jacobian的连续形式

• 真实状态：去除噪声后的理想状态
• 标称状态：未去除噪声的大信号
• 误差状态：噪声小信号

真实状态等于标称状态加误差状态：
$$\begin{array}{c}{\hat{\alpha }}_t^{b_k}={\alpha }_t^{b_k}+\delta {\alpha }_t^{b_k}\\ {\hat{\beta }}_t^{b_k}={\beta }_t^{b_k}+\delta {\beta }_t^{b_k}\\ {\hat{\gamma }}_t^{b_k}={\gamma }_t^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_t^{b_k}\end{array}\right]\\ {\hat{b}}_{a_t}=b_{a_t}+\delta b_{a_t}\\ {\hat{b}}_{{\omega }_t}=b_{{\omega }_t}+\delta b_{{\omega }_t}\end{array}$$
真实状态的状态方程：
$$\{\begin{array}{l}{\dot{\hat{\alpha }}}_t^{b_k}={\hat{\beta }}_t^{b_k}\\ {\dot{\hat{\beta }}}_t^{b_k}={\hat{R}}_t^{b_k}\left({\hat{a}}_t-{\hat{b}}_{a_t}-n_a\right)\\ {\dot{\hat{\gamma }}}_t^{b_k}=\frac{1}{2}\Omega \left({\omega }_t-{\hat{b}}_{{\omega }_t}-n_{\omega }\right){\hat{\gamma }}_t^{b_k}\\ {\dot{\hat{b}}}_{a_t}=n_{b_a}\\ {\dot{\hat{b}}}_{{\omega }_t}=n_{b_{\omega }}\end{array}$$
标称状态的状态方程：没有噪声和扰动
$$\{\begin{array}{l}{\dot{\alpha }}_t^{b_k}={\beta }_t^{b_k}\\ {\dot{\beta }}_t^{b_k}=R_t^{b_k}\left({\hat{a}}_t-b_{a_t}\right)\\ {\dot{\gamma }}_t^{b_k}=\frac{1}{2}\Omega \left({\omega }_t-b_{{\omega }_t}\right){\gamma }_t^{b_k}\\ {\dot{b}}_{a_t}=0\\ {\dot{b}}_{{\omega }_t}=0\end{array}$$
根据定义得到$\delta {\dot{\alpha }}_t^{b_k}$, $\delta {\dot{b}}_{a_t}$, $\delta {\dot{b}}_{w_t}$：
$$\begin{array}{rl}\delta {\dot{\alpha }}_t^{b_k}& ={\dot{\hat{\alpha }}}_t^{b_k}-{\dot{\alpha }}_t^{b_k}\\ & ={\hat{\beta }}_t^{b_k}-{\beta }_t^{b_k}\\ & =\delta {\beta }_t^{b_k}\\ \delta {\dot{b}}_{a_t}& ={\dot{b}}_{a_t}-0\\ & =n_{b_a}\\ \delta {\dot{b}}_{w_t}& ={\dot{b}}_{w_t}-0\\ & =n_{b_w}\end{array}$$
对于$\delta {\dot{\beta }}_t^{b_k}$，真值状态为：
$$\begin{array}{rl}{\hat{\dot{\beta }}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}& ={\hat{\mathrm{R}}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}\left({\hat{\mathrm{a}}}_{\mathrm{t}}-{\hat{\mathrm{b}}}_{{\mathrm{a}}_{\mathrm{t}}}-{\mathrm{n}}_{\mathrm{a}}\right)\\ & ={\mathrm{R}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}\exp \left([\delta \theta {]}_{\times }\right)\left({\hat{\mathrm{a}}}_{\mathrm{t}}-{\mathrm{n}}_{\mathrm{a}}-{\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}-\delta {\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}\right)\\ & ={\mathrm{R}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}\left(1+[\delta \theta {]}_{\times }\right)\left({\hat{\mathrm{a}}}_{\mathrm{t}}-{\mathrm{n}}_{\mathrm{a}}-{\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}-\delta {\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}\right)\\ & ={\mathrm{R}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}\left({\hat{\mathrm{a}}}_{\mathrm{t}}-{\mathrm{n}}_{\mathrm{a}}-{\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}-\delta {\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}+[\delta \theta {]}_{\times }\left({\hat{\mathrm{a}}}_{\mathrm{t}}-{\mathrm{b}}_{{\mathrm{a}}_{\mathrm{t}}}\right)\right)\\ & ={\mathrm{R}}_{\mathrm{t}}^{{\mathrm{b}}_{\mathrm{k}}}\left({\hat{\mathrm{a}}}_{\mathrm{t}}-{\mathrm{n}}_{\mathrm{a}}-{\mathrm{b}}_{\mathrm{at}}-\delta {\mathrm{b}}_{\mathrm{at}}-{\left[{\hat{\mathrm{a}}}_{\mathrm{t}}-{\mathrm{b}}_{\mathrm{at}}\right]}_{\times }\delta \theta \right)\end{array}$$
而标称状态为：
$${\dot{\beta }}_t^{b_k}=R_t^{b_k}\left({\hat{a}}_t-b_{a_t}\right)$$
因此：
$$\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{\theta }}_t^{b_k}$，根据四元数的导数得到：
$${\dot{\mathbf{q}}}_t=\frac{1}{2}\left[\begin{array}{ll}-w^{\wedge }& w\\ -w^T& 0\end{array}\right]{\mathbf{q}}_t=\frac{1}{2}\Omega (w){\mathbf{q}}_t=\frac{1}{2}\mathcal{R}\left(\left[\begin{array}{l}w\\ 0\end{array}\right]\right){\mathbf{q}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes \left[\begin{array}{l}w\\ 0\end{array}\right]$$
可以得到：

• 真实值：
  $$\begin{array}{rl}\hat{\dot{q}}& =\frac{1}{2}\hat{q}\otimes \left[\begin{array}{c}\hat{w}\\ 0\end{array}\right]\\ & =\frac{1}{2}\hat{q}\otimes \delta q\otimes \left[\begin{array}{c}\hat{w}-b_{w_t}-n_w-\delta b_{w_t}\\ 0\end{array}\right]\end{array}$$
• 标称值：
  $${\dot{q}}_t=\frac{1}{2}{q}_t\otimes \left[\begin{array}{c}{\hat{w}}_t-b_{w_t}\\ 0\end{array}\right]$$
  根据导数的性质，可以得到：
  $$\begin{array}{rl}{\hat{\dot{q}}}_t& =\dot{\overbrace{q_t\otimes \delta q}}\\ & ={\dot{q}}_t\otimes \delta q+q_t\otimes \delta \dot{q}\\ & =\frac{1}{2}{q}_t\otimes \left[\begin{array}{c}{\hat{w}}_t-b_{w_t}\\ 0\end{array}\right]\otimes \delta q+q_t\otimes \delta \dot{q}\end{array}$$

因此：
$$\begin{array}{l}\frac{1}{2}{\mathbf{q}}_t\otimes \delta \mathbf{q}\otimes \left[\begin{array}{c}{\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\\ 0\end{array}\right]=\frac{1}{2}{\mathbf{q}}_t\otimes \left[\begin{array}{c}{\hat{w}}_t-b_{w_t}\\ 0\end{array}\right]\otimes \delta \mathbf{q}+{\mathbf{q}}_t\otimes \dot{\delta \mathbf{q}}\\ \iff \frac{1}{2}\delta \mathbf{q}\otimes \left[\begin{array}{c}{\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\\ 0\end{array}\right]=\frac{1}{2}\left[\begin{array}{c}{\hat{w}}_t-b_{w_t}\\ 0\end{array}\right]\otimes \delta \mathbf{q}+\dot{\delta \mathbf{q}}\\ \iff 2\dot{\delta }\mathbf{q}=\delta \mathbf{q}\otimes \left[\begin{array}{c}{\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\\ 0\end{array}\right]-\left[\begin{array}{c}{\hat{w}}_t-b_{w_t}\\ 0\end{array}\right]\otimes \delta \mathbf{q}\\ \iff 2\dot{\delta }\mathbf{q}=\mathcal{R}\left(\left[\begin{array}{c}{\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\\ 0\end{array}\right]\right)\delta \mathbf{q}-\mathcal{L}\left(\left[\begin{array}{c}{\hat{w}}_t-b_{w_t}\\ 0\end{array}\right]\right)\delta \mathbf{q}\\ \iff 2\dot{\delta }\mathbf{q}=\left(\left[\begin{array}{cc}-{\left({\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\right)}^{\wedge }& {\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\\ -{\left({\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\right)}^T& 0\end{array}\right]-\left[\begin{array}{cc}{\left({\hat{w}}_t-b_{w_t}\right)}^{\wedge }& {\hat{w}}_t-b_{w_t}\\ -{\left({\hat{w}}_t-b_{w_t}\right)}^T& 0\end{array}\right]\right)\left[\begin{array}{c}\frac{\delta \theta }{2}\\ 1\end{array}\right]\\ \iff 2\dot{\delta \mathbf{q}}=\left[\begin{array}{cc}-{\left(2{\hat{w}}_t-2b_{w_t}-n_w-\delta b_{w_t}\right)}^{\wedge }& -n_w-\delta b_{w_t}\\ {\left(n_w+\delta b_{w_t}\right)}^T& 0\end{array}\right]\left[\begin{array}{c}\frac{\delta \theta }{2}\\ 1\end{array}\right]\end{array}$$
上式左侧可以写成：
$$2\dot{\delta \mathbf{q}}=\left[\begin{array}{c}\dot{\delta \theta }\\ 0\end{array}\right]=\left[\begin{array}{cc}-{\left(2{\hat{w}}_t-2b_{w_t}-n_w-\delta b_{w_t}\right)}^{\wedge }& -n_w-\delta b_{w_t}\\ {\left(n_w+\delta b_{w_t}\right)}^T& 0\end{array}\right]\left[\begin{array}{c}\frac{\delta \theta }{2}\\ 1\end{array}\right]$$
最终可以得到：
$$\begin{array}{ccc}\dot{\delta \theta }& =& -{\left(2{\hat{w}}_t-2b_{w_t}-n_w-\delta b_{w_t}\right)}^{\wedge }\frac{\delta \theta }{2}-n_w-\delta b_{w_t}\\ & \approx & -{\left({\hat{\omega }}_t-b_{w_t}\right)}^{\wedge }\delta \theta -n_w-\delta b_{w_t}\end{array}$$

误差的递推方程：
$$\begin{array}{l}\left[\begin{array}{c}\delta {\dot{\alpha }}_t^{b_k}\\ \delta {\dot{\beta }}_t^{b_k}\\ \delta {\dot{\theta }}_t^{b_k}\\ \delta {\dot{b}}_{a_t}\\ \delta {\dot{b}}_{w_t}\end{array}\right]=\left[\begin{array}{ccccc}0& \mathbf{I}& 0& 0& 0\\ 0& 0& -{\mathbf{R}}_t^{b_k}{\left[{\hat{a}}_t-b_{a_t}\right]}^{\wedge }& -{\mathbf{R}}_t^{b_k}& 0\\ 0& 0& -{\left[{\hat{w}}_t-b_{w_t}\right]}^{\wedge }& 0& -\mathbf{I}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\delta {\alpha }_t^{b_k}\\ \delta {\beta }_t^{b_k}\\ \delta {\theta }_t^{b_k}\\ \delta b_{a_t}\\ \delta b_{w_t}\end{array}\right]+\left[\begin{array}{cccc}0& 0& 0& 0\\ -{\mathbf{R}}_t^{b_k}& 0& 0& 0\\ 0& -\mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}{\mathbf{n}}_a\\ {\mathbf{n}}_w\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_{we}}\end{array}\right]\\ ={\mathbf{F}}_t\delta {\mathbf{z}}_t^{b_k}+{\mathbf{G}}_t{\mathbf{n}}_t\end{array}$$

• 其中：
  • $F_t^{15\times 15}$
  • $G_t^{15\times 12}$
  • $\delta z_t^{b_{k}15\times 1}$
  • $n_t^{12\times 1}$

4.协方差与雅可比

根据公式：
$$\delta {\dot{z}}_t^{b_k}=F_t\delta z_t^{b_k}+G_t{n}_t$$
可得到：
$$\begin{array}{rl}\delta z_{t+\delta t}^{b_k}& =\delta z_t^{b_k}+\delta {\dot{z}}_t^{b_k}\delta t\\ & =\left(I+F_t\delta t\right)\delta z_t^{b_k}+\left(G_t\delta t\right)n_t\\ & =F\delta z_t^{b_k}+V{n}_t\end{array}$$
其中，$F=I+F_t\delta t$，$V=G_t\delta t$
因此，根据当前时刻的值，预测下一时刻的协方差为：
$$P_{t+\delta t}^{b_k}=\left(I+F_t\delta t\right)P_t^{b_k}{\left(I+F_t\delta t\right)}^T+\left(G_t\delta t\right)Q_t{\left(G_t\delta t\right)}^T,t\in \left[k,k+1\right]$$
其中，
$$Q^{12\times 12}=\left[\begin{array}{cccc}{\delta }_a^2& 0& 0& 0\\ 0& {\delta }_w^2& 0& 0\\ 0& 0& {\delta }_{b_a}^2& 0\\ 0& 0& 0& {\delta }_{b_w}^2\end{array}\right]$$
雅可比矩阵更新公式为：
$$J_{t+\delta t}=\left(I+F_t\delta t\right)J_t$$
其中，jacobian的初值为I