我最近在python中实现了一个扩展卡尔曼滤波器，它采用机器人里程计和激光测距仪输入。但是，它没有按预期工作，所以我在每一步都记录了我的协方差矩阵，试图找出错误。
我认为问题是在增加第二个里程碑期间。
以下显示了每个步骤，因为我将P协方差矩阵从零地标增长到两个地标。在这种情况下，机器人首先向前驱动53mm。
状态向量，前后添加地标见于：970mm范围，23度。
[[ 53.] | [[ 53. ]
[ 0.] | [ 0. ]
[ 0.]] | [ 0. ]
| [ 944.969203 ]
| [ 378.61846351]]
P，前后添加地标协方差（RR是否显示为正确？给定状态^）：
[[ 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. ]
状态向量，后添加第二个地标：813mm范围，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.]]
导致最后一部分极值的原因是什么，以前的步骤是否正常？提前谢谢。