0

This may be well a trivial quesiton, but I can't find a solution I am pleased with.

I'd like to calculate the distance (angle, radiants) between two rotating objects, whose x,y coordinates are given. Let's assume the objects rotate on the (xc=0.0, yc=0.0, r=0.0) circle. I could use the function 'atan2' to retrieve the associated angle to point one and two, a1 and a2 respectively.

I have coded a small function that converts atan2 results to a smooth function 0: 2pi.

If a1 = 0.3and a2 = 0.2, a1 - a2 works fine. However, how to accomodate for the case when the first object has completed the fist lap, while the second is still lagging? For example a1=0.1 and a2=6.2.

Edit

I have worked out an example:

from math import *
import random


def estimate_angular_velocity(last_measurement, previous_measurement, old_angular_vel, circle_params, iter, h=0.5):
    def angle(meas, circle):
        x, y = meas
        xc, yc, r = circle
        calc_angle = math.atan2(y - yc, x - xc)
        if calc_angle < 0:
            return (2 * pi + calc_angle) % (2 * pi)
        else:
            return calc_angle % (2 * pi)

    def angle_distance(first_angle, second_angle):
        if abs(first_angle - second_angle) < abs(first_angle + (2*pi - second_angle)):
            error_modulus = abs(first_angle - second_angle) % (2 * pi)
            error_sign = math.copysign(1.0, first_angle - second_angle)
        else:
            error_modulus = abs(first_angle + (2*pi - second_angle)) % (2 * pi)
            error_sign = -math.copysign(1.0, first_angle - second_angle)
        return error_sign * error_modulus

    if old_angular_vel is None:
        return angle(last_measurement, circle_params) - angle(previous_measurement, circle_params)
    else:
        old_angle = angle(previous_measurement, circle_params)
        exp_angle = (old_angle + old_angular_vel) % (2 * pi)
        new_angle = angle(last_measurement, circle_params)
        error = angle_distance(new_angle, exp_angle)
        new_ang_velocity = old_angular_vel + h * error
        print "iter: %f, old: %f, exp: %f, new: %f, est_ang_vel: %f, err: %f" % (iter, old_angle, exp_angle, new_angle, new_ang_velocity, error)
        return new_ang_velocity


true_speed = -pi / 2.7
print "true speed: ", true_speed
est_speed = 0.0
est_speed2 = 0.0
r = 10.0
for i in range(1, 40):
    old_angle = true_speed * (i-1) + random.gauss(0.0, 0.1)
    new_angle = true_speed * i + random.gauss(0.0, 0.1)
    old_meas = (r * cos(old_angle), r * sin(old_angle))
    meas = (r * cos(new_angle), r * sin(new_angle))
    est_speed2 = estimate_angular_velocity(meas, old_meas, est_speed2, (0, 0, 9.9), i)

I was wondering if this isn't too convoluted and I have missed some basic trig tricks to do this without so much code bloat.

By convention, if a1 is ahead, the angle would be positive, negative otherwise This is more an estimation problem rather than some sort of rate, there are no lappings

NoIdeaHowToFixThis
  • 4,484
  • 2
  • 34
  • 69
  • `(xc=0.0, yc=0.0, r=0.0)` is point, not a circle. What angular distance do you want to be between `a1` and `a2` in the latter case? How do you know that `a2` isn't ahead of `a1` rather than the other way around? – martineau Jun 25 '14 at 11:42
  • @martineau - Thanks for looking into this. (1) (xc=0.0, yc=0.0, r=0.0) are actually the parameters that define a circle: the center and the radius. (2) If `a2` is adhead of `a1` then the function should return a negative angle. – NoIdeaHowToFixThis Jun 25 '14 at 11:58
  • A circle of radius zero is a point. In the `a1==0.1` and `a2==6.2` case, how do you know that `a1` is ahead of `a2` and not way behind it? – martineau Jun 25 '14 at 12:11
  • @martineu: yes, of course, I meant (1) (xc=0.0, yc=0.0, r>0.0), apologies. – NoIdeaHowToFixThis Jun 25 '14 at 12:15
  • I think your basic problem is that fact that you keep doing a `% (2 * pi)` operation on the computed angles which causes them to wrap back to zero. `atan` will work just fine given values > `2 * pi` so the modulo operation isn't necessary. Think of angles as measure of total revolutions around the circle, so a measure of `4 * pi` would mean two revolutions had been traveled. – martineau Jun 25 '14 at 13:17
  • @martineau - there's no way for me to know how many laps have been travelled; and in any case, given that the input is the position, I have to apply `atan2` (or some other variant of arctan) that will wrap back everything to -pi:pi – NoIdeaHowToFixThis Jun 25 '14 at 14:24
  • 2
    If all you know is the angular position of each object, the information you want simply isn't available. On the other hand if the objects are moving, something moves them, and in theory at least that thing should be able to determine the total angular movement or assist in computing it. – martineau Jun 25 '14 at 14:32

0 Answers0