VINS 預積分 雅各比矩陣和協方差矩陣推導過程

VINS 預積分 雅各比矩陣和協方差矩陣推導過程

根據中值積分

$\omega_{k+1} = \frac{\omega_{k+1} + \omega_k}{2} - b_{\omega_{k+1}}$
$q_{k+1} = q_k \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \omega_k \delta t \end{bmatrix}$
$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 \begin{bmatrix} 1 \\ \delta \theta \end{bmatrix}$對該公式進行求導得
$\begin{bmatrix} 0 \\ \dot{\delta \theta} \end{bmatrix} = 2 \dot{\delta q}$

$\begin{bmatrix} 0 \\ \dot{\delta \theta_t} \end{bmatrix} = 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$
$=\begin{bmatrix} 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{bmatrix}\begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_t \end{bmatrix}$
$=\begin{bmatrix} 0 & -{\delta \omega}^T \\ \delta \omega & -\left[2 \omega_t + \delta \omega\right]_{\times} \end{bmatrix}\begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_t \end{bmatrix}$
根據上式整理得：
$\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}=Ra_B + g + \dot{\delta v} = Ra_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}^2I$
  協方差橢球是以原點爲中心的球面，意味着其均值和協方差在旋轉時是不變的。

$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}^2IR^T = {\sigma_a}^2I$

重新定義加速度計的噪聲：
$Rn_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}$

根據以上的計算結果整理成矩陣形式

$\begin{bmatrix} \delta \alpha_{k+1} \\ \delta \theta_{k+1} \\ \delta \beta_{k+1} \\ \delta b_{a_{k+1}} \\ \delta b_{g_{k+1}} \end{bmatrix}= \begin{bmatrix} 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{bmatrix} \begin{bmatrix} \delta \alpha_k \\ \delta \theta_k \\ \delta \beta_k \\ \delta b_{a_k} \\ \delta b_{g_k} \end{bmatrix}$
$+\begin{bmatrix} \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{bmatrix} \begin{bmatrix} n_{a_0} \\ n_{w_0} \\ n_{a_1} \\ n_{w_1} \\ n_{b_a} \\ n_{b_g} \end{bmatrix}$
其中：
$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} = FJ_k$
$P_{k+1} = FP_kF^T + VQV^T$