How can I correctly calculate the direction for a moving object?

I'm solving the following problem: I have an object and I know its position now and its position 300ms ago. I assume the obj开发者_如何学运维ect is moving. I have a point to which I want the object to get.

What I need is to get the angle from my current object to the destination point in such a format that I know whether to turn left or right.

The idea is to assume the current angle from the last known position and the current position.

I'm trying to solve this in MATLAB. I've tried using several variations with atan2 but either I get the wrong angle in some situations (like when my object is going in circles) or I get the wrong angle in all situations.

Examples of code that screws up:

a = new - old;
b = dest - new;
alpha = atan2(a(2) - b(2), a(1) - b(1);

where new is the current position (eg. x = 40; y = 60; new = [x y];), old is the 300ms old position and dest is the destination point.

Edit

Here's a picture to demonstrate the problem with a few examples:

In the above image there are a few points plotted and annotated. The black line indicates our estimated current facing of the object.

If the destination point is dest1 I would expect an angle of about 88°.

If the destination point is dest2 I would expect an angle of about 110°.

If the destination point is dest3 I would expect an angle of about -80°.

Firstly, you need to note the scale on the sample graph you show above. The x-axis ticks move in steps of 1, and the y-axis ticks move in steps of 20. The picture with the two axes appropriately scaled (like with the command axis equal) would be a lot narrower than you have, so the angles you expect to get are not right. The expected angles will be close to right angles, just a few degrees off from 90 degrees.

The equation Nathan derives is valid for column vector inputs a and b:

theta = acos(a'*b/(sqrt(a'*a) * sqrt(b'*b)));

If you want to change this equation to work with row vectors, you would have to switch the transpose operator in both the calculation of the dot product as well as the norms, like so:

theta = acos(a*b'/(sqrt(a*a') * sqrt(b*b')));

As an alternative, you could just use the functions DOT and NORM:

theta = acos(dot(a,b)/(norm(a)*norm(b)));

Finally, you have to account for the direction, i.e. whether the angle should be positive (turn clockwise) or negative (turn counter-clockwise). You can do this by computing the sign of the z component for the cross product of b and a. If it's positive, the angle should be positive. If it's negative, the angle should be negative. Using the function SIGN, our new equation becomes:

theta = sign(b(1)*a(2)-b(2)*a(1)) * acos(dot(a,b)/(norm(a)*norm(b)));

For your examples, the above equation gives an angle of 88.85, 92.15, and -88.57 for your three points dest1, dest2, and dest3.

NOTE: One special case you will need to be aware of is if your object is moving directly away from the destination point, i.e. if the angle between a and b is 180 degrees. In such a case you will have to pick an arbitrary turn direction (left or right) and a number of degrees to turn (180 would be ideal ;) ). Here's one way you could account for this condition using the function EPS:

theta = acos(dot(a,b)/(norm(a)*norm(b)));   %# Compute theta
if abs(theta-pi) < eps  %# Check if theta is within some tolerance of pi
  %# Pick your own turn direction and amount here
else
  theta = sign(b(1)*a(2)-b(2)*a(1))*theta;  %# Find turn direction
end

You can try using the dot-product of the vectors.

Define the vectors 'a' and 'b' as:

a = new - old;
b = dest - new;

and use the fact that the dot product is:

a dot b = norm2(a) * norm2(b) * cos(theta)

where theta is the angle between two vectors, and you get:

cos(theta) = (a dot b)/ (norm2(a) * norm2(b))

The best way to calculate a dot b, assuming they are column vectors, is like this:

a_dot_b = a'*b;

and:

norm2(a) = sqrt(a'*a);

so you get:

cos(theta) = a'*b/(sqrt((a'*a)) * sqrt((b'*b)))

Depending on the sign of the cosine you either go left or right

Essentially you have a line defined by the points old and new and wish to determine if dest is on right or the left of that line? In which case have a look at this previous question.