0

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

我想计算两个旋转物体之间的距离（角度，辐射），它们的 x,y 坐标是给定的。(xc=0.0, yc=0.0, r=0.0)让我们假设对象在圆上旋转。我可以使用函数“atan2”来分别检索到点一和二的关联a1角度a2。

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

如果a1 = 0.3和a2 = 0.2,a1 - a2工作正常。但是，如果第一个对象已经完成了第一圈，而第二个对象仍然落后，如何适应呢？例如a1=0.1和a2=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在前面，则角度为正，否则为负 这更多是估计问题而不是某种速率，没有重叠

4

0 回答 0