姿态解算模块函数void IMUSO3Thread(void)，其作用是将采集的陀螺仪角度值和加速度值进行自适应互补滤波，算出差值后校正更新四元素，最终通过四元素的值算出方位、俯仰、横滚角。
下面是初始化部分：
//姿态解算软件
//! Time constant
float dt = 0.01f; //s 时间常数初始化，后边有赋值操作，感觉这里没意义
static uint32_t tPrev=0,startTime=0; //us 时间变量初始化
uint32_t now;
uint8_t i;
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f}; //rad 初始化欧拉角
/* Initialization */
float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */ //初始化旋转矩阵
float acc[3] = {0.0f, 0.0f, 0.0f}; //m/s^2 加速度数组初始化
float gyro[3] = {0.0f, 0.0f, 0.0f}; //rad/s 角度初始化
float mag[3] = {0.0f, 0.0f, 0.0f}; //磁力计数组初始化
//need to calc gyro offset before imu start working
static float gyro_offsets_sum[3]= { 0.0f, 0.0f, 0.0f }; // gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }, //角度偏移数组
static uint16_t offset_count = 0; //偏移计数
下面是记录下当前时间，用于无人机初始过程中陀螺仪水平放置时的校准过程。
now=micros();//返回系统当前运行时间
dt=(tPrev>0)?(now-tPrev)/1000000.0f:0;//一个循环的时间，换算成秒
tPrev=now; //记录上一拍的时间
下面的函数之前介绍过，读取陀螺仪与加速计值并做滤波处理：
ReadIMUSensorHandle(); //读取角速度与加速度值，并将其进行二阶巴特沃斯滤波处理
下面函数是无人机初始时的校准过程，将无人机放置水平地面上，30s时间进行校准：
if(!imu.ready) //陀螺仪是否准备好校准
{
if(startTime==0)
startTime=now; //记录当前时间
gyro_offsets_sum[0] += imu.gyroRaw[0]; //x轴数值累加
gyro_offsets_sum[1] += imu.gyroRaw[1]; //y轴数值累加
gyro_offsets_sum[2] += imu.gyroRaw[2]; //z轴数值累加
offset_count++;
if(now > startTime + GYRO_CALC_TIME) // 计时30s
{
imu.gyroOffset[0] = gyro_offsets_sum[0]/offset_count; //求平均值x 这样采集的更准确
imu.gyroOffset[1] = gyro_offsets_sum[1]/offset_count; //求平均值y
imu.gyroOffset[2] = gyro_offsets_sum[2]/offset_count; //求平均值z
offset_count=0; //求完后将数据清零
gyro_offsets_sum[0]=0; //求完后将数据清零
gyro_offsets_sum[1]=0; //求完后将数据清零
gyro_offsets_sum[2]=0; //求完后将数据清零
imu.ready = 1; //重置状态量
startTime=0; //求完后将数据清零
}
return;
}
陀螺仪读取的角度值是一个相对值，只有减去水平放置时候的角度值才是真正的角度。三轴加速度也是一样，只不过在ReadIMUSensorHandle();函数中处理了。
//陀螺仪采集的是一个相角度值
gyro[0] = imu.gyro[0] - imu.gyroOffset[0]; //通过陀螺仪当前读取的相对角度值减去刚开始水平放置时求得的相对平均角度值，求得飞机的实际x轴角度值。
gyro[1] = imu.gyro[1] - imu.gyroOffset[1]; //y方向
gyro[2] = imu.gyro[2] - imu.gyroOffset[2]; //z方向
acc[0] = -imu.accb[0]; //加速度值取负值，用时又取一次负值，感觉此处无意义，直接赋值就好了
acc[1] = -imu.accb[1];
acc[2] = -imu.accb[2];
四元素的互补滤波算法，之前介绍过用于校准四元素：
// NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], //自适应互补滤波算法，更新四元素
-acc[0], -acc[1], -acc[2],
mag[0], mag[1], mag[2],
so3_comp_params_Kp,
so3_comp_params_Ki,
dt);
将由地理系到机体系的四元数变换矩阵写入到数组中，四元素变换矩阵拿过来直接用就行了，由机体系到地理系的求个转置就行了，具体推导参看：四元素的推导
/******************地理系到机体系的四元素变换矩阵三行三列矩阵*******************************
q0q0 + q1q1 - q2q2 - q3q3 2.f * (q1*q2 - q0*q3) 2.f * (q1*q3 + q0*q2)
2.f * (q1*q2 + q0*q3) q0q0 - q1q1 + q2q2 - q3q3 2.f * (q2*q3 - q0*q1)
2.f * (q1*q3 - q0*q2) 2.f * (q2*q3 + q0*q1) q0q0 - q1q1 - q2q2 + q3q3
*********************************************************************************/
// Convert q->R, This R converts inertial frame to body frame.
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 //地理系到机体系的四元素变换矩阵
Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
然后利用四元素与欧拉角的关系计算出方位、俯仰与横滚角。
//1-2-3 Representation.
//Equation (290)
//Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel.
// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll //利用四元素与欧拉角关系求横滚角
euler[1] = -asinf(Rot_matrix[2]); //! Pitch //俯仰角
euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //方位角
将四元素变换矩阵写到二维数组中，便于以后计算用，最后再将角度换算成弧度或度：
//DCM . ground to body
for(i=0; i<9; i++)
{
*(&(imu.DCMgb[0][0]) + i)=Rot_matrix[i]; //放到转换矩阵中去
}
imu.rollRad=euler[0]; //弧度值
imu.pitchRad=euler[1];
imu.yawRad=euler[2];
imu.roll=euler[0] * 180.0f / M_PI_F; //转换成角度值
imu.pitch=euler[1] * 180.0f / M_PI_F;
imu.yaw=euler[2] * 180.0f / M_PI_F;