仅通过锚点旋转结合距离与AoA测量估算标签位置的问题求助

问题背景 #

现有定位条件：

• 锚点位于原点$(0, 0)$
• 标签处于未知位置$(x, y)$
• 可测量锚点与标签间的距离$d$
• 可测量标签信号的到达角(AoA)$\theta$，以x轴为基准，范围为$0^{\circ$至$180}\circ$

定位方法尝试 #

首次测量 #

利用$d$和$\theta$计算标签的可能位置：
$$ x = d \cos(\theta), \quad y = \pm d \sin(\theta) $$
由于$\sin(\theta)$在对称角度下取值相同，得到一个$x$值和两个可能的$y$值。

锚点旋转与二次测量 #

将锚点旋转已知角度$\theta_{rot}$（锚点仍在原点，仅改变坐标轴方向），再次测量距离$d$（距离不变）和新的AoA$\theta'$，计算新的可能位置：
$$ x' = d \cos(\theta'), \quad y' = \pm d \sin(\theta') $$

代码实现 #

# 变量定义（注：原代码存在变量名笔误）
estimated_theta_aoa_0 = theta_aoa_0  # 弧度，0到pi之间
iestimated_theta_aoa_1 = theta_aoa_1  # 笔误：多了前缀i
iestimated_distance = d  # 笔误：多了前缀i
iestimated_theta_rotation = theta_rotation  # 笔误：多了前缀i

# 估算初始x和y
iestimated_x0 = d * np.cos(estimated_theta_aoa_0)
iestimated_y0_positive = d * np.sin(estimated_theta_aoa_0)

# 旋转初始估算位置
# 注：此处使用了疑似错误的旋转矩阵，且变量名不匹配（estimated_x0未定义，应为iestimated_x0）
iestimated_rotated_x1 = estimated_x0 * np.cos(theta_rotation) + estimated_y0_positive * np.sin(theta_rotation)
iestimated_rotated_y1 = - estimated_x0 * np.sin(theta_rotation) + estimated_y0_positive * np.cos(theta_rotation)

# 二次测量的位置估算
estimated_x1 = d * np.cos(estimated_theta_aoa_1)
iestimated_y1_positive = d * np.sin(estimated_theta_aoa_1)

# 通过y值匹配确定最终y0
estimated_y0 = estimated_y0_positive if estimated_rotated_y1 == estimated_y1_positive else -estimated_y0_positive

print(f'The estimation x and y position is: {estimated_x0}, {estimated_y0}') 

方法失效的核心原因 #

1. 浮点数精度导致的判断错误
   代码中用==直接比较两个浮点计算结果（estimated_rotated_y1 == estimated_y1_positive），但实际数值计算中，由于浮点运算的精度误差，两个理论上相等的值几乎不可能完全相等，直接导致判断逻辑失效。

2. 变量名笔误引发的计算错误
   原代码存在多处变量名错误，比如iestimated_theta_aoa_1、iestimated_distance等多了前缀i，但后续计算中又使用了无前缀的estimated_x0（实际应为iestimated_x0），这会导致变量未定义或者取值错误，直接破坏计算逻辑。

3. 旋转变换与测量坐标系的逻辑不匹配
   二次测量的$\theta'$是基于旋转后的新坐标轴的AoA，标签在新坐标系下的坐标$(x', y')$应该等于原始坐标$(x,y)$经过旋转变换后的结果。但代码中仅通过旋转后的y值与新测量的正y值比较来判断，忽略了x值的约束，且旋转矩阵的方向是否与锚点旋转方向匹配未验证，容易导致匹配逻辑错误。

4. 旋转矩阵方向可能错误
   标准的逆时针旋转变换公式为：
   $$ x' = x\cos\theta_{rot} - y\sin\theta_{rot} $$
   $$ y' = x\sin\theta_{rot} + y\cos\theta_{rot} $$
   而代码中使用的y旋转公式为-x*sinθ + y*cosθ，这对应顺时针旋转$\theta_{rot}$的变换。如果锚点实际是逆时针旋转，此公式会导致旋转后的坐标计算错误，进而无法匹配二次测量结果。

内容的提问来源于stack exchange，提问作者d3dalo