python - Issue calculating landmark covariances in EKF-SLAM -
i have implemented extended kalman filter in python, takes robot odometry , laser range finder inputs. however, not working expected, i've logged covariance matrix each step try , find faults.
i believe issue during addition of second landmark.
the following shows each step grow p covariance matrix 0 landmarks 2 landmarks. in case, robot first drives 53mm forward.
state vector, pre , post adding landmark seen at: 970mm range, 23 degrees.
[[ 53.] | [[ 53. ] [ 0.] | [ 0. ] [ 0.]] | [ 0. ] | [ 944.969203 ] | [ 378.61846351]] p, pre , 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 , 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 , 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 landmark-landmark cross variances, things 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, how calculate 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 causing extreme values on final section , previous steps appear normal? in advance.
Comments
Post a Comment