# SLAM - Angle calculation failure ?-EKF

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!

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.

    %LIDAR
%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];