VINS是由香港科技大学秦通等人,于2018年提出的一种强大且通用的单目视觉惯性状态估计器,是初学者接触VIO 比较好的开源学习资料,作者的论文条理清晰,分为五个主要部分对该系统展开描述:测量预处理、初始化、后端非线性优化、闭环检测、闭环优化。本节介绍PVQ增量误差、协方差及雅可比推导。

基于误差的卡尔曼滤波

VINS中的误差卡尔曼滤波

通过对预积分量的推导,这里举一个简单的例子,比如小车,在 $k$ 时刻的位置如下图所示,我们通过IMU 数据,可以获得 $k+1$ 时刻的估计值 $x_1$,通过相机相邻帧之间的重投影误差可以获得相邻帧之间的最优位姿,抑或是通过GPS 信号,我们都可以得到 $k+1$ 时刻的观测值 $y_1$,然后我们基于估计值与观测值使用卡尔曼滤波进行预测,就可以得到在 $k+1$ 时刻的预测值 $x_1^{\prime}$,这个过程就是普通的卡尔曼滤波过程。

卡尔曼滤波

但是在VINS中,使用的则是基于误差的卡尔曼滤波的形式,同样地,我们通过IMU和视觉约束可以分别得到估计值 $x_1$ 和观测值 $y_1$,此时我们获取的是误差值,比如IMU的误差量为 $\Delta x$ 以及误差的置信度即协方差 $P_{\Delta x}$,视觉约束的误差量为 $\Delta y = y_1-x_1$ 以及误差的置信度即协方差 $P_{\Delta y}$,实际上在递推的过程中,估计值的误差量 $\Delta x$ 始终是接近 0 的,因为IMU 进行估计的过程中会给出最优的估计,并且误差的置信度 $P_{\Delta x}$ 是不断变小的(因为单单是IMU预测的话,走得越远,不确定性越大);通过将这四个参数值传入卡尔曼滤波进行预测,得到预测的误差量 $\Delta x^{\prime}$,然后就可以补偿到估计值,得到预测值 $x_1^{\prime}=x_1+\Delta x^{\prime}$,补偿之后,$\Delta x$ 归零;

误差卡尔曼滤波

使用误差卡尔曼滤波的原因

  1. 旋转的误差量的值通常很小。通常情况下,旋转误差量的值的范围在 $[-\pi,\pi]$ 之间,这就使得我们可以使用旋转向量表示误差量,而不是使用过参数化的四元数去表示,当然,旋转向量存在一个周期性的问题,但是由于误差量的范围在 $[-\pi,\pi]$ 之间,因此不存在这一问题;同时,旋转向量相比较四元数而言更容易表达协方差矩阵,因为四自由度的四元数的过参数化问题,导致很难直接去表达三自由度的旋转问题;
  2. 误差量非常小的情况下,误差量的二阶雅可比是完全可以忽略的,这样简化了求导的计算。

Ⅰ. 连续形式下

连续形式下,相邻帧之间的预积分量的推导:

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&=\iint_{t\in[t_k, t_{k+1}]}\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t}-n_a)dt^2\\ \beta ^{b_k}_{b_{k+1} }&=\int_{t\in[t_k, t_{k+1}]}\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t}-n_a)dt\\ \gamma^{b_k}_{b_{k+1} }&= \int_{t\in[t_k, t_{k+1}]} \frac{1}{2}\Omega(\hat{w}_t-b_{w_t}-n_w)\gamma^{b_k}_tdt \end{align*}\tag{I} $$

A. PVQ增量误差

IMU 在每一个时刻积分出来的值是有误差的,下面我们对误差进行分析。首先我们直
接给出在 t 时刻误差项的导数 :(即推导出 $(8)$ 的误差项的连续时间线性化方程)

$$ \begin{align*} \begin{bmatrix} \delta \dot{\alpha}^{b_k}_t\\\delta \dot{\beta}^{b_k}_t\\ \delta \dot{\theta}^{b_k}_t\\\delta \dot{b}_{a_t}\\\delta \dot{b}_{w_t}\\ \end{bmatrix}&= \begin{bmatrix} 0&\mathbf{I}&0&0&0\\ 0&0&-\mathbf{R}^{b_k}_t[\hat{a}_t-b_{a_t}]^{\land}&-\mathbf{R}^{b_k}_t&0\\ 0&0&-[\hat{w}_t-b_{w_t}]^{\land}&0&-\mathbf{I}\\ 0&0&0&0&0\\ 0&0&0&0&0\\ \end{bmatrix} \begin{bmatrix} \delta {\alpha}^{b_k}_t\\\delta {\beta}^{b_k}_t\\ \delta {\theta}^{b_k}_t\\\delta {b}_{a_t}\\\delta {b}_{w_t}\\ \end{bmatrix}+ \begin{bmatrix} 0&0&0&0\\ -\mathbf{R}^{b_k}_t&0&0&0\\ 0&-\mathbf{I}&0&0\\ 0&0&\mathbf{I}&0\\ 0&0&0&\mathbf{I}\\ \end{bmatrix} \begin{bmatrix}\mathbf{n}_a\\\mathbf{n}_w\\\mathbf{n}_{b_a}\\\mathbf{n}_{b_w}\end{bmatrix}\\ &=\mathbf{F}_t\delta\mathbf{z}^{b_k}_t+\mathbf{G}_t\mathbf{n}_t \end{align*}\tag{1} $$

其中:$\mathbf{F}^{15\times15}_t,\mathbf{G}^{15\times12}_t,\delta{\mathbf{z}^{b_k}_t}^{15\times1},\mathbf{n}^{12\times1}_t$,下面给出其推导过程。

在推导之前,引入两个概念:

①$true$: 真实测量值,包含了噪声 ,包括附加噪声和随机游走
②$nominal$:无噪声的理论值

  • 对于 $\dot{\beta}^{b_k}_t$ 的推导:

易知$\delta \dot{\beta}^{b_k}_t={\dot{\beta}^{b_k}_t}_{true}-{\dot{\beta}^{b_k}_t}_{nominal}$,我们分别推导${\dot{\beta}^{b_k}_t}_{true}$ 以及${\dot{\beta}^{b_k}_t}_{nominal}$ :

$$ \begin{align*} {\dot{\beta}^{b_k}_t}_{true} &={\mathbf{R}^{b_k}_t}_{true}({\hat{a}_t}_{true}-{b_{a_t} }_{true})\\ &=\mathbf{R}^{b_k}_t\exp(\delta\theta^{\land})(\hat{a}_t-b_{a_t}-n_a-\delta b_{a_t})\\ &=\mathbf{R}^{b_k}_t(I+\delta\theta^{\land})(\hat{a}_t-b_{a_t}-n_a-\delta b_{a_t})\\ &=\mathbf{R}^{b_k}_t[\hat{a}_t-b_{a_t}-n_a-\delta b_{a_t}+\delta\theta^{\land}(\hat{a}_t-b_{a_t})]\\ &=\mathbf{R}^{b_k}_t[\hat{a}_t-b_{a_t}-n_a-\delta b_{a_t}-(\hat{a}_t-b_{a_t})^{\land}\delta\theta]\\ {\dot{\beta}^{b_k}_t}_{nominal}&=\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t}) \end{align*}\tag{IA-1} $$

因此,可以得到:

$$ \begin{align*} \delta \dot{\beta}^{b_k}_t &={\dot{\beta}^{b_k}_t}_{true}-{\dot{\beta}^{b_k}_t}_{nominal}\\ &=\mathbf{R}^{b_k}_t[-n_a-\delta b_{a_t}-(\hat{a}_t-b_{a_t})^{\land}\delta\theta]\\ &=-\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t})^{\land}\delta\theta -\mathbf{R}^{b_k}_t\delta b_{a_t}-\mathbf{R}^{b_k}_tn_a \end{align*}\tag{IA-2} $$

  • 对于 $\dot{\theta}^{b_k}_t$ 的推导:

由公式上一节的公式

$$ \dot{\mathbf{q} }_t =\frac{1}{2}\begin{bmatrix}-w^{\land}&w\\-w^T&0\end{bmatrix}\mathbf{q}_t =\frac{1}{2}\Omega(w)\mathbf{q}_t =\frac{1}{2}\mathcal{R}\left(\begin{bmatrix}w\\0\end{bmatrix}\right)\mathbf{q}_t =\frac{1}{2}\mathbf{q}_t\otimes\begin{bmatrix}w\\0\end{bmatrix} $$

可知:

$$ \begin{align*} {\dot{\mathbf{q} }_t}_{true} &=\frac{1}{2}{\mathbf{q}_t}_{true}\otimes\begin{bmatrix}w_{ture}\\0\end{bmatrix}\\ &=\frac{1}{2}\mathbf{q}_t\otimes\delta\mathbf{q}\otimes \begin{bmatrix}\hat{w}_t-b_{w_t}-n_w-\delta b_{w_t}\\0\end{bmatrix}\\ {\dot{\mathbf{q} }_t}_{nominal}&=\dot{\mathbf{q} }_t\\ &= \frac{1}{2}\mathbf{q}_t\otimes\begin{bmatrix}\hat{w}_t-b_{w_t}\\0\end{bmatrix}\\ \end{align*}\tag{IA-3} $$

根据导数的性质,又有:

$$ \begin{align*} {\dot{\mathbf{q} }_t}_{true}&=\dot{\overbrace{\mathbf{q}_t\otimes\delta \mathbf{q} } }\\ &=\dot{\mathbf{q} }_t\otimes\delta \mathbf{q}+\mathbf{q}_t\otimes\dot{\delta \mathbf{q} }\\ &=\frac{1}{2}\mathbf{q}_t\otimes\begin{bmatrix}\hat{w}_t-b_{w_t}\\0\end{bmatrix}\otimes\delta \mathbf{q}+\mathbf{q}_t\otimes\dot{\delta \mathbf{q} }\\ \end{align*}\tag{IA-4} $$

综合$ (\mathrm{IA-3})$ 和 $ (\mathrm{IA-4})$:

$$ \begin{align*} &\frac{1}{2}\mathbf{q}_t\otimes\delta\mathbf{q}\otimes \begin{bmatrix}\hat{w}_t-b_{w_t}-n_w-\delta b_{w_t}\\0\end{bmatrix} =\frac{1}{2}\mathbf{q}_t\otimes\begin{bmatrix}\hat{w}_t-b_{w_t}\\0\end{bmatrix}\otimes\delta \mathbf{q}+\mathbf{q}_t\otimes\dot{\delta \mathbf{q} }\\ &\Leftrightarrow \frac{1}{2}\delta\mathbf{q}\otimes \begin{bmatrix}\hat{w}_t-b_{w_t}-n_w-\delta b_{w_t}\\0\end{bmatrix} =\frac{1}{2}\begin{bmatrix}\hat{w}_t-b_{w_t}\\0\end{bmatrix}\otimes\delta \mathbf{q}+\dot{\delta \mathbf{q} }\\ &\Leftrightarrow 2\dot{\delta \mathbf{q} }=\delta\mathbf{q}\otimes\begin{bmatrix}\hat{w}_t-b_{w_t}-n_w-\delta b_{w_t}\\0\end{bmatrix}-\begin{bmatrix}\hat{w}_t-b_{w_t}\\0\end{bmatrix}\otimes\delta \mathbf{q}\\ &\Leftrightarrow 2\dot{\delta \mathbf{q} }=\mathcal{R}\left(\begin{bmatrix}\hat{w}_t-b_{w_t}-n_w-\delta b_{w_t}\\0\end{bmatrix} \right)\delta \mathbf{q}-\mathcal{L}\left(\begin{bmatrix}\hat{w}_t-b_{w_t}\\0\end{bmatrix} \right)\delta\mathbf{q}\\ &\Leftrightarrow 2\dot{\delta \mathbf{q} }=\left( \begin{bmatrix}-(\hat{w}_t-b_{w_t}-n_w-\delta b_{w_t})^{\land}&\hat{w}_t-b_{w_t}-n_w-\delta b_{w_t}\\-(\hat{w}_t-b_{w_t}-n_w-\delta b_{w_t})^T&0 \end{bmatrix} -\begin{bmatrix}(\hat{w}_t-b_{w_t})^{\land}&\hat{w}_t-b_{w_t}\\-(\hat{w}_t-b_{w_t})^T&0\end{bmatrix} \right)\begin{bmatrix}\frac{\delta\theta}{2}\\1\end{bmatrix}\\ &\Leftrightarrow 2\dot{\delta \mathbf{q} }= \begin{bmatrix}-(2\hat{w}_t-2b_{w_t}-n_w-\delta b_{w_t})^{\land}&-n_w-\delta b_{w_t}\\(n_w+\delta b_{w_t})^T&0 \end{bmatrix}\begin{bmatrix}\frac{\delta\theta}{2}\\1\end{bmatrix} \end{align*} $$

上式左侧也可以写成:

$$ 2\dot{\delta \mathbf{q} }=\begin{bmatrix}\dot{\delta\theta}\\0\end{bmatrix} =\begin{bmatrix}-(2\hat{w}_t-2b_{w_t}-n_w-\delta b_{w_t})^{\land}&-n_w-\delta b_{w_t}\\(n_w+\delta b_{w_t})^T&0 \end{bmatrix}\begin{bmatrix}\frac{\delta\theta}{2}\\1\end{bmatrix} $$

故而,可得到:

$$ \begin{align*} \dot{\delta\theta}&=-(2\hat{w}_t-2b_{w_t}-n_w-\delta b_{w_t})^{\land}\frac{\delta\theta}{2}-n_w-\delta b_{w_t}\\ &\approx-(\hat{w}_t-b_{w_t})^{\land}\delta\theta-n_w-\delta b_{w_t} \end{align*}\tag{IA-5} $$

  • 其他的推导

对于 $\delta \dot{\alpha}^{b_k}_t$ 而言,我们知道加速度和速度之间就是相差一个导数,因此:$\delta \dot{\alpha}^{b_k}_t=\delta \beta^{b_k}_t$

对于 $\delta \dot{b}_{a_t}$ 和 $\delta \dot{b}_{w_t}$ ,已知加速度偏差和陀螺仪偏置被建模为随机游走,其导数为高斯,$n_{b_a}\sim\mathcal{N}(0, \sigma^2_{b_a}),n_{b_w}\sim\mathcal{N}(0, \sigma^2_{b_w})$:

$$ \dot{b}_{a_t}=n_{b_a},\ \ \ \ \ \ \dot{b}_{w_t}=n_{b_w} $$

故而: $\delta \dot{b}_{a_t}=\delta\mathbf{n_{b_a} }$ , $\delta \dot{b}_{w_t}=\delta\mathbf{n_{b_w} }$

B. 协方差及雅可比

对于公式 $(1)$ 可以简写为:

$$ \delta\dot{\mathbf{z} }^{b_k}_t=\mathbf{F}_t\delta\mathbf{z}^{b_k}_t+\mathbf{G}_t\mathbf{n}_t\tag{2} $$

根据导数定义可知:$\delta\dot{\mathbf{z} }^{b_k}_t=\underset{\delta t\rightarrow0}{\lim}\frac{\delta\mathbf{z}^{b_k}_{t+\delta t}+\delta\mathbf{z}^{b_k}_t}{\delta t}$

$$ \begin{align*} \delta\mathbf{z}^{b_k}_{t+\delta t}&=\delta\mathbf{z}^{b_k}_{t}+\delta\dot{\mathbf{z} }^{b_k}_t \delta t\\ &=(\mathbf{I}+\mathbf{F}_t\delta t)\delta\mathbf{z}^{b_k}_t+(\mathbf{G}_t\delta t)\mathbf{n}_t\\ &=\mathbf{F}\delta\mathbf{z}^{b_k}_t+\mathbf{V}\mathbf{n}_t \end{align*}\tag{3} $$

其中,$\mathbf{F}=\mathbf{I}+\mathbf{F}_t\delta t,\mathbf{V}=\mathbf{G}_t\delta t$。

这里我们对公式 $(3)$ 的 IMU 误差运动方程再说明,将上式和 EKF 对比可知,上式恰好给出了如 EKF 一般对非线性系统线性化的过程,这里的意义是表示下一个时刻的 IMU 测量误差与上一个时刻的成线性关系,这样我们根据当前时刻的值,可以预测出下一个时刻的均值和协方差,而公式 $(3)$ 给出的是均值预测,协方差预测公式如下:

$$ \mathbf{P}^{b_k}_{t+\delta t}= (\mathbf{I}+\mathbf{F}_t\delta t)\mathbf{P}^{b_k}_{t}(\mathbf{I}+\mathbf{F}_t\delta t)^T+ (\mathbf{G}_t\delta t)\mathbf{Q}_t(\mathbf{G}_t\delta t)^T,t\in[k,k+1]\tag{4} $$

上式给出了协方差的迭代公式,初始值 $\mathbf{P}^{b_k}_{b_k}=0$。其中, $\mathbf{Q}$ 为表示噪声项的对角协方差矩阵:

$$ \mathbf{Q}^{12\times12}= \begin{bmatrix}\delta^2_{a}&0&0&0\\0&\delta^2_{w}&0&0\\ 0&0&\delta^2_{b_a}&0\\ 0&0&0&\delta^2_{b_w}\\ \end{bmatrix}\tag{5} $$

另外根据 $(3)$ 式可获得误差项的 Jacobian 的迭代公式:

$$ \mathbf{J}_{t+\delta t}=(\mathbf{I}+\mathbf{F}_t\delta t)\mathbf{J}_\tag{6}t $$

其中,Jacobian 的初始值 $\mathbf{J}^{b_k}_{b_k}=\mathbf{I}$

Ⅱ. 离散形式下

离散时间下,相邻帧之间的预积分量的推导:

$$ \begin{align*} \hat{\alpha}^{b_k}_{i+1}&=\hat{\alpha}^{b_k}_{i}+\hat{\beta}^{b_k}_{i}\delta t+\frac{1}{2}\overline{\hat{a} }_i^{\prime}\delta t^2\\ \hat{\beta}^{b_k}_{i+1}&=\hat{\beta}^{b_k}_{i}+\overline{\hat{a} }_i^{\prime}\delta t\\ \hat{\gamma}^{b_k}_{i+1}&= \hat{\gamma}^{b_k}_{i}\otimes\hat{\gamma}^{i}_{i+1} =\hat{\gamma}^{b_k}_{i}\otimes \begin{bmatrix}1\\\frac{1}{2}\overline{\hat{w} }_i^{\prime}\delta t\end{bmatrix} \\ \end{align*}\tag{II} $$

其中,论文中按照欧拉积分,则:

$$ \begin{align*} \overline{\hat{a} }_i^{\prime}&=\mathbf{R}(\hat{\gamma}^{b_k}_{i})(\hat{a}_i-b_{a_i})\\ \overline{\hat{w} }_i^{\prime}&=\hat{w}_i-b_{w_i} \end{align*} $$

代码中则采用的是中值积分:

$$ \begin{align*} \overline{\hat{a} }_i^{\prime}&=\frac{1}{2} [\mathbf{q}_i(\hat{a}_i-b_{a_i})+\mathbf{q}_{i+1}(\hat{a}_{i+1}-b_{a_i})]\\ \overline{\hat{w} }_i^{\prime}&=\frac{1}{2}(\hat{w}_i+\hat{w}_{i+1})-b_{w_i} \end{align*} $$

A. PVQ增量误差

我们首先直接给出 PVQ 增量误差在离散形式下的矩阵形式,为了与代码一致,我们修改下变量顺序,这和代码中 midPointIntegration() 函数是一致的。 ( 但不知为何计算的 V 中与前四个噪声项相关的差个负号? )

$$ \begin{align*} \begin{bmatrix} \delta\alpha_{k+1}\\ \delta\theta_{k+1}\\ \delta\beta_{k+1}\\ \delta b_{a_{k+1} }\\ \delta b_{w_{k+1} }\\ \end{bmatrix} &=\begin{bmatrix} \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{bmatrix} \begin{bmatrix} \delta\alpha_k\\\delta\theta_k\\\delta\beta_k\\\delta b_{a_k}\\\delta b_{w_k}\\ \end{bmatrix} +\begin{bmatrix} 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{bmatrix} \begin{bmatrix} \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{bmatrix}\\ &=\mathbf{F} \begin{bmatrix} \delta\alpha_k\\\delta\theta_k\\\delta\beta_k\\\delta b_{a_k}\\\delta b_{w_k}\\ \end{bmatrix} +\mathbf{G} \begin{bmatrix} \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{bmatrix}\\ \end{align*}\tag{7} $$

$\mathbf{F},\mathbf{G}$为两个时刻间的协方差传递矩阵。

下面给出推导过程。

首先是两个比较简单的:

$$ \delta b_{a_{k+1} }=\delta b_{a_k}+\delta t\mathbf{n}_a \\ \delta b_{w_{k+1} }=\delta b_{w_k}+\delta t\mathbf{n}_w \\ $$

主要推导其他三个:$\delta\alpha_{k+1}, \delta\theta_{k+1}, \delta\beta_{k+1}$,分别推导:

  • 对于 $\delta\theta_{k+1}$ 的推导

由公式 $\mathrm{(IA-5)}$ 可知,角度误差的连续形式为:

$$ \dot{\delta\theta}\approx-(\hat{w}_t-b_{w_t})^{\land}\delta\theta-n_w-\delta b_{w_t} $$

则离散形式为:

$$ \dot{\delta\theta_k}\approx- \left(\frac{\hat{w}_k+\hat{w}_{k+1}}{2}-b_{w_k}\right)^{\land}\delta\theta_k- \frac{n_{w_k}+n_{w_{k+1} } }{2}-\delta b_{w_k} $$

由导数的定义:

$$ \begin{align*} \delta\theta_{k+1} &=\delta\theta_{k}+\dot{\delta\theta_{k} }\ \delta t\\ &=f_{22}\ {\color{red}\delta\theta_{k} }-\delta t\ {\color{red}\delta b_{w_k} } -\frac{\delta t}{2}{\color{red}n_{w_k} } -\frac{\delta t}{2}{\color{red}n_{w_{k+1} } } \end{align*}\tag{IIA-1} $$

其中,

$$ f_{22}=I- \left(\frac{\hat{w}_k+\hat{w}_{k+1}}{2}-b_{w_k}\right)^{\land}\delta t \tag{IIA-2} $$

  • 对于 $\delta\beta_{k+1}$ 的推导

由公式 $\mathrm{(IA-2)}$ 可知,速度误差的连续形式为:

$$ \delta \dot{\beta}^{b_k}_t =-\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t})^{\land}\delta\theta -\mathbf{R}^{b_k}_t\delta b_{a_t}-\mathbf{R}^{b_k}_tn_a $$

则离散形式为:

$$ \delta \dot{\beta}^{b_k}_t =-\frac{ \mathbf{R}^{b_k}_k(\hat{a}_k-b_{a_k})^{\land}\delta\theta_k+ \mathbf{R}^{b_{k} }_{k+1} (\hat{a}_{k+1} -b_{a_k})^{\land}\delta\theta_{k+1} } {2}-\frac{\mathbf{R}^{b_k}_k+\mathbf{R}^{b_{k} }_{k+1} } {2}\delta b_{a_k} -\frac{\mathbf{R}^{b_k}_kn_{a_k}+\mathbf{R}^{b_{k} }_{k+1}n_{a_{k+1} } }{2} $$

将公式 $\mathrm{(IIA-1)}$ 代入上式:

$$ \begin{align*} \delta \dot{\beta}^{b_k}_t= &-\frac{1}{2}\mathbf{R}^{b_k}_k(\hat{a}_k-b_{a_k})^{\land}\delta\theta_k\\ &-\frac{1}{2}\mathbf{R}^{b_{k} }_{k+1} (\hat{a}_{k+1} -b_{a_k})^{\land} \left\{ \left[ I- \left(\frac{\hat{w}_k+\hat{w}_{k+1}}{2}-b_{w_k}\right)^{\land}\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}(\mathbf{R}^{b_k}_k+\mathbf{R}^{b_{k} }_{k+1})\delta b_{a_k} -\frac{1}{2}\mathbf{R}^{b_k}_kn_{a_k} -\frac{1}{2}\mathbf{R}^{b_{k} }_{k+1}n_{a_{k+1} }\\ =&\left\{ -\frac{1}{2}\mathbf{R}^{b_k}_k(\hat{a}_k-b_{a_k})^{\land} -\frac{1}{2}\mathbf{R}^{b_{k} }_{k+1} (\hat{a}_{k+1} -b_{a_k})^{\land} \left[ I- \left(\frac{\hat{w}_k+\hat{w}_{k+1}}{2}-b_{w_k}\right)^{\land}\delta t \right] \right\}\delta\theta_{k}\\ &+\frac{\delta t}{4}\mathbf{R}^{b_{k} }_{k+1} (\hat{a}_{k+1} -b_{a_k})^{\land}n_{w_k} +\frac{\delta t}{4}\mathbf{R}^{b_{k} }_{k+1}(\hat{a}_{k+1} -b_{a_k})^{\land}n_{w_{k+1} } +\frac{\delta t}{2}\mathbf{R}^{b_{k} }_{k+1}(\hat{a}_{k+1} -b_{a_k})^{\land}\delta b_{w_k}\\ &-\frac{1}{2}(\mathbf{R}^{b_k}_k+\mathbf{R}^{b_{k} }_{k+1})\delta b_{a_k} -\frac{1}{2}\mathbf{R}^{b_k}_kn_{a_k} -\frac{1}{2}\mathbf{R}^{b_{k} }_{k+1}n_{a_{k+1} }\\ \end{align*} $$

同理,由导数的定义可知:

$$ \begin{align*} \delta \beta^{b_k}_{k+1}&=\delta \beta^{b_k}_k+\delta \dot{\beta}^{b_k}_t\ \delta t\\ =&f_{32}\ {\color{red}\delta\theta_k}+{\color{red}\delta \beta^{b_k}_k}-\frac{1}{2}(\mathbf{R}^{b_k}_k+\mathbf{R}^{b_{k} }_{k+1})\delta t\ {\color{red}\delta b_{a_k} } +f_{35}\ {\color{red}\delta b_{w_k} }\\ &-\frac{1}{2}\mathbf{R}^{b_k}_k\delta t\ {\color{red}n_{a_k} }+g_{32}\ {\color{red}n_{w_k} } -\frac{1}{2}\mathbf{R}^{b_{k} }_{k+1}\delta t\ {\color{red}n_{a_{k+1} } } +g_{34}\ {\color{red}n_{w_{k+1} } } \end{align*}\tag{IIA-3} $$

其中:

$$ \begin{align*} f_{32}&= -\frac{1}{2}\mathbf{R}^{b_k}_k(\hat{a}_k-b_{a_k})^{\land}\delta t -\frac{1}{2}\mathbf{R}^{b_{k} }_{k+1} (\hat{a}_{k+1} -b_{a_k})^{\land} \left[ I- \left(\frac{\hat{w}_k+\hat{w}_{k+1}}{2}-b_{w_k}\right)^{\land}\delta t \right]\delta t\\ f_{35}&=\frac{\delta t}{2}\mathbf{R}^{b_{k} }_{k+1}(\hat{a}_{k+1} -b_{a_k})^{\land}\delta t^2\\ g_{32}&=\frac{\delta t}{4}\mathbf{R}^{b_{k} }_{k+1} (\hat{a}_{k+1} -b_{a_k})^{\land}\delta t^2\\ g_{34}&=\frac{\delta t}{4}\mathbf{R}^{b_{k} }_{k+1} (\hat{a}_{k+1} -b_{a_k})^{\land}\delta t^2\\ \end{align*}\tag{IIA-4} $$

  • 对于 $\delta\alpha_{k+1}$ 的推导

我们知道加速度和速度之间就是相差一个导数,因此:$\delta \dot{\alpha}^{b_k}_t=\delta \beta^{b_k}_t$

则离散形式为:

$$ \begin{align*} \delta \dot{\alpha}^{b_k}_k=&\frac{1}{2}(\delta \beta^{b_k}_k+\delta \beta^{b_k}_{k+1})\\ =&\frac{1}{2}(\delta \beta^{b_k}_k+\delta \beta^{b_k}_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}(\mathbf{R}^{b_k}_k+\mathbf{R}^{b_{k} }_{k+1})\delta t\ \delta b_{a_k} -\frac{1}{2}\mathbf{R}^{b_k}_k\delta t\ n_{a_k} -\frac{1}{2}\mathbf{R}^{b_{k} }_{k+1}\delta t\ n_{a_{k+1} })\\ =&\delta \beta^{b_k}_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}(\mathbf{R}^{b_k}_k+\mathbf{R}^{b_{k} }_{k+1})\delta t\ \delta b_{a_k} -\frac{1}{4}\mathbf{R}^{b_k}_k\delta t\ n_{a_k} -\frac{1}{4}\mathbf{R}^{b_{k} }_{k+1}\delta t\ n_{a_{k+1} }\\ \end{align*} $$

同理,由导数的定义可知:

$$ \begin{align*} \delta\alpha_{k+1}&=\delta\alpha_{k}+\delta \dot{\alpha}^{b_k}_k\ \delta t\\ =&\delta\alpha_{k}+(\delta \beta^{b_k}_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}(\mathbf{R}^{b_k}_k+\mathbf{R}^{b_{k} }_{k+1})\delta t\ \delta b_{a_k} -\frac{1}{4}\mathbf{R}^{b_k}_k\delta t\ n_{a_k} -\frac{1}{4}\mathbf{R}^{b_{k} }_{k+1}\delta t\ n_{a_{k+1} })\delta t\\ =&\delta\alpha_{k}+\delta \beta^{b_k}_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}(\mathbf{R}^{b_k}_k+\mathbf{R}^{b_{k} }_{k+1})\delta t\ \delta b_{a_k} -\frac{\delta t}{4}\mathbf{R}^{b_k}_k\delta t\ n_{a_k} -\frac{\delta t}{4}\mathbf{R}^{b_{k} }_{k+1}\delta t\ n_{a_{k+1} }\\ =&{\color{red}\delta\alpha_{k} }+\frac{\delta t}{2}f_{32}\ {\color{red}\delta\theta_k}+\delta t\ {\color{red}\delta \beta^{b_k}_k}-\frac{\delta t}{4}(\mathbf{R}^{b_k}_k+\mathbf{R}^{b_{k} }_{k+1})\delta t\ {\color{red}\delta b_{a_k} }+\frac{\delta t}{2}f_{35}\ {\color{red}\delta b_{w_k} }\\ &-\frac{\delta t}{4}\mathbf{R}^{b_k}_k\delta t\ {\color{red}n_{a_k} } +\frac{\delta t}{2}g_{32}\ {\color{red}n_{w_k} } -\frac{\delta t}{4}\mathbf{R}^{b_{k} }_{k+1}\delta t\ {\color{red}n_{a_{k+1} } } +\frac{\delta t}{2}g_{34}\ {\color{red}n_{w_{k+1} } } \\ \end{align*}\tag{IIA-5} $$

所以:

$$ \begin{align*} f_{12}&=\frac{\delta t}{2}f_{32}\\ f_{14}&=-\frac{\delta t}{4}(\mathbf{R}^{b_k}_k+\mathbf{R}^{b_{k} }_{k+1})\delta t\\ f_{15}&=\frac{\delta t}{2}f_{35}\\ g_{11}&=-\frac{\delta t}{4}\mathbf{R}^{b_k}_k\delta t\\ g_{12}&=\frac{\delta t}{2}g_{32}\\ g_{13}&=-\frac{\delta t}{4}\mathbf{R}^{b_{k} }_{k+1}\delta t\\ g_{14}&=\frac{\delta t}{2}g_{34}\\ \end{align*}\tag{IIA-6} $$

B. 协方差及雅可比

公式 $(7)$ 可以简写为:

$$ \delta{\mathbf{z}_{k+1} }^{15\times1}=\mathbf{F}^{15\times15}\delta{\mathbf{z}_k}^{15\times1}+\mathbf{V}^{15\times18}\mathbf{Q}^{18\times1}\tag{8} $$

协方差的迭代公式为:

$$ {\mathbf{P}^{b_k}_{k+1} }^{15\times15} =\mathbf{F}\mathbf{P}^{b_k}_{k}\mathbf{F}^T+\mathbf{V}\mathbf{Q}\mathbf{V}^T\tag{9} $$

其中, 初始值 $\mathbf{P}^{b_k}_{b_k}=0$。 $\mathbf{Q}$ 为表示噪声项的对角协方差矩阵:

$$ \mathbf{Q}^{18\times18}= \begin{bmatrix} \sigma^2_{a}&0&0&0&0&0\\ 0&\sigma^2_{w}&0&0&0&0\\ 0&0&\sigma^2_{a}&0&0&0\\ 0&0&0&\sigma^2_{w}&0&0\\ 0&0&0&0&\sigma^2_{b_a}&0\\ 0&0&0&0&0&\sigma^2_{b_w}\\ \end{bmatrix}\tag{10} $$

另外根据 $(8)$ 式可获得误差项的 Jacobian 的迭代公式:

$$ {\mathbf{J}_{k+1} }^{15\times15}=\mathbf{F}\mathbf{J}_{k}\tag{11} $$

其中, Jacobian 的初始值为 $\mathbf{J}_k= \mathbf{I}$。 这里计算出来的 $\mathbf{J}_{k+1}$ 只是为了给后面提供对 $bias$ 的 Jacobian。

🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/65.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 07 月 08 日
赏杯咖啡