黑匣子搜寻方向角和俯仰角求解方法

 

1 前言

项目做黑匣子搜寻，需要计算黑匣子在AUV的方向角和俯仰角。以AUV为参考，右手坐标系，Z向下，如下图所示。已知黑匣子坐标(x, y, z)。

2 方向角

方向角指的是采用某坐标轴方向作为标准方向所确定的方位角。有时，方向角是从正北或正南方向到目标方向所形成的小于九十度的角。

 这里，以AUV右侧方向角为+，左侧方向角为-。以反正切函数为例，公式如下：

atan() 函数的功能是求反正切值。反正切函数 atan() 和正切函数 tan() 的功能正好相反。tan() 是已知一个角的弧度值 x，求该角的正切值 y；而 atan() 是已知一个角的正切值 y，求该角的弧度值 x。 

*atan求解结果为弧度，公式已经转换为角度。

C语言用法如下：

#include <stdio.h>
#include &l