我最近在 python 中实现了一个扩展的卡尔曼滤波器，它采用机器人里程计和激光测距仪输入。但是，它没有按预期工作，所以我记录了每一步的协方差矩阵以尝试查找故障。
我认为问题在于添加第二个地标。
下面显示了当我将 P 协方差矩阵从零地标增加到两个地标时的每个步骤。在这种情况下，机器人首先向前行驶 53mm。
状态向量，添加地标前后的位置：970mm 范围，23 度。
[[ 53.] | [[ 53. ]
[ 0.] | [ 0. ]
[ 0.]] | [ 0. ]
| [ 944.969203 ]
| [ 378.61846351]]
P，添加地标协方差前后（RR 是否出现 cov 正确？给定状态 ^）：
[[ 521.15 5141.15 521.15 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[[ 521.15 5141.15 521.15 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 530.85 5141.15 0. 0. ]
[ 0. 0. 0. 0. 2809. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
P，添加 Robot-Landmark 和 Landmark-Robot 交叉方差后。
[[ 521.15 5141.15 521.15 521.15 5141.15 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 521.15 0. 0. 530.85 5141.15 0. 0. ]
[ 5141.15 0. 0. 0. 2809. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
状态向量，添加第二个地标后看到：813 毫米范围，53 度。
[[ 53. ]
[ 0. ]
[ 0. ]
[ 944.969203 ]
[ 378.61846351]
[ 542.27561382]
[ 649.29066967]]
P，添加新的地标协方差和 RL、LR 交叉协方差后：
[[ 521.15 5141.15 521.15 521.15 5141.15 521.15 5141.15]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. 0. ]
[ 521.15 0. 0. 530.85 5141.15 0. 0. ]
[ 5141.15 0. 0. 0. 2809. 0. 0. ]
[ 521.15 0. 0. 0. 0. 529.28 5141.15]
[ 5141.15 0. 0. 0. 0. 0. 2809. ]]
P，添加地标-地标交叉方差后，这就是事情变得奇怪的地方：
[[ 5.21150000e+02 5.14115000e+03 5.21150000e+02 5.21150000e+02 5.14115000e+03 5.21150000e+02 5.14115000e+03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00]
[ 5.21150000e+02 0.00000000e+00 0.00000000e+00 5.30850000e+02 5.14115000e+03 2.71597322e+05 0.00000000e+00]
[ 5.14115000e+03 0.00000000e+00 0.00000000e+00 0.00000000e+00 2.80900000e+03 2.67931032e+06 0.00000000e+00]
[ 5.21150000e+02 0.00000000e+00 0.00000000e+00 2.71597322e+05 2.67931032e+06 5.29280000e+02 5.14115000e+03]
[ 5.14115000e+03 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00 2.80900000e+03]]
作为参考，这是我计算上述内容的方式：
if self.lmCount > 0:
lrm = matmult(self.jacobianJXR, matmult(self.covRR, self.crossVarRM[0:3, 0:(self.lmCount*2)]))
self.covMM[self.lmCount*2:((self.lmCount*2)+2), 0:(self.lmCount*2)] = lrm
self.covMM[0:(self.lmCount*2), self.lmCount*2:((self.lmCount*2)+2)] = lrm.T
jacobianJXR 状态：
[[ 1. 0. -0.]
[ 0. 1. 0.]]
是什么导致了最后一部分的极端值，前面的步骤是否正常？提前致谢。