i am currently building up a EKF SLAM. I dont know where my mistake is but i think it has something to do with the angle my robot is driving towards but im not completly sure. First i thought it could be a failure in Data association and new cone placement but in the picture below you can see the trajectorie my robot is driving when the positions are already known! So it´s basically just localization with a lidar sensor at the moment. I use the atan2 function. I would really appreciate your help, thanks!

enter image description here

This is currently my code for calculating the current angle from landmark to robot. I actually tried to play the atan2 function a bit and change it so it gives me negative values if x_local(3,1) [theta] crosses -pi but it didnt seem to improve very much. It still had areas like this.

%Nonlinear differential equations of the localization estimation problem
sigmax = cone_positions_cell{1,1}(cone_positions_cell{1,4}(j,1),1) - x_local(1,1); %X-position robot
sigmay = cone_positions_cell{1,1}(cone_positions_cell{1,4}(j,1),2) - x_local(2,1); %y-position robot

estimated_distance = sqrt(sigmax^2+sigmay^2);
lidar_angle = atan2((sigmay),(sigmax)); 

estimated_angle = lidar_angle - x_local(3,1); %x_local(3,1) is the robot´s angle

nonlinear_h = [estimated_distance; estimated_angle];


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.