Clear Filters
Clear Filters

How to find heading angle between two coordinates

63 views (last 30 days)
Hi there, I have a random generate point with heading angle (e.g. A = [X, Y, theta]) for my mobile robot and a fixed point, B = [4,5], and I wanted to calculate what is the angle (theta in radian) should my mobile robot tilt to point toward point B, which formula or method can I use?
A = [1+10*(rand(2,1)); pi*rand(1,1)]; % random generate point A (with theta)
B = [4,5]; % point B

Accepted Answer

Karim on 12 Jan 2023
see below for one approach
% random generate point position and direction of the robot
currPos = 1+10*(rand(1,2)); % the x-y coordinates
currAng = pi*rand(1,1); % the current heading
current_direction = [cos(currAng) sin(currAng) 0]; % create the vector for the current direction
current_direction = current_direction ./ sqrt( sum( current_direction.^2 )); % normalize the vector
% target point of the robot
targetPos = [4,5];
% now determine the angle between the current heading and the target position
target_direction = [targetPos - currPos 0];
target_direction = target_direction ./ sqrt( sum( target_direction.^2 )); % normalize the vector
% finaly determine the angle between the two directions
delta_theta = atan2(norm(cross(target_direction,current_direction)), dot(target_direction,current_direction))
delta_theta = 1.9253
% visualize the state
hold on
hold off
grid on
legend('target position','robot position','current heading','target heading','Location','best')
axis equal
  1 Comment
Shin on 12 Jan 2023
Hi karim, thanks for your reply. the code work as i wish, greatly appreciated!!

Sign in to comment.

More Answers (0)


Find more on ROS Toolbox in Help Center and File Exchange




Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!