I just started learning about slam and I have been trying to simulate a robot moving around a set of landmarks for the past 3 days. The landmarks have known correspondences.
My problem is, if I add motion noise to the covariance matrix in the prediction step, the robot starts to behave very weirdly. If I don't add motion noise in the prediction step, the robot will move around perfectly. I have been trying to figure out why this is happening for 3 days now but cannot find anything wrong with my code.
I have attached a link to github which has all the files pertaining to my project. In the folder named 'octave' the file 'prediction_step' and 'correction_step' contains code for the prediction and correction steps respectively. The ekf_slam file is the main loop which calls the above two functions.
My github repository also contains 3 videos which correspond to robot with no motion noise, robot with motion noise and another video which shows how the robot should ideally go about.
Please help me in figuring out what is wrong with my code in 'prediction_step' and 'correction_step'.
Link to my github repository: please click here
