飞控IMU姿态估计流程

飞控中使用加速度计，陀螺仪，磁罗盘进行姿态估计算法流程。

step1：

获取陀螺仪，加速度计，磁罗盘的原始数值。

step2：

陀螺仪，加速度计减去固定的偏移后得到校准数值，磁罗盘通过偏移和缩放后得到校准数值。（都是在载体坐标系下的测量值)。

step3：

首先根据上次的飞控姿态四元数计算出载体坐标系在世界坐标系中的旋转矩阵。
公式：从四元数$q(w,x,y,z)$到旋转矩阵
$$\left[\begin{array}{ccc}1-2y^2-2z^2& 2xy+2wz& 2xz-2wy\\ 2xy-2wz& 1-2x^2-2z^2& 2yz+2wx\\ 2xz+2wy& 2yz-2wx& 1-2x^2-2y^2\end{array}\right]$$
该旋转矩阵可以把载体坐标系中的测量值转换到世界坐标系下。
代码

q0q1 = imu->w * imu->x;
q0q2 = imu->w * imu->y;
q1q1 = imu->x * imu->x;
q1q3 = imu->x * imu->z;
q2q2 = imu->y * imu->y;
q2q3 = imu->y * imu->z;
q3q3 = imu->z * imu->z;
q1q2 = imu->x * imu->y;
q0q3 = imu->w * imu->z;
// 载体坐标下的x方向向量，单位化。
att_matrix[0][0] = 1 - (2*q2q2 + 2*q3q3);
att_matrix[0][1] = 2*q1q2 - 2*q0q3;
att_matrix[0][2] = 2*q1q3 + 2*q0q2;

// 载体坐标下的y方向向量，单位化。
att_matrix[1][0] = 2*q1q2 + 2*q0q3;
att_matrix[1][1] = 1 - (2*q1q1 + 2*q3q3);
att_matrix[1][2] = 2*q2q3 - 2*q0q1;

// 载体坐标下的z方向向量（等效重力向量、重力加速度向量），单位化。
att_matrix[2][0] = 2*q1q3 - 2*q0q2;
att_matrix[2][1] = 2*q2q3 + 2*q0q1;
att_matrix[2][2] = 1 - (2*q1q1 + 2*q2q2);

step4：

计算载体坐标系x轴在世界坐标系水平面的方向向量（可将世界坐标系xy平面加速度转换到航向坐标系平面)，取选择矩阵第一列的前两个数并归一化。

imu->hx_vec[0] = att_matrix[0][0];
imu->hx_vec[1] = att_matrix[1][0];
//对hx_vec进行归一化即为载体坐标系x轴在世界坐标系水平面的方向向量

step5：

计算ACC归一化后的重力方向向量和等效重力向量的叉积，结果越大说明两个向量角度越大。（该结果在后续第7步计算融合角速度）

//acc_norm[3]为加速度计传感器测量值归一化后结果
// 测量值与等效重力向量的叉积
vec_err[0] =  (acc_norm[1] * att_matrix[2][2] - att_matrix[2][1] * acc_norm[2]);
vec_err[1] = -(acc_norm[0] * att_matrix[2][2] - att_matrix[2][0] * acc_norm[2]);
vec_err[2] = -(acc_norm[1] * att_matrix[2][0] - att_matrix[2][1] * acc_norm[0]);

step6：

将载体坐标系下罗盘数据通过旋转矩阵转移到世界坐标系下，得到在世界坐标系水平面上的向量并归一化。该向量与（1,0）向量叉乘（计算朝向相对南北的误差）和点乘（计算相对南北的方向）。

step7：

计算机体坐标系下的角速度融合值（融合陀螺仪，磁罗盘）
角速度融合值 = 陀螺仪传感器加速度测量值+重力方向误差系数1+重力方向误差积分系数2+磁罗盘角度误差在各个方向分量*系数3
重力方向误差定义为：姿态估计重力方向和加速度计测量重力方向夹角大小。

for(u8 i = 0;i<3;i++)
{
	vec_err_i[i] +=  vec_err[i] *dT;
	d_angle[i] = (gyr[i] + vec_err[i] * akp  + vec_err_i[i] * aki + mag_yaw_err *imu->z_vec[i] *mkp) * dT / 2 ;
}

这里乘$1/2$的系数的原因，可以参考第8步中四元数的微分形式公式

step8：

使用四元数的微分形式进行姿态更新
$$\left[\begin{array}{c}{\dot{q}}_0\\ {\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\omega }_x^b\\ {\omega }_z^b\\ {\omega }_z^b\end{array}\right]$$
代码：

imu->w = imu->w            - imu->x*d_angle[X] - imu->y*d_angle[Y] - imu->z*d_angle[Z];
imu->x = imu->w*d_angle[X] + imu->x            + imu->y*d_angle[Z] - imu->z*d_angle[Y];
imu->y = imu->w*d_angle[Y] - imu->x*d_angle[Z] + imu->y            + imu->z*d_angle[X];
imu->z = imu->w*d_angle[Z] + imu->x*d_angle[Y] - imu->y*d_angle[X] + imu->z;
//然后对(w,x,y,z)进行归一化，即为新的姿态估计