This is a screenshot of my EKF-SLAM code as it is right now. The blue line is the EKF estimate while the red line is the true trajectory. I took inspiration from https://github.com/lucarinelli/EKF_localization_python/blob/master/EKF_localization.ipynb and https://github.com/AtsushiSakai/PythonRobotics/blob/master/SLAM/EKFSLAM/ekf_slam.py to structure my code and http://www.iri.upc.edu/people/jsola/JoanSola/objectes/curs_SLAM/SLAM2D/SLAM%20course.pdf as a guide to how EKF-SLAM works. However, I've found issues running these two code examples because the first does not seem to update the covariance matrix correctly and the second has issues with the matrix multiplication, making it difficult for me to understand what to do in my code. I will have to look into more code samples to see if I can find something similar and fix my code.