#!/usr/bin/env python3
|
"""分析航向角误差"""
|
import math
|
|
# 从log中提取的数据
|
pos_x, pos_y = 1.89, -2.81 # 当前位置
|
tgt_x, tgt_y = 5.40, -7.17 # 目标位置
|
current_hdg = 308.13 # 当前航向角(度)
|
|
# 计算期望航向（数学坐标系：东=0°，逆时针为正）
|
dx = tgt_x - pos_x
|
dy = tgt_y - pos_y
|
dist = math.sqrt(dx**2 + dy**2)
|
|
# 数学坐标系的角度
|
math_angle_rad = math.atan2(dy, dx)
|
math_angle_deg = math.degrees(math_angle_rad)
|
|
# 转换为罗盘角度（北=0°，顺时针为正）
|
compass_angle_deg = 90.0 - math_angle_deg
|
if compass_angle_deg < 0:
|
compass_angle_deg += 360
|
if compass_angle_deg >= 360:
|
compass_angle_deg -= 360
|
|
print(f"当前位置: ({pos_x:.2f}, {pos_y:.2f})")
|
print(f"目标位置: ({tgt_x:.2f}, {tgt_y:.2f})")
|
print(f"位移向量: dx={dx:.2f}m, dy={dy:.2f}m, dist={dist:.2f}m")
|
print(f"\n期望航向角(数学坐标系，东=0°): {math_angle_deg:.1f}°")
|
print(f"期望航向角(罗盘坐标系，北=0°): {compass_angle_deg:.1f}°")
|
print(f"当前航向角(IM23A报告): {current_hdg:.1f}°")
|
print(f"\n航向误差: {current_hdg - compass_angle_deg:.1f}°")
|
print(f"如果航向反了180°，实际车头方向: {(current_hdg + 180) % 360:.1f}°")
|
print(f"那么航向误差应该是: {((current_hdg + 180) % 360) - compass_angle_deg:.1f}°")
|
