姿态解算

姿态表示

姿态检测算法：姿态的表示可以用欧拉角，也可以用四元数。姿态检测算法的作用就是将加速度计、陀螺仪等传感器的测量值解算成姿态，进而作为系统的反馈量。在获取sensors值之前需要对数据进行滤波，滤波算法主要是将获取到的陀螺仪和加速度计的数据融合进行去噪声及融合，得出正确的角度数据（欧拉角或四元数），数据融合主要采用互补滤波或者卡尔曼滤波。

欧拉角

用欧拉角进行姿态表示，实质上描述的体轴系与其他坐标系的转化，首先我们先明确直升机的三个坐标系：体轴系、地轴系、桨轴系。

我们使用欧拉角表述姿态角就是要表示体轴系到地轴系的转换关系。我们给出下图体轴系与地轴系的转换关系。下图引用陈仁良教授的直升机飞行动力学教材。

将OX_D Y_D Z_D 绕轴OY_D转过偏航角Ψ，成为OX‘Y_D Z’，其转换矩阵为
然后绕轴OZ’转过俯仰角θ，成为OXYZ‘，其转换矩阵为