私は最近、ロボットのオドメトリとレーザー距離計の入力を受け取る拡張カルマン フィルターを Python で実装しました。ただし、期待どおりに機能していないため、各ステップで共分散行列をログに記録して、障害を見つけようとしました。
問題は、2 番目のランドマークの追加中にあると思います。
以下は、P 共分散行列を 0 ランドマークから 2 ランドマークに成長させる各ステップを示しています。この場合、ロボットは最初に 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. ]
状態ベクトル、2 番目のランドマークを追加するポスト: 813 mm の範囲、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.]]
最終セクションの極端な値の原因は何ですか?また、前のステップは正常に見えますか? 前もって感謝します。