EKF SLAM predict

[Chris7462]

PythonRobotics/SLAM/EKFSLAM/ekf_slam.py

Line 33 in 73d1189

G, Fx = jacob_motion(xEst[0:S], u)

It seems to me that the input for jacob_motion should be xEst instead of xEst[0:S], and the dimension of G should be len(xEst) by len(xEst).

PythonRobotics/SLAM/EKFSLAM/ekf_slam.py

Line 35 in 73d1189

PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx

This line updates only the covariance of the pose; it does not update the covariance between the pose and the map, i.e. $\Sigma_{xm}$.