How to find heading angle between two coordinates

54 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)




Community Treasure Hunt

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

Start Hunting!