目录
-
- E EKF更新
- F 讨论
- 4 实验结果
- 5 结论
- 附录
- 参考文献
E EKF更新
在最后一节中，我们提出了一个测量模型，它表示从多个相机位置观察静态特征的几何约束。我们现在已经详细介绍了EKF在更新阶段，观察多被用于更新阶段。EKF以下两个事件之一触发更新：
- 当不再检测到许多图像中跟踪的特征时，使用第三个-D本节介绍的方法处理了该特征的所有测量。这种情况经常发生，因为特征移动到相机常发生。
- 每次记录新图像时，状态向量中包含当前相机位置估计的副本(见第三节)-C)。若相机位置的最大允许数量， N m a x N_{max} Nmax，至少一个旧的必须被删除。在丢弃状态之前，使用所有在相应时刻发生的特征观察，以使用它们的定位信息。在我们的算法中，我们选择了均匀的时间间隔 N m a x / 3 N_{max}/3 Nmax/3个位，从第二个最老的位置开始。在使用这些姿势的共同特征的约束下EKF更新后，这些被丢弃了。我们选择始终保持状态向量中最古老的位置，因为涉及更长时间的位置的几何限制通常对应于更大的基线，因此携带更有价值的定位信息。该方法在实际应用中取得了良好的效果。
我们稍后将详细讨论更新过程。考虑到给定的时间步长，必须选择上述两个标准 L L L处理特征的约束。根据上一节描述的步骤，参见公式(23)，我们计算残差向量 r o ( j ) , j = 1 , ? L r_o^{(j)},\ \ j = 1,\cdots L ro(j),j=1,⋯L，以及每个特征对应的测量矩阵 H o ( j ) , j = 1 ⋯ L H_o^{(j)}, \ \ j = 1\cdots L Ho(j), j=1⋯L。将所有残差堆叠到一个向量中，得到： r o = H X X ~ + n o (25) r_o=H_X\tilde{X}+n_o \tag{25} ro=HXX~+no(25) 其中 r o r_o ro和 n o n_o no是向量，它们分别带有元素 r o ( j ) r_o^{(j)} ro(j)和 n o ( j ) n_o^{(j)} no(j)，其中 j = 1 ⋯ L j=1\cdots L j=1⋯L， H X H_X HX是带有元素 H X ( j ) , j = 1 ⋯ L H_X^{(j)}, \ \ j = 1\cdots L HX(j), j=1⋯L的矩阵。
由于特征测量是统计独立的，噪声向量 n o ( j ) n_o^{(j)} no(j)是不相关的。因此，噪声向量 n o n_o no的协方差矩阵等于 R o = σ i m 2 I d R_o=\sigma_{im}^2I_d Ro=σim2Id，其中 d = ∑ j = 1 L ( 2 M j − 3 ) d = \sum_{j=1}^L(2M_j-3) d=∑j=1L(2Mj−3)是残差 r o r_o ro的维数。在实践中出现的一个问题是， d d d可能是一个相当大的数字。例如，如果在10个相机位姿中分别看到10个特征，则残差的尺寸为170。为了降低EKF更新的计算复杂度，我们对矩阵 H X H_X HX进行了QR分解[9]。具体地说，我们将这种分解表示为 H X = [ Q 1 Q 2 ] [ T H 0 ] H_X=\begin{bmatrix} Q_1 & Q_2 \end{bmatrix} \begin{bmatrix} T_H \\ 0 \end{bmatrix} HX=[Q1Q2][TH0] 其中 Q 1 Q_1 Q1和 Q 2 Q_2 Q2是酉矩阵，它们的列分别组成了 H X H_X HX的值域空间和零空间的基； T H T_H TH是上三角矩阵。根据这些定义，由公式（25）可得： r o = [ Q 1 Q 2 ] [ T H 0 ] X ~ + n o ⇒ (26) r_o=[Q_1 \ \ \ Q_2]\begin{bmatrix} T_H\\ 0 \end{bmatrix} \tilde{X}+n_o \Rightarrow \tag{26} ro=[Q1 Q2][TH0]X~+no⇒(26) [ Q 1 T r o Q 2 T r 0 ] = [ T H 0 ] X ~ + [ Q 1 T n o Q 2 T n o ] (27) \begin{bmatrix} Q_1^Tr_o \\ Q_2^T r_0 \end{bmatrix} = \begin{bmatrix} T_H \\ 0 \end{bmatrix} \tilde{X} + \begin{bmatrix} Q_1^T n_o \\ Q_2^T n_o \end{bmatrix} \tag{27} [Q1TroQ2Tr0]=[TH0]X~+[Q1TnoQ2Tno](27) 由上一个方程可知，通过将残差 r o r_o ro投影到 H x H_x Hx值域空间的基向量上，我们保留了测量中所有有用的信息。残差 Q 2 T r o Q_2^T r_o Q2Tro只是噪声，可以被完全丢弃。因此，我们使用以下残差来更新EKF，而不是公式(25)所示的残差： r n = Q 1 T r o = T H X ~ + n n (28) r_n=Q_1^T r_o=T_H \tilde{X} + n_n \tag{28} rn=Q1Tro=THX~+nn(28) 在这个表达式中， n n = Q 1 T n o n_n=Q_1^Tn_o nn=Q 标签： hoa传感器