IMU包含陀螺仪、加速度计、磁力计，那如何从IMU中提取出姿态角呢？

陀螺仪

陀螺仪输出的为陀螺坐标系的三轴角速度

欧拉角是三次绕轴旋转的角度，需要理解为相对大地坐标系来说既有旋转顺序又有旋转角度的三个角度，欧拉角旋转轴组成的坐标系是一个非正交坐标系

首先需要理解欧拉角的三个轴和陀螺坐标系的三个轴是不重合的，

陀螺坐标系的三个轴很好理解，一直都是相互垂直的三轴也就是正交坐标系

欧拉角的三个轴是非正交坐标系，可以这样理解想像一下，假设我们机体坐标系为正交的XYZ，

我们绕ZYX顺序旋转，第一次绕机体Z轴旋转theta_yaw得到我们的欧拉角的Z轴，此时我们可以想象到两者Z轴还是重合的，接着我们绕机体Y轴旋转theta_roll得到我们的欧拉角的Y轴，此时我们可以想象到欧拉角的Y轴此时和机体坐标系的Y轴是重合的但是Z轴和X轴都不重合了，接着我们绕机体坐标系X轴的旋转theta_pitch得到我们的欧拉角X轴，此时我们可以想象到欧拉角的X轴与机体的X轴重合但是Y轴和Z轴都不重合了，由此可以想象出欧拉角的XYZ轴是非正交坐标系。对欧拉角的非正交坐标系需要好好想象一下。

上面的机体坐标系在这其实就是陀螺坐标系

陀螺坐标系的三轴角速度可以理解为在陀螺坐标系下的一个向量，现在我们的问题变为如何将陀螺坐标系的向量解耦到欧拉角的非正交坐标系。

X轴：陀螺坐标系X轴与欧拉角坐标系X轴重合所以不需要解耦

使用matlab来表达为：gyro_R_att_x = rotx(0) * [1；0；0]

表达了将欧拉角非正交坐标系下的X方向的单位向量在陀螺坐标系下的表达

Y轴：陀螺坐标系Y轴与欧拉角坐标系Y轴不重合是因为陀螺坐标系绕X轴旋转了theta_pitch，所以只需要陀螺坐标系绕X轴旋转-theta_pitch就可以使得Y轴重合

使用matlab来表达为：gyro_R_att_y = rotx(-theta_pitch) * [0；1；0]

表达了将欧拉角非正交坐标系下的Y方向的单位向量在陀螺坐标系下的表达

Z轴：陀螺坐标系Z轴与欧拉角坐标系Z轴不重合是因为陀螺坐标系先绕Y轴旋转theta_roll再绕X轴旋转了theta_pitch，所以只需要陀螺坐标系先绕X轴旋转-theta_pitch再绕Y轴旋转-theta_roll就可以使得Z轴重合

使用matlab来表达为：gyro_R_att_z = rotx(-theta_pitch) * roty(-theta_roll) * [0；0；1]

表达了将欧拉角非正交坐标系下的Z方向的单位向量在陀螺坐标系下的表达

欧拉角三轴坐标系向量解耦到陀螺坐标系的矩阵：gyro_R_att = [att_R_gyro_x; att_R_gyro_y; att_R_gyro_z]

陀螺坐标系向量解耦到欧拉角三轴坐标系的矩阵：att_R_gyro = inv(gyro_R_att)

设陀螺坐标系角速度向量为gyro = [gyro_x; gyro_y; gyro_z]

则得到解耦后的姿态角：att = att_R_gyro * gyro

由此就得到了在欧拉角非正交坐标系下的三轴角速度，对解耦后的角速度进行积分就能得到三轴欧拉角

陀螺仪优缺点：

1.陀螺仪是积分得到角度，偏置误差积分将对角度产生较大影响

2.陀螺仪易受温度影响产生偏置误差

加速度计

加速度计输出的为重力加速度（重力加速度可以看成是一个向量）在机体坐标系（机体坐标系与上面提到的陀螺坐标系是同一坐标）上的表达,在大地坐标系上重力加速度向量可以看成[0; 0; 1]

假设机体坐标系旋转顺序为ZYX，大地坐标系默认与初始机体坐标系重合

那么可得到

G_T_B = rotz(theta_yaw) * roty(theta_roll) * rotx(theta_pitch)

B_T_G = inv(B_T_G）

acc = B_T_G * [0; 0; 1]

再根据已知加速度计数据，反解得到姿态角theta_pitch和theta_roll

由上可得知加速度计不能得到航向角

加速度计存在以下优缺点：

1.加速度计不能得到航向角

2.加速度计噪声较大

3.加速度计根据重力加速度得到欧拉角，当机体本身存在加速度时加速度计的可信度下降

4.加速度计静态误差较小，动态误差较大

磁力计

磁力计输出的为地磁场（地磁场可以看成一个向量）在机体坐标系上的表达

假设旋转顺序为ZYX

在旋转前，我们可以知道地磁场是一个单位向量[1; 0; 0]

我们假设先进行Z轴旋转得到一个绕Z轴旋转后的坐标系，那么假设我们此时知道地磁场向量在这个坐标系下的表达，那么我们就可以通过简单的atan（y,x）得到yaw轴旋转的角度

那么现在我们只需要得到地磁场向量在Z轴旋转后坐标系下的表达就能得到航向角

G_T_B = rotz(0) * roty(theta_roll) * rotx(theta_pitch)

m = G_T_B * m_sensor

其中m_sensor为地磁场向量在机体坐标系的表达也就是磁力计的三轴输出，m为地磁场向量在大地坐标系先旋转Z轴后的坐标系的表达

由此得到航向角theta_yaw = atan(m_y, m_x)

磁力计只能得到航向角