我必须根据给定的环境地图估计位置。状态表示为

$X = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}$

给定，

$X_{t-1} = \begin{bmatrix} x_{t-1} \\ y_{t-1} \\ \theta_{t-1} \end{bmatrix}$这是协方差$P_{t-1},$一个 3X3 矩阵。

我有一个测量模型，它检测代表障碍物的线，因此，在一次测量中可以有任意数量的线，这些线表示为

我已经成功完成了所有步骤，直到计算创新协方差，即

$S = H_t\hat{P_t}H_t^T + R_t$,

在哪里$H_t$是测量雅可比行列式，$\hat{P_t}$是估计的运动协方差和$R_t$是测量噪声矩阵。

当我有单线测量时，测量雅可比，$H_t$是二维的$2\times 3$矩阵和测量噪声矩阵，$R_t$是$2\times 2$. 但是，当有多个线测量时，例如 8 线，$H_t$变成$2\times 3 \times 8$三维矩阵和$R_t$一种$2\times 2 \times 8$三维矩阵。

由于这种维度的变化，我无法进行创新协方差的计算$S$作为$\hat{P_t}$是一个$3 \times 3$矩阵。

如何合并多行读数并抑制三维矩阵以匹配创新协方差方程？