Question

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

我想计算两个旋转物体之间的距离（角度，辐射角），它们的x，y坐标给出。让我们假设对象在(xc=0.0, yc=0.0, r=0.0)圈上旋转。我可以使用函数＆＃39; atan2＆＃39;检索相关角度分别指向第一和第二，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在前方，则角度为正，否则为负这更像是一个估计问题，而不是某种速率，没有问题

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

0 个答案: