飞控IMU姿态估计流程
飞控中使用加速度计,陀螺仪,磁罗盘进行姿态估计算法流程。 step1: 获取陀螺仪,加速度计,磁罗盘的原始数值。 step2: 陀螺仪,加速度计减去固定的偏移后得到校准数值,磁罗盘通过偏移和缩放后得到校准数值。(都是在载体坐标系下的测量值)。 step3: 首先根据上次的飞控姿态四元数计算出载体坐标系在世界坐标系中的旋转矩阵。 公式:从四元数 q ( w , x , y , z ) q(w,x,y,z) q ( w , x , y , z ) 到旋转矩阵 [ 1 − 2 y 2 − 2 z 2 2 x y + 2 w z 2 x z − 2 w y 2 x y − 2 w z 1 − 2 x 2 − 2 z 2 2 y z + 2 w x 2 x z + 2 w y 2 y z − 2 w x 1 − 2 x 2 − 2 y 2 ] \begin{bmatrix} {1 - 2{y^2} - 2{z^2}} & {2xy + 2wz} & {2xz - 2wy} \\ {2xy - 2wz} & {1 - 2{x^2} - 2{z^2}} & {2yz + 2wx} \\ {2xz + 2wy} & {2yz - 2wx} & {1 - 2{x^2} - 2{y^2}} \\ \end{bmatrix} ⎣ ⎡ 1 − 2 y 2 − 2 z 2 2 x y − 2 w z 2