Issue calculating landmark covariances in EKF-SLAM

931 Views Asked by At

I have recently implemented an extended kalman filter in python, that takes robot odometry and laser range finder inputs. However, it is not working as expected, so I've logged my covariance matrix each step to try and find faults.

I believe the issue is during the addition of the second landmark.

The following shows each step as I grow my P covariance matrix from zero landmarks to two landmarks. In this case, the robot first drives 53mm forward.

State vector, pre and post adding landmark seen at: 970mm range, 23 degrees.

[[ 53.]   | [[  53.        ]
 [  0.]   |  [   0.        ]
 [  0.]]  |  [   0.        ]
          |  [ 944.969203  ]
          |  [ 378.61846351]]

P, pre and post adding landmark covariance (Does RR appear cov correct? Given state ^):

[[  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, post adding Robot-Landmark and Landmark-Robot cross-variances.

[[  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.  ]

State vector, post adding second landmark seen at: 813mm range, 53 degrees.

[[  53.        ]
 [   0.        ]
 [   0.        ]
 [ 944.969203  ]
 [ 378.61846351]
 [ 542.27561382]
 [ 649.29066967]]

P, post adding new landmark covariance and RL, LR cross covariances:

[[  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, post adding the landmark-landmark cross variances, this is where things get weird:

[[  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]]

For reference, this is how I calculate the above:

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

State of jacobianJXR:

[[ 1.  0. -0.]
[ 0.  1.  0.]]

What is causing the extreme values on the final section and do the previous steps appear normal? Thanks in advance.

0

There are 0 best solutions below