A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation
摘要
本文介绍了一个基于扩展卡尔曼滤波器的算法，用于实时视觉辅助的惯性导航算法。本项工作的主要贡献是观测模型的导出，其能够表达从多个相机pose观察到静态特征所构成的几何约束。该测量模型不需要在EKF的状态向量中包含3D特征点的位置。
提出的视觉辅助惯性导航算法的计算复杂性只与特征点数量线性相关，并且能够在大型现实环境中进行高精度的姿态估计。
系统描述
目标是估计与IMU固定的坐标系相对于全局参考坐标系的3D位姿。
为了简化地球自转对IMU测量的影响，本文选取全局坐标系为ECEF坐标系，整体算法流程如算法1所示.
IMU测量被用于进行实时处理，用于传播EKF状态和协方差（Section III-B）。另一方面，每一帧图像到达时，相机的位姿估计被添加到状态向量中（Section III-C）。状态增广是为了处理特征观测所必须的，因为每个跟踪的特征点的观测用于在所有相机位姿中施加约束，从而用于对观测到该特征点的相机位姿进行约束。
因此，在任何时候，EKF状态向量都包含：
- IMU状态
- 过去Nmax帧相机位姿
EKF状态向量的结构
IMU状态：
\[ \mathbf{X}_{\mathrm{IMU} }=\left[\begin{array}{lllll} { }_{G}^{I} \bar{q}^{T} & \mathbf{b}_{g}{ }^{T} & { }^{G} \mathbf{v}_{I}^{T} & \mathbf{b}_{a}{ }^{T} & { }^{G} \mathbf{p}_{I}^{T} \end{array}\right]^{T} \]
其中，
- 单位四元数[19] \({ }_{G}^{I} \bar{q}\)用于描述从全局坐标系\(G\)到IMU坐标系\(I\)的旋转
- \({ }^{G} \mathbf{p}_{I}\)和\({ }^{G} \mathbf{v}_{I}\)分别描述了IMU位置和速度，相对于全局坐标系\(G\)
- \(b_g\)和\(b_a\)是 3x1的向量，描述IMU的bias，IMU Biases被建模为受高斯噪声\(n_{wg},n_{wa}\)驱动的随机游走过程。
因此，IMU的误差状态模型被定义为如下：
\[ \widetilde{\mathbf{X} }_{\mathrm{IMU} }=\left[\begin{array}{lllll} \boldsymbol{\delta} \boldsymbol{\theta}_{I}^{T} & \widetilde{\mathbf{b} }_{g}^{T} & { }^{G} \widetilde{\mathbf{v} }_{I}^{T} & \widetilde{\mathbf{b} }_{a}^{T} & { }^{G} \widetilde{\mathbf{p} }_{I}^{T} \end{array}\right]^{T} \]
对于位置、速度、biases，使用了标准的误差add定义，如位置误差\(\widetilde{x}=x-\hat{x}\)，然而，对于四元数，则使用另外的准则。
实际上，如果\(\hat{\bar{q} }\)是四元数\(\bar{q}\)的估计值，那么，旋转误差可以描述为四元数误差\(\delta \bar{q}\)，其中\(\bar{q}=\delta \bar{q} \otimes \hat{\bar{q} }\) (这个要区分误差是定义在哪里，这里跟ESKF一样，定义在局部的，只不过这里的q是从全局坐标系到机体坐标系)
此处，误差四元数如下：
\[ \delta \bar{q} \simeq\left[\begin{array}{ll} \frac{1}{2} \boldsymbol{\delta} \boldsymbol{\theta}^{T} & 1 \end{array}\right]^{T} \]
直观的，误差四元数\(\delta \bar{q}\)描述了一个估计姿态和真实姿态之间的小角度旋转。因为姿态包含了3个自由度，使用\(\delta \theta\)来描述姿态误差才是最小的表示。
假设在时间步骤k的EKF状态向量中包含N个相机姿势，该向量具有以下形式：
\[ \hat{\mathbf{X} }_{k}=\left[\begin{array}{llllll} \hat{\mathbf{X} }_{\mathrm{IMU}_{k} }^{T} & { }_{G}^{C_{1} } \hat{\bar{q} }^{T} & { }^{G} \hat{\mathbf{p} }_{C_{1} }^{T} & \ldots & { }_{G}^{C_{N} } \hat{q}^{T} & { }^{G} \hat{\mathbf{p} }_{C_{N} }^{T} \end{array}\right]^{T} \]
其中，\({}_G^{G_i} \hat{\bar{q} }, {}^G {\hat{p}_{C_i}, i=1\dots N}\)是N个相机的姿态和位置。
因此，EKF的误差状态向量定义如下：
\[ \widetilde{\mathbf{X} }_{k}=\left[\begin{array}{llllll} \widetilde{\mathbf{X} }_{\mathrm{IMU}_{k} }^{T} & \boldsymbol{\delta} \boldsymbol{\theta}_{C_{1} }^{T} & { }^{G} \widetilde{\mathbf{p} }_{C_{1} }^{T} & \ldots & \boldsymbol{\delta} \boldsymbol{\theta}_{C_{N} }^{T} & { }^{T} \widetilde{\mathbf{p} }_{C_{N} }^{T} \end{array}\right]^{T} \]
传播
滤波器传播的等式通过连续时间模型的离散化形式导出，定义如下：
连续时间系统模型
IMU状态微分方程描述：
\[ { }_{G}^{I} \dot{\bar{q} }(t)=\frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t))_{G}^{I} \bar{q}(t) \]
\[ \dot{\mathbf{b} }_{g}(t)=\mathbf{n}_{w g}(t) \]
\[ { }^{G} \dot{\mathbf{v} }_{I}(t)={ }^{G} \mathbf{a}(t) \]
\[ \dot{\mathbf{b} }_{a}(t)=\mathbf{n}_{w a}(t) \]
\[ { }^{G} \mathbf{p}_{I}(t)={ }^{G} \mathbf{v}_{I}(t) \]
其中，\({ }^{G} \mathbf{a}\)表示机体加速度在全局坐标系的表示，\(\boldsymbol{\omega}=\left[\begin{array}{lll}\omega_{x} & \omega_{y} & \omega_{z}\end{array}\right]^{T}\)表示IMU坐标系的旋转角速度。
另外的计算符号定义如下：
\[ \boldsymbol{\Omega}(\boldsymbol{\omega})=\left[\begin{array}{cc} -\lfloor\boldsymbol{\omega} \times\rfloor & \boldsymbol{\omega} \\ -\boldsymbol{\omega}^{T} & 0 \end{array}\right], \quad\lfloor\boldsymbol{\omega} \times\rfloor=\left[\begin{array}{ccc} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \end{array}\right] \]
陀螺仪和加速度计的测量描述如下 [20 ]：
\[ \boldsymbol{\omega}_{m}=\boldsymbol{\omega}+\mathbf{C}\left({ }_{G}^{I} \bar{q}\right) \boldsymbol{\omega}_{G}+\mathbf{b}_{g}+\mathbf{n}_{g} \]
\[ \begin{aligned} \mathbf{a}_{m}=& \mathbf{C}\left({ }_{G}^{I} \bar{q}\right)\left({ }^{G} \mathbf{a}-{ }^{G} \mathbf{g}+2\left\lfloor\boldsymbol{\omega}_{G} \times\right\rfloor^{G} \mathbf{v}_{I}+\left\lfloor\boldsymbol{\omega}_{G} \times\right\rfloor^{2}{ }^{G} \mathbf{p}_{I}\right) \\ &+\mathbf{b}_{a}+\mathbf{n}_{a} \end{aligned} \]
其中，
- \(C(\cdot)\)表示旋转矩阵
- \(n_g,n_a\)为零均值高斯白噪声
- 值得注意的是，IMU测量结合了星球的旋转，\(w_{G}\)的效果
- 此外，加速度计测量包括引力加速度,\({ }^{G} \mathbf{g}\), (expressed in the local frame)
在上面的连续时间状态方程中，应用这些运算符，就可以得到了IMU的状态估计方程：
\[ {}_{G}^{I} \dot{\hat{\bar{q} } }=\frac{1}{2} \boldsymbol{\Omega}(\hat{\boldsymbol{\omega} })_{G}^{I} \hat{\bar{q} } \]
\[ \dot{\hat{\mathbf{b} } }_{g}=\mathbf{0}_{3 \times 1} \]
\[ { }^{G} \dot{\hat{\mathbf{v} } }_{I}=\mathbf{C}_{\hat{q} }^{T} \hat{\mathbf{a} }-2\left\lfloor\boldsymbol{\omega}_{G} \times\right\rfloor^{G} \hat{\mathbf{v} }_{I}-\left\lfloor\boldsymbol{\omega}_{G} \times\right\rfloor^{2}{ }^{G} \hat{\mathbf{p} }_{I}+{ }^{G} \mathbf{g} \]
\[ \dot{\hat{\mathbf{b} } }_{a}=\mathbf{0}_{3 \times 1} \]
\[ { }^{G} \dot{\hat{\mathbf{p} } }_{I}={ }^{G} \hat{\mathbf{v} }_{I} \]
其中，
- \(\mathbf{C}_{\hat{q} }=C({}_G^{I}\hat{\bar{q} })\)
- \(\hat{a}=a_m-\bar{b}_{a}\)
- \(\hat{\omega}=\omega_{m}-\hat{b}_{g}-C_{\hat{q} }\omega_{G}\)
IMU误差状态的线性化连续时间模型如下表示：
\[ \dot{\widetilde{\mathbf{X} } }_{\mathrm{IMU} }=\mathbf{F} \tilde{\mathbf{X} }_{\mathrm{IMU} }+\mathbf{G} \mathbf{n}_{\mathrm{IMU} } \]
其中，
- \(\mathbf{n}_{\mathrm{IMU} }=\left[\begin{array}{llll}\mathbf{n}_{g}^{T} & \mathbf{n}_{w g}^{T} & \mathbf{n}_{a}^{T} & \mathbf{n}_{w a}^{T}\end{array}\right]^{T}\)是系统噪声
- 关于\(\mathbf{n}_{IMU}\)的协方差矩阵，\(\mathbf{Q}_{\mathrm{IMU} }\)，取决于IMU噪声特性，可以再传感器标定期间离线计算。
最后，F矩阵和G矩阵可以整理如下：
\[ \mathbf{F}=\left[\begin{array}{ccccc} -\lfloor\hat{\boldsymbol{\omega} } \times\rfloor & \mathbf{- I}_{3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ -\mathbf{C}_{\hat{q} }^{T}\lfloor\hat{\mathbf{a} } \times\rfloor & \mathbf{0}_{3 \times 3} & -2\left\lfloor\boldsymbol{\omega}_{G} \times\right\rfloor & -\mathbf{C}_{\hat{q} }^{T} & -\left\lfloor\boldsymbol{\omega}_{G} \times\right\rfloor^{2} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{I}_{3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \end{array}\right] \]
\[ \mathbf{G}=\left[\begin{array}{cccc} -\mathbf{I}_{3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{I}_{3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & -\mathbf{C}_{\hat{q} }^{T} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{I}_{3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \end{array}\right] \]
离散时间模型实现
由于IMU在周期T内进行采样，得到采样信号\(\omega_{m},a_{m}\)，每次接收到新的IMU测量时，IMU状态估计采用5阶龙哥库塔(RK-5)记性积分传播。
此外，EKF的协方差矩阵必须传播，因此，我们介绍对协方差的分区：
\[ \mathbf{P}_{k \mid k}=\left[\begin{array}{ll} \mathbf{P}_{I I_{k \mid k} } & \mathbf{P}_{I C_{k \mid k} } \\ \mathbf{P}_{I C_{k \mid k} }^{T} & \mathbf{P}_{C C_{k \mid k} } \end{array}\right] \]
其中，
- \(\mathbf{P}_{I I_{k \mid k} }\)是 15x15的协方差矩阵，关于IMU状态
- \(\mathbf{P}_{C C_{k \mid k} }\)是6Nx6N的协方差矩阵，关于相机位姿估计的
- \(\mathbf{P}_{I C_{k \mid k} }\)是IMU状态和相机位姿估计误差的相关性
通过这样的分块，传播状态的协方差矩阵按如下进行：
\[ \mathbf{P}_{k+1 \mid k}=\left[\begin{array}{cc} \mathbf{P}_{I I_{k+1 \mid k} } & \mathbf{\Phi}\left(t_{k}+T, t_{k}\right) \mathbf{P}_{I C_{k \mid k} } \\ \mathbf{P}_{I C_{k \mid k} }^{T} \mathbf{\Phi}\left(t_{k}+T, t_{k}\right)^{T} & \mathbf{P}_{C C_{k \mid k} } \end{array}\right] \]
其中，
- \(\mathbf{P}_{I I_{k+1 \mid k} }\)有李雅普诺夫(Lyapunov)等式进行数值积分得到：
\[ \dot{\mathbf{P} }_{I I}=\mathbf{F} \mathbf{P}_{I I}+\mathbf{P}_{I I} \mathbf{F}^{T}+\mathbf{G} \mathbf{Q}_{\mathrm{IMU} } \mathbf{G}^{T} \]
数值积分即以初始值\(\mathbf{P}_{I I_{k\mid k} }\)对时间间隔\((t_k,t_{k+T})\)进行积分。
- 误差转移矩阵\(\mathbf{\Phi}\left(t_{k}+T, t_{k}\right)\)由微分方程进行数值积分来近似得到：
\[ \dot{\boldsymbol{\Phi} }\left(t_{k}+\tau, t_{k}\right)=\mathbf{F} \boldsymbol{\Phi}\left(t_{k}+\tau, t_{k}\right), \quad \tau \in[0, T] \]
其中，初始条件为
\[ \mathbf{\Phi}\left(t_{k}, t_{k}\right)=\mathbf{I}_{15\times 15} \]
状态增广
当接受到新的图像时，首先聪IMU姿态估计来计算相机的姿态估计初值：
\[ _{G}^{C} \hat{\bar{q} }=\underset{I}{C} \bar{q} \otimes_{G}^{I} \hat{\bar{q} } \]
\[ { }^{G} \hat{\mathbf{p} }_{C}={ }^{G} \hat{\mathbf{p} }_{I}+\mathbf{C}_{\hat{q} }^{T}{ }^{I} \mathbf{p}_{C} \]
其中，
- \({}_I^C \bar{q}\)表示从IMU坐标系到相机坐标系的变换
- \({ }^{I} \mathbf{p}_{C}\)表示相对于IMU坐标系的原点，相机坐标系的位置。
由于相机位姿估计附加到状态向量中，因此EKF的协方差矩阵也进行增广:
\[ \mathbf{P}_{k \mid k} \leftarrow\left[\begin{array}{c} \mathbf{I}_{6 N+15} \\ \mathbf{J} \end{array}\right] \mathbf{P}_{k \mid k}\left[\begin{array}{c} \mathbf{I}_{6 N+15} \\ \mathbf{J} \end{array}\right]^{T} \]
其中，雅克比\(J\)根据式(14)进行微分推导 （谁对谁求导？）
式(14)截图如下： 
最后得到雅克比如下：
\[ \mathbf{J}=\left[\begin{array}{cccc} \mathbf{C}\left({ }_{I}^{C} \bar{q}\right) & \mathbf{0}_{3 \times 9} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 6 N} \\ \left\lfloor\mathbf{C}_{\hat{q} }^{T I} \mathbf{p}_{C} \times\right\rfloor & \mathbf{0}_{3 \times 9} & \mathbf{I}_{3} & \mathbf{0}_{3 \times 6 N} \end{array}\right] \]
测量模型
我们现在介绍用于更新状态估计的测量模型，这是本文的主要贡献。
由于EKF用于状态估计，因此对于构造测量模型，需要定义残差r ， 这取决于误差状态\(\widetilde{\mathbf{X} }\)，因此，根据通用形式，有：
\[ \mathbf{r}=\mathbf{H} \widetilde{\mathbf{X} }+ noise \]
在这个表达式中，H是测量雅比亚矩阵。对于EKF框架，应用的对于误差状态的噪声项必须是零均值、不相关的白噪声。
为了衍生我们的测量模型，我们的动机是通过多个相机的静态特征来实现涉及所有这些姿势的约束。在我们的工作中，相机观测按跟踪的特征进行分组，而不是每个相机姿势记录对应的观测（如[7,13,14]）。
同一个3D点的所有测量值都用于定于约束方程（在后面的等式24），与测量发生的所有相机姿势所相关联。这样的方法实现了在状态向量中可以不包含特征点的位置。
我们通过考虑单个特征 \(f_j\) 被多个相机pose\(\left({ }_{G}^{C_{i} } \bar{q},{ }^{G} \mathbf{p}_{C_{i} }\right), i \in \mathcal{S}_{j}\)所构成的集合\(M_{j}\)所共同观测到的情况来提出测量模型。 \(M_{j}\)中的每个观测都可以使用如下模型来描述：
\[ \mathbf{z}_{i}^{(j)}=\frac{1}{ { }^{C_{i} } Z_{j} }\left[\begin{array}{c} { }^{C_{i} } X_{j} \\ { }^{C_{i} } Y_{j} \end{array}\right]+\mathbf{n}_{i}^{(j)}, \quad i \in \mathcal{S}_{j} \]
其中，
- \(\mathbf{n}_{i}^{(j)}\)是2x1的图像噪声向量，具有协方差矩阵\(\mathbf{R}_{i}^{(j)}=\sigma_{i m}^{2} \mathbf{I}_{2}\)
- 特征点的位置\({ }^{C_{i} } \mathbf{p}_{f_{j} }\)被表示为在相机坐标系中，如下得到：
\[ { }^{C_{i} } \mathbf{p}_{f_{j} }=\left[\begin{array}{c} { }^{C_{i} } X_{j} \\ { }^{C_{i} } Y_{j} \\ { }^{C_{i} } Z_{j} \end{array}\right]=\mathbf{C}\left({ }_{G}^{C_{i} } \bar{q}\right)\left({ }^{G} \mathbf{p}_{f_{j} }-{ }^{G} \mathbf{p}_{C_{i} }\right) \]
其中，
- \({ }^{G} \mathbf{p}_{f_{j} }\)表示特征点在全局坐标系的3D位置，由于这是未知的，在我们的算法的第一步中，我们使用最小二乘最小化以获得估计值\({ }^{G} \hat{\mathbf{p} }_{f_{j} }\)。 这是利用观测值\(\mathbf{z}_{i}^{(j)}, i \in \mathcal{S}_{j}\)以及滤波器估计值关于相机姿态来实现的（具体参考附录）
一旦获得了特征位置的估计，我们计算观测的残差：
\[ \mathbf{r}_{i}^{(j)}=\mathbf{z}_{i}^{(j)}-\hat{\mathbf{z} }_{i}^{(j)} \]
其中，
\[ \hat{\mathbf{z} }_{i}^{(j)}=\frac{1}{ {}^{C_{i} } \hat{Z}_{j} }\left[\begin{array}{c} { }^{C_{i} } \hat{X}_{j} \\ { }^{C_{i} } \hat{Y}_{j} \end{array}\right] \]
\[ \left[\begin{array}{c} { }^{C_{i} } \hat{X}_{j} \\ { }^{C_{i} } \hat{Y}_{j} \\ { }^{C_{i} } \hat{Z}_{j} \end{array}\right]=\mathbf{C}\left({ }_{G}^{C_{i} } \hat{q}\right)\left({ }^{G} \hat{\mathbf{p} }_{f_{j} }-{ }^{G} \hat{\mathbf{p} }_{C_{i} }\right) \]
对上式( 等式20 )中关于相机位姿和特征点位置进行线性化，得到近似如下：
\[ \mathbf{r}_{i}^{(j)} \simeq \mathbf{H}_{\mathbf{X}_{i} }^{(j)} \widetilde{\mathbf{X} }+\mathbf{H}_{f_{i} }^{(j) G} \widetilde{\mathbf{p} }_{f_{j} }+\mathbf{n}_{i}^{(j)} \]
其中，
- \(\mathbf{H}_{\mathbf{X}_{i} }^{(j)}\)是观测\(\mathbf{z}_{i}^{(j)}\)对状态的雅克比
- \(\mathbf{H}_{f_{i} }^{(j)}\)是观测\(\mathbf{z}_{i}^{(j)}\)对特征点位置的雅克比 ( \(\mathbf{z}_{i}^{(j)}\) 难道不是直接从图像获取的特征点吗)
- \(\widetilde{\mathbf{X} }\)滤波器的误差状态
- \({ }^{G} \widetilde{\mathbf{p} }_{f_{j} }\) 关于特征点\(f_j\)的误差
- 上面H矩阵的具体推导可参见[21]
通过叠加关于特征点\(f_j\)的相机位姿集合\(M_j\)中所有的残差，可以得到：
\[ \mathbf{r}^{(j)} \simeq \mathbf{H}_{\mathbf{X} }^{(j)} \widetilde{\mathbf{X} }+\mathbf{H}_{f}^{(j) G} \widetilde{\mathbf{p} }_{f_{j} }+\mathbf{n}^{(j)} \]
其中，\(\mathbf{r}^{(j)}, \mathbf{H}_{\mathbf{X} }^{(j)}, \mathbf{H}_{f}^{(j)}\), and \(\mathbf{n}^{(j)}\)都是分别包含如下元素\(\mathbf{r}_{i}^{(j)}, \mathbf{H}_{\mathbf{X}_{i} }^{(j)}, \mathbf{H}_{f_{i} }^{(j)}\), and \(\mathbf{n}_{i}^{(j)}\)的向量或矩阵。 并且由于不同图像中的特征观测是独立的，因此\(\mathbf{n}^{(j)}\)的协方差矩阵为\(\mathbf{R}^{(j)}=\sigma_{\mathrm{im} }^{2} \mathbf{I}_{2 M_{j} }\)。
请注意，由于状态估计值\(\mathbf{X}\)，用于计算特征点的位置估计（参考附录），式(22) 即线性化后的残差等式与误差状态\(\mathbf{\tilde{X} }\)相关联。因此，残差\(\mathbf{r}^{(j)}\)并非如等式(17)的形式(\(\mathbf{r}=\mathbf{H} \tilde{\mathbf{X} }+\) noise)，不能直接用于EKF的测量更新步骤。
为了克服这个问题，我们通过将\(\mathbf{r}^{(j)}\)投影到矩阵\(\mathbf{H}_{f}^{(j)}\)的左零空间，从而定义了一个新的残差\(\mathbf{r}_{o}^{(j)}\)。特别的，如果我们使用酉矩阵来表示\(\mathbf{A}\)，其中它的列形成了关于\(\mathbf{H}_{f}\)左零空间中的bias （这说的啥意思）
\[ \begin{aligned} \mathbf{r}_{o}^{(j)} &=\mathbf{A}^{T}\left(\mathbf{z}^{(j)}-\hat{\mathbf{z} }^{(j)}\right) \simeq \mathbf{A}^{T} \mathbf{H}_{\mathbf{X} }^{(j)} \widetilde{\mathbf{X} }+\mathbf{A}^{T} \mathbf{n}^{(j)} \\ &=\mathbf{H}_{o}^{(j)} \widetilde{\mathbf{X} }^{(j)}+\mathbf{n}_{o}^{(j)} \end{aligned} \]
因为 \(2M_j \times 3\)的矩阵\(\mathbf{H}_{f}^{(j)}\)是列满秩的，他的左零空间维度为\(2 M_{j}-3\)。因此\(\mathbf{r}_{o}^{(j)}\)是\(\left(2 M_{j}-3\right) \times 1\)的向量。这种残差独立于特征坐标中的误差，因此可以基于它执行EKF更新。
式\(\mathbf{H}_{o}^{(j)} \widetilde{\mathbf{X} }^{(j)}+\mathbf{n}_{o}^{(j)}\)定义了所有观测到特征点\(f_j\)。这表达了测量\(\mathbf{z}_{i}^{(j)}\)为\(M_j\)的状态提供所有的有效信息，因此产生的EKF更新是最优的，除了由于线性化所引起的不准确性。
应该提到的是，为了计算残差\(\mathbf{r}_{O}^{(j)}\)和观测矩阵\(\mathbf{H}_{o}^{(j)}\)，酉矩阵\(\mathbf{A}\)不需要显式地被评估。相反，残差\(\mathbf{r}\)和矩阵\(\mathbf{H}_{\mathbf{X} }^{(j)}\)在\(\mathbf{H}_{f}^{(j)}\)矩阵左零空间的投影可以通过使用Givens旋转[22]来计算得到，操作复杂度为\(O\left(M_{j}^{2}\right)\)。另外，\(\mathbf{A}\)是酉矩阵，因此向量\(\mathbf{n}_{o}^{(j)}\)的协方差可以如下计算：
\[ E\left\{\mathbf{n}_{o}^{(j)} \mathbf{n}_{o}^{(j) T}\right\}=\sigma_{\mathrm{im} }^{2} \mathbf{A}^{T} \mathbf{A}=\sigma_{\mathrm{im} }^{2} \mathbf{I}_{2 M_{j}-3} \]
EKF更新
在前面的部分中，我们呈现了一种测量模型，其表示通过观察来自多个相机姿势的静态特征而施加的几何约束。 我们现在详细介绍了EKF的更新阶段，其中使用从观察多个特征的约束。 EKF更新由以下两个事件之一触发
- 当检测不到之前在多个图像跟踪的特征时，则使用第III-D部分中呈现的方法处理此特征的所有测量。 这种情况最常出现，因为特征点有可能在相机视野范围之外。
- 每次记录新图像时，当前相机姿势估计将被包含在状态向量中，如果已经达到了设定的最大相机位姿数\(N_{max}\)，则必须删除最少一个旧的相机位姿。在丢弃状态之前，使用在相应的时间瞬间发生的所有特征观测，以便利用其局部信息。在我们的算法中，从第二最旧的相机位姿开始，我们选择均匀间隔的\(\frac{N_{max} }{3}\)的位姿，在使用这些姿势共有的特征的约束执行 EKF 更新后，这些被丢弃。我们选择始终保持最古老的姿势在状态向量中，因为涉及及时进一步姿势的几何结构通常对应于较大的基线，因此携带更有价值的定位信息，这种方法在实践中表现得非常好。
考虑到在给定的时间步骤中，必须处理由上述两个标准选择的\(L\)个特征点的约束。遵循前一节中描述的过程，我们对每一个特征点计算残差向量\(\mathbf{r}_{o}^{(j)}, j=1 \ldots L\)以及相关联的观测矩阵\(\mathbf{H}_{o}^{(j)}, j=1 \ldots L\)。通过将所有残差堆叠在一个向量中，我们得到：
\[ \mathbf{r}_{o}=\mathbf{H}_{\mathbf{X} } \widetilde{\mathbf{X} }+\mathbf{n}_{o} \]
其中，
- \(\mathbf{r}_{o}\)的块元素为\(\mathbf{r}_{o}^{(j)}\)
- \(\mathbf{n}_{o}\)的块元素为\(\mathbf{n}_{o}^{(j)}, j=1 \ldots L\)
- \(\mathbf{H}_{\mathbf{X} }\)矩阵具有行块元素\(\mathbf{H}_{\mathbf{X} }^{(j)}, j=1 \ldots L\)
由于特征测量是统计上的，因此噪声向量\(\mathbf{n}_{o}^{(j)}\)是不相关的，因此，其协方差矩阵等价于\(\mathbf{R}_{o}=\sigma_{\mathrm{im} }^{2} \mathbf{I}_{d}\)，其中 \(d=\sum_{j=1}^{L}\left(2 M_{j}-3\right)\) 是残差\(\mathbf{r}_{o}\)的维度
一个实践中的问题是，\(d\)可以是一个相当大的数字，例如，如果10个特征点在10个相机位姿中都被观测到，那么残差的维度是170 {(2x10-3)x10=170}.
为了降低EKF更新的计算复杂度，我们采用QR分解，对\(\mathbf{H}_{\mathbf{X} }\)，特别的，我们记分解为如下形式：
\[ \mathbf{H}_{\mathbf{X} }=\left[\begin{array}{ll} \mathbf{Q}_{1} & \mathbf{Q}_{2} \end{array}\right]\left[\begin{array}{c} \mathbf{T}_{H} \\ \mathbf{0} \end{array}\right] \]
其中，
- Q1和Q2是关于矩阵\(\mathbf{H}_{\mathbf{X} }\)列形式的range和nullspace。
- \(\mathbf{T}_{H}\)是上三角矩阵
根据这个定义，等式(25)(\(\mathbf{r}_{o}=\mathbf{H}_{\mathbf{X} } \widetilde{\mathbf{X} }+\mathbf{n}_{o}\)) 可以产生如下形式：
\[ \begin{aligned} \mathbf{r}_{o} &=\left[\begin{array}{ll} \mathbf{Q}_{1} & \mathbf{Q}_{2} \end{array}\right]\left[\begin{array}{c} \mathbf{T}_{H} \\ \mathbf{0} \end{array}\right] \tilde{\mathbf{X} }+\mathbf{n}_{o} \Rightarrow \\ \left[\begin{array}{c} \mathbf{Q}_{1}^{T} \mathbf{r}_{o} \\ \mathbf{Q}_{2}^{T} \mathbf{r}_{o} \end{array}\right] &=\left[\begin{array}{c} \mathbf{T}_{H} \\ \mathbf{0} \end{array}\right] \widetilde{\mathbf{X} }+\left[\begin{array}{c} \mathbf{Q}_{1}^{T} \mathbf{n}_{o} \\ \mathbf{Q}_{2}^{T} \mathbf{n}_{o} \end{array}\right] \end{aligned} \]
从最后一个等式开始，通过投影\(\mathbf{H}_{\mathbf{X} }\)范围的基础向量，我们保留了测量中的所有有用信息。
残差中的\(\mathbf{Q}_{2}^{T} \mathbf{r}_{o}\)只是噪声，并且可以完全丢弃。因此，相比于使用等式(25)中的残差表示，我们使用下面形式的残差来执行EKF更新：
\[ \mathbf{r}_{n}=\mathbf{Q}_{1}^{T} \mathbf{r}_{o}=\mathbf{T}_{H} \widetilde{\mathbf{X} }+\mathbf{n}_{n} \]
其中，
- \(\mathbf{n}_{n}=\mathbf{Q}_{1}^{T} \mathbf{n}_{o}\) 是噪声向量，其协方差矩阵等价于\(\mathbf{R}_{n}=\mathbf{Q}_{1}^{T} \mathbf{R}_{o} \mathbf{Q}_{1}=\sigma_{\operatorname{im} }^{2} \mathbf{I}_{r}\)，且r为Q1的列数
EKF更新步骤计算卡尔曼增益：
\[ \mathbf{K}=\mathbf{P} \mathbf{T}_{H}^{T}\left(\mathbf{T}_{H} \mathbf{P} \mathbf{T}_{H}^{T}+\mathbf{R}_{n}\right)^{-1} \]
并且，矫正的状态按下式给出：
\[ \Delta \mathbf{X}=\mathbf{K} \mathbf{r}_{n} \]
最后，状态的协方差矩阵如下更新：
\[ \mathbf{P}_{k+1 \mid k+1}=\left(\mathbf{I}_{\xi}-\mathbf{K} \mathbf{T}_{H}\right) \mathbf{P}_{k+1 \mid k}\left(\mathbf{I}_{\xi}-\mathbf{K} \mathbf{T}_{H}\right)^{T}+\mathbf{K} \mathbf{R}_{n} \mathbf{K}^{T} \]
其中，
- \(\xi=6N+15\)是协方差矩阵的维度
审查EKF更新期间所需的操作的计算复杂性很有意思，残差 \(\mathbf{r}_{n}\)以及矩阵\(\mathbf{T}_{H}\)可以使用 Givens 旋转计算，操作复杂度是\(O\left(r^{2} d\right)\)，而无需显式地计算Q1的形式。
另一方面，等式(31)包含了\(\xi\)维度的方阵的乘法计算，是\(O\left(\xi^{3}\right)\)的操作。因此，EKF更新的复杂度是\(\max \left(O\left(r^{2} d\right), O\left(\xi^{3}\right)\right)\)。
另一方面，如果使用的残余向量\(\mathbf{r}_{o}\)，而不将其投影在\(\mathbf{H}_{\mathbf{X} }\)的range内，计算卡尔曼增益的计算成本是\(O\left(d^{3}\right)\)，然而，通常\(d \gg \xi, r\)，所以可知，使用残差\(\mathbf{r}_{n}\)可以减少计算量。
实验（暂略）
