姿态解算
       准确快速的姿态解算是无人机姿态控制和位置控制的基础。现在常用的姿态解算方法主要有互补滤波、扩展卡尔曼滤波。相比较两种方法，互补滤波较为简单，容易调试，我这里也是选择Mahony互补滤波方法进行姿态解算。
无人机姿态

       无人机姿态角一共有三个，分别是滚转、俯仰、偏航，分别是绕无人机的X轴、Y轴、Z轴旋转的角度。知道无人机的实时姿态情况是进行下一步控制的基础，很难想象不知道无人机的姿态角度还能控制它平稳的飞行。
机体坐标系和大地坐标系

       大地坐标系比较容易理解，我们选取正北、正东、正上构成这个坐标系的X、Y、Z轴的正方向。我们用R表示大地坐标系。为了编程和控制的方便，我们在飞机上也构想一个坐标系，称为机体坐标系，分别选取滚转、俯仰、偏航所绕的轴作为x、y、z的正方向，我们用r表示机体坐标系。

       通过传感器能够得到飞机相对于r坐标系的姿态，但是飞机在高空中飞来飞去，坐标系是不断变化的，我们很难肉眼观察到它相对于机体坐标系的角度。但是如果能够知道飞机的姿态角相对于大地坐标系的值，我们就可以很直接看出来或者显示出来，方便后面的控制。
姿态表示方法

       姿态表示的方法常见的有三种：四元数、欧拉角、旋转矩阵。我们所说的滚转、俯仰、偏航就是用欧拉角来表示的。（这里放图不是很方便，后面再放图来形象表示一下吧）




算法
优点
缺点
其他




四元数
直接求解四元数微分方程，四个未知参数，计算量小
漂移比较严重
可用于变换时保存姿态角


欧拉角
直接求解欧拉微分方程，得到pitch、roll、yaw，直观容易理解
求解方程困难，当pitch接近90度时候会出现退化现象



旋转矩阵
求解姿态矩阵微分方程，避免退化现象
参数量多
可用于向量的表示和变换


姿态解算

       姿态解算是根据IMU数据求解无人机在空中的姿态，也叫做IMU数据融合。得到陀螺仪的角速率后（后面再写一篇博客说说怎么进行IMU传感器数据的获取和校准），按照道理可以用陀螺仪的角速率积分得到角度，因为陀螺仪为三轴传感器，可以得到滚转、俯仰、偏航的角度。但是由于积分的作用，会随着时间产生非常大的积分误差。那如何尽量消除这个误差呢？

       IMU模块一般都不会仅仅包含一个陀螺仪传感器，至少还会有一个加速度计。而加速度计通过飞机的受力计算可以得到一个角度。（但加速度计无法感知水平方向的角度，所以必须得加上磁力计才能修正偏航角）那这两个角度更应该相信谁呢？由于飞机在空中飞行时，难免会产生振动，但加速度计对震动比较敏感，陀螺仪却不敏感。所以我们可以说，要长时间相信陀螺仪的数据，偶尔相信加速度计和磁力计的数据，也就是说用加速度计和磁力计的数据来修正陀螺仪带来的积分误差。
四元数和欧拉角在姿态解算中使用

       姿态解算的核心在于旋转，旋转的表示方法主要有：四元数、旋转矩阵、欧拉角。其中，欧拉角最为直观，用来表示姿态角具体为多少度。旋转矩阵表示向量最为方便，而四元数参数较少，更为方便表示旋转，用于保存变换中的飞机姿态。
主要代码分析

/* ! Using accelerometer, sense the gravity vector. */
/* ! Using magnetometer, sense yaw. */
static void NonlinearSO3AHRSinit(float ax,  float ay,  float az,  float mx,  float my,  float mz)
{
  float initialRoll, initialPitch;
  float cosRoll, sinRoll, cosPitch, sinPitch;
  float magX, magY;
  float initialHdg, cosHeading, sinHeading;
  
  initialRoll = atan2(-ay,-az);     //求出Roll，Pitch用加速度表示方法
  initialPitch = atan2(ax, -az);
  
  cosRoll = cosf(initialRoll);      //求余弦正弦
  sinRoll = sinf(initialRoll);
  cosPitch = cosf(initialPitch);
  sinPitch = sinf(initialPitch);
  
   //其中一种的旋转方法引起Z轴的角度变化情况
  magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
  
  magY = my * cosRoll - mz * sinRoll;
  
  initialHdg = atan2f(-magY, magX);
  
  //具体公式参见全权老师的《多旋翼无人机设计与控制》 103页
  cosRoll = cosf(initialRoll * 0.5f);
  sinRoll = sinf(initialRoll * 0.5f);
  
  cosPitch = cosf(initialPitch * 0.5f);
  sinPitch = sinf(initialPitch * 0.5f);
  
  cosHeading = cosf(initialHdg * 0.5f);
  sinHeading = sinf(initialHdg * 0.5f);
  //四元数q0,q1,q2,q3表示
  q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
  q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
  q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
  q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
  q0q0=1;
  // auxillary variables to reduce number of repeated operations, for 1st pass
  q0q0 = q0 * q0;
  q0q1 = q0 * q1;
  q0q2 = q0 * q2;
  q0q3 = q0 * q3;
  q1q1 = q1 * q1;
  q1q2 = q1 * q2;
  q1q3 = q1 * q3;
  q2q2 = q2 * q2;
  q2q3 = q2 * q3;
  q3q3 = q3 * q3;
}

/*
    使用的是Mahony互补滤波算法，没有使用Kalman滤波算法
    改算法是直接参考pixhawk飞控的算法，可以在Github上看到出处
  https://github.com/hsteinhaus/PX4Firmware/blob/master/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp   
*/
static void NonlinearSO3AHRSupdate(float gx,  float gy,  float gz,  float ax,  float ay,  float az,  float mx,  float my,  float mz,  float twoKp,  float twoKi,  float dt)
{
    float recipNorm;
    float halfex = 0.0f,  halfey = 0.0f,  halfez = 0.0f;
    
     // Make filter converge to initial solution faster
    // This function assumes you are in static position.
    // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
    if(bFilterInit == 0) 
    {
        NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
        bFilterInit = 1;
    }
    //增加一个条件：  加速度的模量与G相差不远时。 0.75*G < normAcc < 1.25*G
    /*Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)*/
    /* 如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误 */
     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
     {
         float halfvx, halfvy, halfvz;
         // Normalise accelerometer measurement
        //归一化，得到单位加速度
         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
/*把加速度计的数据进行归一化处理。其中invSqrt是平方根的倒数，使用平方根的倒数而不是直接使用平方
根的原因是使得下面的ax，ay，az的运算速度更快,通过归一化处理后，ax，ay，az的数值范围变成-1到+1 */
         ax *= recipNorm;
         ay *= recipNorm;
         az *= recipNorm;
         // Estimated direction of gravity and magnetic field
         //根据当前四元数的姿态值来估算出各重力分量
         halfvx = q1q3 - q0q2;
         halfvy = q0q1 + q2q3;
         halfvz = q0q0 - 0.5f + q3q3;
         // Error is sum of cross product between estimated direction and measured direction of field vectors
         halfex += ay * halfvz - az * halfvy;
         halfey += az * halfvx - ax * halfvz;
         halfez += ax * halfvy - ay * halfvx;
     }
     ! If magnetometer measurement is available, use it.
   if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)))
   {
      float hx, hy, hz, bx, bz;
     float halfwx, halfwy, halfwz;
      
      // Normalise magnetometer measurement  归一化处理
      recipNorm = invSqrt(mx * mx + my * my + mz * mz);
      mx *= recipNorm;
      my *= recipNorm;
      mz *= recipNorm;
      
      // Reference direction of Earth's magnetic field 旋转矩阵中磁力计  这个不是很明白!!
      hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
      hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
      hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
      bx = sqrt(hx * hx + hy * hy);
      bz = hz;
      
      // Estimated direction of magnetic field   
     //根据当前四元数的姿态值来估算出各地磁分量Wx，Wy，Wz
      halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
      halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
      halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
     
     // Error is sum of cross product between estimated direction and measured direction of field vectors
      halfex += (my * halfwz - mz * halfwy);
      halfey += (mz * halfwx - mx * halfwz);
      halfez += (mx * halfwy - my * halfwx);
   }
    
    // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
    if(halfex != 0.0f  &&  halfey != 0.0f  &&  halfez != 0.0f)
    {
      // Compute and apply integral feedback if enabled
      if(twoKi > 0.0f)
      {
        gyro_bias[0]  +=  twoKi * halfex * dt;	// integral error scaled by Ki
        gyro_bias[1]  +=  twoKi * halfey * dt;  //Ki*Ts*X(n)  
        gyro_bias[2]  +=  twoKi * halfez * dt;
        
        // apply integral feedback
        gx += gyro_bias[0];
        gy += gyro_bias[1];
        gz += gyro_bias[2];
      }
      else
      {
        gyro_bias[0] = 0.0f;	// prevent integral windup
        gyro_bias[1] = 0.0f;
        gyro_bias[2] = 0.0f;
      }
      // Apply proportional feedback
       gx += twoKp * halfex;    //Kp*X(n)
       gy += twoKp * halfey;  
       gz += twoKp * halfez; 
    }
    // Time derivative of quaternion. q_dot = 0.5*q\otimes omega.
    //! q_k = q_{k-1} + dt*\dot{q}
    //! \dot{q} = 0.5*q \otimes P(\omega)
      dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
      dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
      dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
      dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx);
      
      q0 += dt*dq0;
      q1 += dt*dq1;
      q2 += dt*dq2;
      q3 += dt*dq3;
    // Normalise quaternion
      recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
      q0 *= recipNorm;
      q1 *= recipNorm;
      q2 *= recipNorm;
      q3 *= recipNorm;
     // Auxiliary variables to avoid repeated arithmetic
      q0q0 = q0 * q0;
      q0q1 = q0 * q1;
      q0q2 = q0 * q2;
      q0q3 = q0 * q3;
      q1q1 = q1 * q1;
      q1q2 = q1 * q2;
      q1q3 = q1 * q3;
      q2q2 = q2 * q2;
      q2q3 = q2 * q3;
      q3q3 = q3 * q3;   
}

#define so3_comp_params_Kp 1.5f
#define so3_comp_params_Ki 0.05f   //0.05f

//函数名：IMUSO3Thread(void)
//描述：姿态软件解算融合函数
//该函数对姿态的融合是软件解算
void IMUSO3Thread(void)
{
  //! Time constant
  float dt = 0.01f;   //s
  static uint32_t tPrev=0,startTime=0;	//us
  uint32_t now;
  uint8_t i;
  static uint8_t t=0;
	
   /* output euler angles */
  float euler[3] = {0.0f, 0.0f, 0.0f};	//rad
  
  float gyro[3] = {0.0f, 0.0f, 0.0f};   //rad/s
  float acc[3] = {0.0f, 0.0f, 0.0f};		//m/s^2
  float mag[3] = {0.0f, 0.0f, 0.0f};
  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 */

  now=HAL_GetTick();
  dt=(tPrev>0)?(now-tPrev)/1000.0f:0;
  tPrev=now;
	
  ReadIMUSensorHandle(); //得到IMU数据，注意单位

  gyro[0] = imu.gyro[0];
  gyro[1] = imu.gyro[1];
  gyro[2] = imu.gyro[2];
   
  acc[0] = -imu.accb[0];
  acc[1] = -imu.accb[1];
  acc[2] = -imu.accb[2];
   
  mag[0]= imu.accg[0];
  mag[1]= imu.accg[1];
  mag[2]= imu.accg[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);
  // 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];
		
	/* 直接利用陀螺仪的Z轴角速度会有很大的漂移 */	
	 Yaw_Gyro_Earth_Frame = -sin(imu.rollRad) * imu.gyro[0]*180.f/M_PI_F+
                       		cos(imu.rollRad)*sin(imu.pitchRad) * imu.gyro[1]*180.f/M_PI_F+ 
		                      cos(imu.pitchRad) * cos(imu.rollRad ) * imu.gyro[2]*180.f/M_PI_F;  //用于Yaw 角速度反馈
		
		/* 对磁力计的磁偏角进行补偿 https://blog.csdn.net/u013636775/article/details/72675148 */
		/* https://blog.csdn.net/xiaoxilang/article/details/80525236 */
		/* 计算磁力计数据在水平面 X Y轴上的投影 补偿前提是磁力计与加速度计同轴，方向正反错误需要调整*/
    imu.accg[1] = imu.accg[0] * sin(imu.pitchRad) * sin(imu.rollRad)
  + imu.accg[1] * cos(imu.pitchRad)
    - imu.accg[2] * cos(imu.rollRad) * sin(imu.pitchRad);
    imu.accg[0] = imu.accg[0] * cos(imu.rollRad)+ imu.accg[2] * sin(imu.rollRad);
		
   euler[2]= atan2((double)(imu.accg[0]),(double)(imu.accg[1]));  /* 磁力计的 X Y 轴颠倒了,在滤波前调换了一下*/
		
	 Mag_Yaw = euler[2] * 180.0f / M_PI_F; /* 磁力计计算出的偏航角 单位 deg */
	 att_angle[0] += Yaw_Gyro_Earth_Frame * dt; /* 陀螺仪积分计算出的偏航角 单位 deg */
	 /*---- 偏航角一阶互补 -----*/
	 if((Mag_Yaw>90 && att_angle[0]<-90)|| (Mag_Yaw<-90 && att_angle[0]>90))
	 {
		 att_angle[0] = -att_angle[0] * 0.99f + Mag_Yaw * 0.01f;
	 } 
   else 
	 {
		 att_angle[0] = att_angle[0] * 0.99f + Mag_Yaw * 0.01f;
	 }
   imu.yaw = att_angle[0];  /* 最后计算出的偏航角 单位 deg */
   
 /* 计算欧拉角，单位 deg/s 实际上Pitch 和Roll反了，参考Pixhawk的硬件的结果*/
   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;

  if(imu.yaw<0)
  {
     imu.yaw+=360;
  }
  else if(imu.yaw>360)
  {
     imu.yaw-=360;
  }  	
}

代码原理分析

       对于四元数法的姿态求解，需要求解的就是四元数的值。方向余弦矩阵用来转换时表示向量，本来矩阵的元素都是一些三角函数，这里可以用四元数来表示矩阵的元素。最后变成求解由四元数构成的余弦矩阵。

       加速度计仅仅测量的是重力加速，三轴加速度计是重力加速度的三轴分量。重力加速度的方向和大小是固定的，通过这种关系，可以得到机体坐标系和地面坐标系的角度关系。在大地坐标系R中，加速度计a输出为

$[0,0,1]^{T}$

经过旋转矩阵的变换到机体坐标系r中，a变成

$[v x, v y, v z]^{T}$

在机体坐标系中，加速度计的测量值为

$[a x, a y, a z]^{T}$

那得到两个加速度计的向量值，我们由此可以做向量积，得到误差向量，再用误差向量来修正旋转矩阵。（这一个思想在光流传感器数据互补融合中也用到了）还有一些数学理论知识，这里打公式不太好搞，等以后慢慢更新。

四元数 - 姿态解算
四元数 - 姿态解算
1 获取四元数
2 惯性单元测量值融合
3 如何消除误差呢？
4 得到欧拉角
5 欧拉角法和旋转矢量法介绍
6 姿态控制

四元数 - 姿态解算
在前面我们介绍了旋转的表示方法

以及，四元数的定义与几何意义
我们要获得无人机的飞行姿态必须把姿态传感器获取的数据，表示为我们上面说过的几种方式，我们在这里介绍用四元数来进行姿态解算的方法和相关知识：

我们首先来了解一下姿态解算的步骤：
1 获取四元数
首先我们要获取四元数，这里我们利用龙格库塔公式求四元数的q0~q3，需要这个周期的角速度gx，gy，gz和周期时间deltaT

（注：四元数可以初始化为q0=1，q1、q2、q3=0）：


这里是一阶龙格库塔公式更新四元数的理解:（为什么通过角速度gx，gy，gz和周期时间deltaT可以更新四元数)


2 惯性单元测量值融合
惯性单元测量值融合:四轴上安装陀螺仪，可以测量四轴倾斜的角度，由于陀螺仪输出的是四轴的角速度，不会收到四轴振动影响。因此该信号中噪声很小。四轴的角度又是通过对角速度积分而得，这可进一步平滑信号，从而使得角度信号更加稳定。因此四轴控制所需要的角度和角速度可以使用陀螺仪所得到的信号。通过上面的公式获取四元数的值是存在误差的，这个误差的主要来源于，三轴陀螺仪获取角速度信息，是要经过积分运算的，这个过程存在微小的误差，经过积分运算之后，就会形成积累误差。随时间延长而逐步增加，最终导致无法形成正确的角度信号；
3 如何消除误差呢？
如何消除误差呢？可以通过上面的加速度传感器获得的角度信息对此进行校正。利用加速度计所获得的角度信息与陀螺仪积分后的角度进行比较，叫比较的误差信号进过比例放大后与陀螺仪输出的角速度信号叠加之后在进行积分。对于加速度计给定的角度，经过比例、积分环节之后产生的角度必然等于加速度计所获得的角度。由于加速度计获得的角度信息不会存在积分误差，所以最终将输出角度中的积累误差消除了。但是加速度计所产生的角度信号中会叠加很强的四轴运动加速度噪声信号，为了避免给信号对于角度的影响，因此比例系数应该非常小。这样加速度的噪声信号经过比例、积分后，在输出角度信息中就会非常小了。由于存在积分环节，所以无论比例系数多么小，最终输出的角度必然与加速度计测量的角度相等，只是这个调节过程会随着比例系数的减小而延长。
4 得到欧拉角
进行完消除误差的处理之后，我们利用四元数的方向余弦矩阵和欧拉角的关系得到欧拉角。这里我们把程序的思路整理一下：





5 欧拉角法和旋转矢量法介绍
为什么用四元数求解姿态呢？这里有一点关于欧拉角法和旋转矢量法的介绍：

6 姿态控制
最后利用 PID 等相关控制算法进行无人机飞行姿态控制。（会在下一篇文章中介绍）。
四元数的几何意义

四元数AHRS姿态解算和IMU姿态解算分析
AHRS是自动航向基准系统(Automatic Heading Reference System)的简称。目前，使用四元数来进行AHRS姿态解算的算法被广泛采用于四轴飞行器上。该算法源自英国Bristol大学的Ph.D Sebastian Madgwick，他在2009年开发并发布了该算法。下面我们来对该算法的代码进行详细分析。AHRS是自动航向基准系统(Automatic Heading Reference System)的简称。目前，使用四元数来进行AHRS姿态解算的算法被广泛采用于四轴飞行器上。

我们首先来看IMU部分。IMU是惯性测量装置(Inertial Measurement Unit)的简称，通常包含陀螺仪和加速度计。陀螺仪测量的是角速度，即物体转动的速度，把速度和时间相乘，即可以得到某一时间段内物体转过的角度。加速度计测量的是物体的加速度，我们知道，重力加速度是一个物体受重力作用的情况下所具有的加速度。当物体处于静止状态时，加速度计测量出来的值就等于重力加速度1g, 约等于9.8米每平方秒。重力加速度g的方向总是竖直向下的，通过获得重力加速度在其X轴，Y轴上的分量，我们可以计算出物体相对于水平面的倾斜角度。典型的IMU惯性测量芯片为MPU6050，它被广泛采用在四轴飞行器上。

根据加速度计和地磁计的数据，转换到地理坐标系后，与对应参考的重力向量和地磁向量进行求误差，这个误差用来校正陀螺仪的输出，然后用陀螺仪数据进行四元数更新，再转换到欧拉角

对于陀螺仪的角速度测量，大家比较好理解，简单来说，相当于一个人绕着一个圆圈行走，假如他的速度是1度没秒，那么通过速度乘以时间，我们就可以知道他距离起点走了多少度。

那么我们如何理解用加速度计来测量倾角呢？一个简单的例子如下: 一个单轴的加速计位于重力水平面上的时候，它在垂直方向上受到的加速度为1g，在水平方向上受到的加速度为0。当我们把它旋转一个角度的时候，就会在水平轴上产生一个加速度分量。通过它们的关系，就可以计算出该单轴加速计的倾角。

 

在一个三轴的加速度传感器中，可以通过下列的反正切函数运算公式来计算加速度计各个轴的倾角。

 

另外，我们还要注意各传感器的方向。下图是MPU6050和MPU9150的传感器方向定义,其中MPU6050只包含陀螺仪和加速计共六个轴，而MPU9150还包含磁力计，共九个轴。从MPU6050和MPU9150获得的数据可以直接在下面的算法中使用，而不需要做符号变换。

 

现在，让我们回到姿态解算代码的分析。下述代码的核心思想是：

通过陀螺仪的积分来获得四轴的旋转角度，然后通过加速度计的比例和积分运算来修正陀螺仪的积分结果。下面代码中的gx，gy，gz分别代表陀螺仪在X轴，Y轴和Z轴三个轴上的分量，ax，ay，az分别代表加速度计在在X轴，Y轴和Z轴三个轴上的分量。

 

#.未加入磁力计（磁力计输出为0）IMU惯性测量姿态解算

void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {

float recipNorm;

float halfvx, halfvy, halfvz;

float halfex, halfey, halfez;

float qa, qb, qc;

如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误。

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)

if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

把加速度计的数据进行归一化处理。其中invSqrt是平方根的倒数，使用平方根的倒数而不是直接使用平方根的原因是使得下面的ax，ay，az的运算速度更快。通过归一化处理后，ax，ay，az的数值范围变成-1到+1之间。

// Normalise accelerometer measurement

recipNorm = invSqrt(ax * ax + ay * ay + az * az);

ax *= recipNorm;

ay *= recipNorm;

az *= recipNorm;

根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比，从而实现对四轴姿态的修正。

// Estimated direction of gravity and vector perpendicular to magnetic flux

halfvx = q1 * q3 – q0 * q2;

halfvy = q0 * q1 + q2 * q3;

halfvz = q0 * q0 – 0.5f + q3 * q3;

使用叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。

// Error is sum of cross product between estimated and measured direction of gravity

halfex = (ay * halfvz – az * halfvy);

halfey = (az * halfvx – ax * halfvz);

halfez = (ax * halfvy – ay * halfvx);

把上述计算得到的重力差进行积分运算，积分的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。积分系数是Ki，如果Ki参数设置为0，则忽略积分运算。
// Compute and apply integral feedback if enabled

if(twoKi > 0.0f) {

integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki

integralFBy += twoKi * halfey * (1.0f / sampleFreq);

integralFBz += twoKi * halfez * (1.0f / sampleFreq);

gx += integralFBx;                                                     // apply integral feedback

gy += integralFBy;

gz += integralFBz;

}

else {

integralFBx = 0.0f;                                                         // prevent integral windup

integralFBy = 0.0f;

integralFBz = 0.0f;

}

把上述计算得到的重力差进行比例运算。比例的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。比例系数为Kp。
// Apply proportional feedback

gx += twoKp * halfex;

gy += twoKp * halfey;

gz += twoKp * halfez;

}

通过上述的运算，我们得到了一个由加速计修正过后的陀螺仪数据。接下来要做的就是把修正过后的陀螺仪数据整合到四元数中。

// Integrate rate of change of quaternion

gx *= (0.5f * (1.0f / sampleFreq));                           // pre-multiply common factors

gy *= (0.5f * (1.0f / sampleFreq));

gz *= (0.5f * (1.0f / sampleFreq));

qa = q0;

qb = q1;

qc = q2;

q0 += (-qb * gx – qc * gy – q3 * gz);

q1 += (qa * gx + qc * gz – q3 * gy);

q2 += (qa * gy – qb * gz + q3 * gx);

q3 += (qa * gz + qb * gy – qc * gx);

把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
// Normalise quaternion

recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

q0 *= recipNorm;

q1 *= recipNorm;

q2 *= recipNorm;

q3 *= recipNorm;

}

 

#.加入磁力计（磁力计输出不为0）AHRS姿态解算

在一个三维的空间内，由于重力加速度的存在，加速度计为我们提供了一个水平位置的绝对参考，但是它无法给我们提供一个方向的参考。这时候，我们首先考虑到的是指南针，磁力计就是这样的一个传感器，它给人们提供了一个正北方向的绝对参考。在四轴中常使用的地磁传感器是HMC5883L。

有了上面IMU惯性测量姿态解算的基础，我们就能很容易理解AHRS姿态解算的代码。它是在IMU惯性测量的基础上加入了地磁姿态的解算。下面代码中的gx，gy，gz分别代表陀螺仪在X轴，Y轴和Z轴三个轴上的分量，ax，ay，az分别代表加速度计在在X轴，Y轴和Z轴三个轴上的分量。mx，my，mz分别代表地磁在在X轴，Y轴和Z轴三个轴上的分量。

void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {

float recipNorm;

float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

float hx, hy, bx, bz;

float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;

float halfex, halfey, halfez;

float qa, qb, qc;

如果地磁传感器各轴的数均是0，那么忽略该地磁数据。否则在地磁数据归一化处理的时候，会导致除以0的错误。

// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)

if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {

MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);

return;

}

如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误。

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)

if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

把加速度计的数据进行归一化处理。

// Normalise accelerometer measurement

recipNorm = invSqrt(ax * ax + ay * ay + az * az);

ax *= recipNorm;

ay *= recipNorm;

az *= recipNorm;

把地磁的数据进行归一化处理。

// Normalise magnetometer measurement

recipNorm = invSqrt(mx * mx + my * my + mz * mz);

mx *= recipNorm;

my *= recipNorm;

mz *= recipNorm;

预先进行四元数数据运算，以避免重复运算带来的效率问题。

// Auxiliary variables to avoid repeated arithmetic

q0q0 = q0 * q0;

q0q1 = q0 * q1;

q0q2 = q0 * q2;

q0q3 = q0 * q3;

q1q1 = q1 * q1;

q1q2 = q1 * q2;

q1q3 = q1 * q3;

q2q2 = q2 * q2;

q2q3 = q2 * q3;

q3q3 = q3 * q3;

// Reference direction of Earth’s magnetic field

hx = 2.0f * (mx * (0.5f – q2q2 – q3q3) + my * (q1q2 – q0q3) + mz * (q1q3 + q0q2));

hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f – q1q1 – q3q3) + mz * (q2q3 – q0q1));

bx = sqrt(hx * hx + hy * hy);

bz = 2.0f * (mx * (q1q3 – q0q2) + my * (q2q3 + q0q1) + mz * (0.5f – q1q1 – q2q2));

根据当前四元数的姿态值来估算出各重力分量Vx，Vy，Vz和各地磁分量Wx，Wy，Wz。

// Estimated direction of gravity and magnetic field

halfvx = q1q3 – q0q2;

halfvy = q0q1 + q2q3;

halfvz = q0q0 – 0.5f + q3q3;

halfwx = bx * (0.5f – q2q2 – q3q3) + bz * (q1q3 – q0q2);

halfwy = bx * (q1q2 – q0q3) + bz * (q0q1 + q2q3);

halfwz = bx * (q0q2 + q1q3) + bz * (0.5f – q1q1 – q2q2);

使用叉积来计算重力和地磁误差。

// Error is sum of cross product between estimated direction and measured direction of field vectors

halfex = (ay * halfvz – az * halfvy) + (my * halfwz – mz * halfwy);

halfey = (az * halfvx – ax * halfvz) + (mz * halfwx – mx * halfwz);

halfez = (ax * halfvy – ay * halfvx) + (mx * halfwy – my * halfwx);

把上述计算得到的重力和磁力差进行积分运算，

// Compute and apply integral feedback if enabled

if(twoKi > 0.0f) {

integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki

integralFBy += twoKi * halfey * (1.0f / sampleFreq);

integralFBz += twoKi * halfez * (1.0f / sampleFreq);

gx += integralFBx; // apply integral feedback

gy += integralFBy;

gz += integralFBz;

}

else {

integralFBx = 0.0f; // prevent integral windup

integralFBy = 0.0f;

integralFBz = 0.0f;

}

把上述计算得到的重力差和磁力差进行比例运算。

// Apply proportional feedback

gx += twoKp * halfex;

gy += twoKp * halfey;

gz += twoKp * halfez;

}

把由加速计和磁力计修正过后的陀螺仪数据整合到四元数中。

// Integrate rate of change of quaternion

gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors

gy *= (0.5f * (1.0f / sampleFreq));

gz *= (0.5f * (1.0f / sampleFreq));

qa = q0;

qb = q1;

qc = q2;

q0 += (-qb * gx – qc * gy – q3 * gz);

q1 += (qa * gx + qc * gz – q3 * gy);

q2 += (qa * gy – qb * gz + q3 * gx);

q3 += (qa * gz + qb * gy – qc * gx);

把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。

// Normalise quaternion

recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

q0 *= recipNorm;

q1 *= recipNorm;

q2 *= recipNorm;

q3 *= recipNorm;

}

上文详细描述了由Madgwick提出四元数AHRS姿态解算和IMU姿态解算方法，如需转载，请勿用于商业目的，并注明：该文章出自圆点博士无人机www.bspilot.com

 

	

在姿态解算入门系列里 Sugar 以初中数学为基础展开了用加速度计的姿态解算方法。上一篇《姿态解算进阶之理解旋转矩阵(也称方向余弦矩阵)》进行了一次理论进阶。本篇 Sugar 用进阶的理论重写一下用加速度计做姿态解算的方法，然后与入门级算法对比一下结果有哪些改进。
姿态角定义的悄然改变
在《MPU6050 姿态解算系列一：加速度姿态解算》一文中 Sugar 有说明：
1、横滚角 roll 指：重力加速度与“X-Z 平面”的夹角；
2、俯仰角 pitch 指：重力加速度与“Y-Z 平面”的夹角。
通过我们本文的算法升级，这样的定义也会有所改变，具体怎么个变法读者掌握本文算法后自然会知道。这里 Sugar 放前面先说，目的是起到一个思维导向作用，防止思维陷在之前的定义里出不来从而影响对本文算法的理解。
本文姿态角的定义将采用飞控上常用的定义方法：绕 X 轴转是横滚角 Roll、绕 Y 轴转是俯仰角 Pitch、绕 Z 轴转是偏航角 Yaw。
加速度计姿态解算进阶
在《姿态解算进阶之理解旋转矩阵(也称方向余弦矩阵)》中 Sugar 介绍了旋转矩阵。既然用旋转矩阵能对三维空间内的向量进行任意旋转，那么我们不难知道：加速度计在各个轴上的分量就是通过在“竖直的加速度向量”上施加旋转而得到的。
在姿态解算上我们关心的是向量角度而非向量模长，为了能够顺利的使用反三角函数做姿态解算，我们必须首先对加速度计数据进行单位化，保证反三角函数有实数解。假设上图的红色箭头(直线的那个)是通过对单位向量[0;0;1]施加三维旋转得到的。为了便于表示，我们做如下约定：
% cr:cos(roll)  sr:sin(roll)% cp:cos(pitch) sp:sin(pitch)% cy:cos(yaw)   sy:sin(yaw)from_euler_xyz = [ cp*cy, cy*sp*sr - cr*sy, sr*sy + cr*cy*sp][ cp*sy, cr*cy + sp*sr*sy, cr*sp*sy - cy*sr][   -sp,            cp*sr,            cp*cr]
在这样的约定下，我们有：
[加速度计单位化后的读数] = from_euler_xyz * [0;0;1];
记加速度计单位化后的读数为[Ax;Ay;Az]，将上面方程的右侧展开，可得：
 Ax = sr*sy + cr*cy*sp Ay = cr*sp*sy - cy*sr Az =            cp*cr
这个解告诉我们一个现象：在这个旋转矩阵的作用下，单位向量在 Z 轴上的投影只与横滚角 roll和俯仰角 pitch有关。深刻理解这个现象是本篇算法的一个很重要的关键点。下面 Sugar 以roll 30 度、pitch 30 度为例，用几张动图形象地表述一下这个现象：
对于这个 3D 旋转我们看一下X-Y 方向的投影，如下：
我们看到一个形状不变的三角形在转动，说明：旋转过程过中所有姿态的 roll 和 pitch 都是相同的。我们再来看一下X-Z 方向的投影，如下：
上图表明上述 Ax、Ay、Az 的解所描述的是：一组法向量与竖直方向夹角相等的姿态。下面 Sugar 以剑影分光术的形式更清楚地表现一下这个现象，为了不让众多剑影的出现太突然，先用一成功力弄出两个剑影：
然后提高功力，输出一圈剑影：
已经眼花缭乱了，但红色的各个姿态的法向量有一个明显特征：伞骨状。为了体现量的特点，Sugar 选用了 pitch 60 的一组姿态且法向量在姿态的两侧都是单位向量，我们看一下X-Z视图表现出的量的特点：
又是动图又是剑影分光术的，Sugar 要表达的一个重点就是：在 from_euler_xyz 旋转矩阵的作用下，各个偏航角不同的姿态对应的横滚角 roll 和俯仰角 pitch 都是跟偏航角为 0 的姿态是一样的。因为用 IMU 无法估算偏航角 Yaw，不妨随意标定一个姿态是 Yaw 为 0 的姿态，这样就有 sy = 0、cy = 1，这样 Ax、Ay 和 Az 就是：
 Ax = cr*sp Ay = -sr Az = cp*cr
至此，我们就得到了用加速度计解算姿态的新公式：
r = -asin(Ay)p = atan2(Ax,Az)
进阶算法效果有什么改善
Sugar 在 MATLAB 同时绘出进阶算法和入门级算法的姿态对比图，主要代码如下：
效果对比如下：
为什么 pitch 是正负 180 度
我们知道反正切函数atan()的值域是-90 度 ~ 90 度，这个公式算出的怎么会超过这个值域范围的？原因是：这里我们使用的是四象限反正切。
四象限反正切函数的特点：
四象限反正切并不是 MATLAB 独有的，ArduPilot 代码里也是用四象限反正切函数做姿态解算的。