これは些細な質問かもしれませんが、満足のいく解決策が見つかりません。
x、y 座標が指定されている 2 つの回転オブジェクト間の距離 (角度、放射) を計算したいと思います。(xc=0.0, yc=0.0, r=0.0)オブジェクトが円上を回転すると仮定しましょう。関数 'atan2' を使用して、関連する角度をポイント 1 と 2 にそれぞれ取得できa1ますa2。
atan2結果を滑らかな関数に変換する小さな関数をコーディングしました0: 2pi。
と の場合a1 = 0.3、正常a2 = 0.2にa1 - a2動作します。ただし、最初のオブジェクトが最初のラップを完了し、2 番目のオブジェクトがまだ遅れている場合にどのように対応すればよいでしょうか? たとえば、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が進んでいる場合、角度は正、それ以外の場合は負になります これは、ある種の速度というよりも推定の問題であり、ラッピングはありません