两个旋转物体之间的角度距离

时间:2014-06-25 11:12:47

标签: python atan2

这可能是一个微不足道的问题,但我无法找到我满意的解决方案。

我想计算两个旋转物体之间的距离(角度,辐射角),它们的x,y坐标给出。 让我们假设对象在(xc=0.0, yc=0.0, r=0.0)圈上旋转。 我可以使用函数' atan2'检索相关角度分别指向第一和第二,a1a2

我编写了一个小函数,可将atan2结果转换为平滑函数0: 2pi

如果a1 = 0.3a2 = 0.2a1 - a2工作正常。 但是,当第一个物体完成第一圈时,如何适应情况,而第二个物体仍然落后?例如a1=0.1a2=6.2

修改

我找到了一个例子:

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)

我想知道这是不是太复杂,我错过了一些基本的触发技巧,没有太多的代码膨胀。

按惯例,如果a1在前方,则角度为正,否则为负  这更像是一个估计问题,而不是某种速率,没有问题

0 个答案:

没有答案