匿名飞控姿态解算-优快云博客

本文链接：https://blog.youkuaiyun.com/qq_52396872/article/details/122634720

匿名飞控TI版_姿态解算

准备电赛准备大创先看看匿名姿态解算的代码

文章目录

- 匿名飞控TI版_姿态解算
一，姿态解算原理
二，匿名姿态解算代码分析

一，姿态解算原理

1.介绍

       姿态通俗的说就是指我们站在地面上观察飞行器的俯仰(pitch)/横滚(roll)/航向(yaw)状态。飞行器需要实时知道当前自己的姿态，才能够根据需要操控其接下来的动作，例如保持平稳，翻滚。
       数学模型:姿态是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系(姿态角)﹐有一些数学表示方法。很常见的就是欧拉角,四元数，矩阵，轴角。
       飞行器姿态描述套用上面的数学模型，参考坐标系就对应着导航坐标系是固定不变的。我们通常用坐标系R表示。固连坐标系就对应着机体坐标系，用坐标系r表示。那么我们就可以用欧拉角，四元数等数学方法来描述r与R的角位置关系。这就是飞行器姿态解算的数学模型。
       姿态解算也叫做姿态分析，姿态估计，姿态融合。姿态解算是根据IMU(惯性测量单元)数据（陀螺仪、加速度计、罗盘等）求解出飞行器的空中姿态，所以也叫做IMU数据融合。
       匿名坐标系采用西北天来定义正方向。
       姿态有多种数学表示方式，常见的是四元数，欧拉角，方向余弦矩阵。他们各自有其自身的优点，在不同的领域使用不同的表示方式。在四轴飞行器中通常使用的是四元数、欧拉角和方向余弦矩阵。
       飞机姿态角是按欧拉概念定义的，故亦称欧拉角。飞机姿态角是由机体坐标系与地理坐标系之间的关系确定的，用航向角、俯仰角和横滚角三个欧拉角表示。不同的转动顺序会形成不同的坐标变换矩阵，通常按航向角、俯仰角和横滚角的顺序来表示机体坐标系相对地理坐标系的空间转动。

下面介绍方向余弦矩阵，欧拉角和四元数的定义和应用

2.方向余弦矩阵

“方向余弦矩阵”是由两组不同的标准正交基的基底向量之间的方向余弦所形成的矩阵。方向余弦矩阵可以用来表达一组标准正交基与另一组标准正交基之间的关系，也可以用来表达一个向量对于另一组标准正交基的方向余弦。
方向余弦矩阵的写法有两种，一种是本体坐标系相对于全局坐标系，一种是全局坐标系相对于本体坐标系。

（1）方向余弦

在世界坐标系 R 中，对任意一个向量V有V={V_x,V_y,V_z}，该向量对世界坐标系的X Y Z轴分别成α,β,γ角度.
则cosα,cosβ,cosγ分别为V在三轴上的方向余弦，大小分别等于|V_x|,|V_y|,|V_z|

（2）DCM矩阵

我们将本体坐标系(机体坐标系)定义为Oxyz，另外一个全局坐标系(世界坐标系)定义为OXYZ，两坐标系的具有相同的原点O，如图所示。本体坐标系的x、y、z轴对应的单位向量分别为i，j，k；全局坐标系X、Y、Z轴对应的单位向量分别为I，J，K。

因此，通过定义，全局坐标系(世界坐标系)下的单位向量I，J，K表示形式可写为：

$I^G = {1,0,0} ^T, J^G={0,1,0} ^T , K^G = {0,0,1} ^T$

相应的，本体坐标系下的单位向量i，j，k表示形式可写为：

$i^B = {1,0,0} ^T, j^B={0,1,0}^T, k^B = {0,0,1} ^T$

下面为了方便理解，令
                                                                             x^r= i^B ， X^R= I^G
                                                                             y^r=j^B ， Y^R= J^G
                                                                             z^r= k^B ， Z^R= K^G

方向余弦矩阵就是机体坐标系 r 的x, y, z轴分别在世界坐标系下的方向余弦所构成的矩阵。
cos(x^r,X^R)为机体坐标系的x轴正方向与世界坐标系的X轴正方向形成的夹角余弦。
cos(x^r,X^R)=cos(X^R,x^r)
$\left\{ \begin{matrix} cos(x^r,X^R) & cos(x^r,Y^R) & cos(x^r,Z^R) \\ cos(y^r,X^R) & cos(y^r,Y^R) & cos(y^r,Z^R) \\ cos(z^r,X^R) & cos(z^r,Y^R) & cos(z^r,Z^R) \end{matrix} \right\}$

3.欧拉角

偷了个图过来方便理解
~~图是偷的~~ 手动狗头传送门
俯仰角pitch、偏航角yaw、翻转角roll
欧拉角使用最简单的x,y,z值来分别表示在x，y，z轴上的旋转角度，其取值为0-360(或者0-2pi），一般使用roll，pitch，yaw来表示这些分量的旋转值。需要注意的是，这里的旋转是针对世界坐标系说的，这意味着第一次的旋转不会影响第二、三次的转轴。
欧拉角容易出现的问题是 1）不易在任意方向的旋转轴插值； 2）万向节死锁；3）旋转的次序无法确定。

4.坐标变换和旋转矩阵

旋转矩阵的旋转其实包含两种意思，一是在同一个坐标系下，向量的旋转；二是坐标系的旋转，使得同一向量在不同的坐标系下有不同的坐标。

坐标变换

下面是坐标变换，请看如下坐标系旋转：
原坐标系绕x轴旋转变成新坐标系旋转角度为 $\theta$
在这里插入图片描述
            B_x = A_x
            B_y = cos $\theta$ A_y+sin $\theta$ A_z
            B_z = -sin $\theta$ A_y+cos $\theta$ A_z
由此可以推出下面三个旋转矩阵
下面分别是绕x轴y轴z轴的旋转矩阵，这3个旋转矩阵是坐标变换的旋转矩阵。

绕IMU的Z轴旋转：航向角yaw，转动 y 角度
绕IMU的Y轴旋转：俯仰角pitch，转动 p 角度
绕IMU的X轴旋转：横滚角row，转动 r 角度
这里的r角度等价于上述 $\theta$ 角

对于原坐标系中一个向量[r_x,r_y,r_z],转成新坐标系的向量[R_x,R_y,R_z]
$\left\{ \begin{matrix} R_x \\ R_y \\ R_z \end{matrix} \right\} =M_x* \left\{ \begin{matrix} r_x \\ r_y \\ r_z \end{matrix} \right\}$

旋转矩阵

同理类推的话，如果按照ZYX的顺序进行旋转，得到的旋转矩阵
$M_x*M_y*M_z=\left\{ \begin{matrix} cosp*cosy & cosp*siny & -sinp \\ -siny*cosr+sinr*cosy*sinp & cosr*cosy+sinr*siny*sinp & sinr*cosp \\ sinr*siny+cosr*cosy*sinp& -sinr*cosy+cosr*siny*sinp& cosr*cosp \end{matrix} \right\}$

5.四元数

四元数可以方便的表示3维空间的旋转，但其概念不太好理解，可以类比复数，复数表示的其实是2维平面中的旋转。

四元数的基本表示形式为：q₀+q₁*i+q₂*j+q₃*k，即1个实部3个虚部。
q₀
i² =j² = k² = -1
关于四元数的详细介绍
在右手、左手坐标系下建立的四元数表示方式分别叫做四元数的两种表达：Hamilton/JPL

这两种表达的区别如下：
在这里插入图片描述
Hamilton对应右手系，JPL对应左手系

定义的差异直接导致计算公式的不同，但实际应用中只要统一标准应该问题不大。

Eigen ，Matlab ，ROS、Google Ceres Solver等都使用Hamilton四元数，而JPL则多用于航空航天领域。

四元数表示为一种旋转

$\boldsymbol{n}=[n_x\quad n_y\quad n_z]表示的转轴逆时针旋转角度 \theta 的旋转，可以用单位四元数表示成：$ $p=(\cos\frac{\theta}{2}, \boldsymbol{n}\sin\frac{\theta}{2})$
对于i、j和k本身的几何意义可以理解为一种旋转，其中i旋转代表Z轴与Y轴相交平面中Z轴正向向Y轴正向的旋转，j旋转代表X轴与Z轴相交平面中X轴正向向Z轴正向的旋转，k旋转代表Y轴与X轴相交平面中Y轴正向向X轴正向的旋转，-i、-j、-k分别代表i、j、k旋转的反向旋转。
在这里插入图片描述

四元数转旋转矩阵

匿名的坐标系变换，是按是按ZYX顺序从地理坐标系转到机体坐标系的， (注意这三个式子中， $\phi$ 是绕X轴转角（roll）， $\theta$ 是绕Y轴转角（(pitch)）， $\psi$ 是绕Z轴转角(yaw))
即上述旋转矩阵中 y= $\psi$ ,p= $\theta$ ,r= $\phi$
在这里插入图片描述
程序中用到的二维数组att_matrix表示从机体坐标系转到地理坐标系的旋转矩阵，是上面那个旋转矩阵的转置

欧拉角转四元数的转换方法如下
在这里插入图片描述

四元数和欧拉角和旋转矩阵之间的各种转换
欧拉角化为四元数后得到
单位四元数 q₀²+q₁²+q₂²+q₃²=1
在这里插入图片描述

匿名这里使用的是Hamilton即右手系的四元数运算。

//att_matrix是机体系r到地理系（世界坐标系）R的旋转矩阵
	// 载体（机体）坐标下的x方向向量，单位化。
    att_matrix[0][0] = imu->x_vec[X] = 1 - (2*q2q2 + 2*q3q3);//r系 x轴转换为R系 X轴的分量
    att_matrix[0][1] = imu->x_vec[Y] = 2*q1q2 - 2*q0q3;//r系 x轴转换为R系 Y轴的分量
    att_matrix[0][2] = imu->x_vec[Z] = 2*q1q3 + 2*q0q2;//r系 x轴转换为R系 Z轴的分量
		
	// 载体坐标下的y方向向量，单位化。
    att_matrix[1][0] = imu->y_vec[X] = 2*q1q2 + 2*q0q3;//r系 y轴转换为R系 X轴的分量
    att_matrix[1][1] = imu->y_vec[Y] = 1 - (2*q1q1 + 2*q3q3);//r系 y轴转换为R系 Y轴的分量
    att_matrix[1][2] = imu->y_vec[Z] = 2*q2q3 - 2*q0q1;//r系 y轴转换为R系 Z轴的分量
		
    // 载体坐标下的z方向向量（等效重力向量、重力加速度向量），单位化。
    att_matrix[2][0] = imu->z_vec[X] = 2*q1q3 - 2*q0q2;//r系 z轴转换为R系 X轴的分量
    att_matrix[2][1] = imu->z_vec[Y] = 2*q2q3 + 2*q0q1;//r系 z轴转换为R系 Y轴的分量
    att_matrix[2][2] = imu->z_vec[Z] = 1 - (2*q1q1 + 2*q2q2);//r系 z轴转换为R系 Z轴的分量

四元数虽然方便表示旋转，但其形式不太直观，旋转转换成pitch、roll、yaw的表示形式，方便观察姿态。

6.姿态的解算

飞行器姿态的改变，可以对应到旋转矩阵的改变，进一步对应到四元数的改变。实时姿态计算，实际上也就是实时更新四元数。
我们可以构建四元数关于时间的微分方程，来研究四元数关于时间的变化规律，求解该微分方程，即可解出四元数。

一阶龙格库塔法求解微分方程

在这里插入图片描述
图片由来

    // 计算姿态。姿态更新

    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;
		
		q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z);
    imu->w *= q_norm_l;
    imu->x *= q_norm_l;
    imu->y *= q_norm_l;
    imu->z *= q_norm_l;

由此可以得到实时的姿态。

二，匿名姿态解算代码分析

//世界坐标平面XY转平面航向坐标XY
void w2h_2d_trans(float w[VEC_XYZ],float ref_ax[VEC_XYZ],float h[VEC_XYZ])
{
	h[X] =  w[X] *  ref_ax[X]  + w[Y] *ref_ax[Y];
	h[Y] =  w[X] *(-ref_ax[Y]) + w[Y] *ref_ax[X];
	
}

//平面航向坐标XY转世界坐标平面XY
void h2w_2d_trans(float h[VEC_XYZ],float ref_ax[VEC_XYZ],float w[VEC_XYZ])
{
	w[X] = h[X] *ref_ax[X] + h[Y] *(-ref_ax[Y]);
	w[Y] = h[X] *ref_ax[Y] + h[Y] *  ref_ax[X];
	
}

上面两个函数就是忽略绕 x轴和y轴选即（忽略Pitch和roll）只进行z轴旋转即Yaw
带入之前推导的旋转矩阵M_z，
ref_ax[X] 为cos $\psi$ ,ref_ax[Y]为sin $\psi$ 。
下面就是坐标系的转换

//载体坐标转世界坐标（ANO约定等同与地理坐标）
float att_matrix[3][3]; //必须由姿态解算算出该矩阵
void a2w_3d_trans(float a[VEC_XYZ],float w[VEC_XYZ])
{
		for(u8 i = 0;i<3;i++)
		{
			float temp = 0;
			for(u8 j = 0;j<3;j++)
			{
				
				temp += a[j] *att_matrix[i][j];
			}
			w[i] = temp;
		}
}

IMU_update函数其实是对IMU数据的传递和解算
参数包括计算周期dT，Imu标志state，滤波转换后的三轴陀螺仪数据gyr，滤波转换后的三轴加速度计数据acc，滤波转换后的三轴磁力计数据gyr，Imu数据结构体Imu
通过IMU数据进行解算姿态和加速度，速度，
四元数的来源是陀螺仪的数据但是单一的陀螺仪数据不能满足飞控的安全要求，需要对其进行补偿和融合
两种传感器有不同的特点：

陀螺仪积分有低频误差
加速度计有高频误差
通过互补滤波的方法提高获取的姿态角的精度。
还有一些姿态数据的融合（罗盘等）
等于说对两个传感器进行一个置信度的分配
最后还对加速度计和磁力计进行互补融合修正使用PI调节修正误差
之后会详细介绍互补滤波的原理和技巧

void IMU_update(float dT,_imu_state_st *state,float gyr[VEC_XYZ], s32 acc[VEC_XYZ],s16 mag_val[VEC_XYZ],_imu_st *imu)
{
//	const float kp = 0.2f,ki = 0.001f;
//	const float kmp = 0.1f;
	
	static float kp_use = 0,ki_use = 0,mkp_use = 0;

	float acc_norm_l;//(加速度向量的模)
	float acc_norm_l_recip;//(加速度向量的模的倒数)
	float q_norm_l;//(四元数的模)
		
	float acc_norm[VEC_XYZ];//(单位化的加速度向量)

	float d_angle[VEC_XYZ];//(微元转角，即滤波修正后的角速度值)
	


	
//		q0q0 = imu->w * imu->w;							
		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;
	
	
		if(state->obs_en)
		{
			//计算机体坐标下的运动加速度观测量。坐标系为北西天
			for(u8 i = 0;i<3;i++)
			{
				s32 temp = 0;
				for(u8 j = 0;j<3;j++)
				{
					
					temp += imu->obs_acc_w[j] *att_matrix[j][i];//t[i][j] 转置为 t[j][i]
				}
				imu->obs_acc_a[i] = temp;
				
				imu->gra_acc[i] = acc[i] - imu->obs_acc_a[i];
			}
		}
		else
		{
			for(u8 i = 0;i<3;i++)
			{			
				imu->gra_acc[i] = acc[i];//(微元转角，即滤波修正后的角速度值)
			}
		}
    //(加速度向量模的倒数)
		acc_norm_l_recip = my_sqrt_reciprocal(my_pow(imu->gra_acc[X]) + my_pow(imu->gra_acc[Y]) + my_pow(imu->gra_acc[Z]));
		//(加速度向量模)	
		acc_norm_l = safe_div(1,acc_norm_l_recip,0);
		
		// 加速度计的读数，单位化。
		for(u8 i = 0;i<3;i++)
		{
			acc_norm[i] = imu->gra_acc[i] *acc_norm_l_recip;
		}

		

		
	// 载体坐标下的(地理)x方向向量，单位化。(地面系北方x [1 0 0]T转到机体系中，w到a系旋转矩阵第一列，可以看作地理x方向在机体系的三个分量)	
    att_matrix[0][0] = imu->x_vec[X] = 1 - (2*q2q2 + 2*q3q3);
    att_matrix[0][1] = imu->x_vec[Y] = 2*q1q2 - 2*q0q3;
    att_matrix[0][2] = imu->x_vec[Z] = 2*q1q3 + 2*q0q2;
		
	// 载体坐标下的y方向向量，单位化。	(地面系西方y [0 1 0]T转到机体系中，w到a系旋转矩阵第二列，可以看作地理y方向在机体系的三个分量)			
    att_matrix[1][0] = imu->y_vec[X] = 2*q1q2 + 2*q0q3;
    att_matrix[1][1] = imu->y_vec[Y] = 1 - (2*q1q1 + 2*q3q3);
    att_matrix[1][2] = imu->y_vec[Z] = 2*q2q3 - 2*q0q1;
		
    // 载体坐标下的z方向向量（等效重力向量、重力加速度向量），单位化。(地面系天空z [0 0 1]T转到机体系中，w到a系旋转矩阵第三列，可以看作地理z方向在机体系的三个分量)			
    att_matrix[2][0] = imu->z_vec[X] = 2*q1q3 - 2*q0q2;
    att_matrix[2][1] = imu->z_vec[Y] = 2*q2q3 + 2*q0q1;
    att_matrix[2][2] = imu->z_vec[Z] = 1 - (2*q1q1 + 2*q2q2);
		
	//水平面方向向量 (a到w系第一列XY，机体系x [1 0 0]T 转到地面系后的XY向量，也就是航向坐标系的机头指向)
	float hx_vec_reci = my_sqrt_reciprocal(my_pow(att_matrix[0][0]) + my_pow(att_matrix[1][0]));
	imu->hx_vec[X] = att_matrix[0][0] *hx_vec_reci;
	imu->hx_vec[Y] = att_matrix[1][0] *hx_vec_reci;
	
	
	// 计算载体坐标下的运动加速度。(加速度传感器测量值去掉重力加速度成分，得到机体系中的a_acc与姿态解算无关)
		for(u8 i = 0;i<3;i++)
		{
			imu->a_acc[i] = (s32)(acc[i] - 981 *imu->z_vec[i]);
		}
		
    
		//计算世界坐标下的运动加速度。坐标系为北西天(利用旋转矩阵变换a_acc,得到地理系中的加速度值w_acc)
		for(u8 i = 0;i<3;i++)
		{
			s32 temp = 0;
			for(u8 j = 0;j<3;j++)
			{
				
				temp += imu->a_acc[j] *att_matrix[i][j];
			}
			imu->w_acc[i] = temp;
		}
		//(再从地理系转换到航向系，得到航向系中加速度值h_acc)
		w2h_2d_trans(imu->w_acc,imu_data.hx_vec,imu->h_acc);
		
		
    // 测量值与等效重力向量的叉积（计算向量误差）。
	/*(这里是把加速度计测量的三轴向量，和载体坐标下的（地理）z方向向量imu->z_vec做叉乘，得到和二者垂直的新向量,
	  acc_norm表示的重力方向来自加速度计，imu->z_vec表示的重力方向来自陀螺仪，这里计算向量外积实质上是在算二者夹角，准备进行融合滤波)		*/

    vec_err[X] =  (acc_norm[Y] * imu->z_vec[Z] - imu->z_vec[Y] * acc_norm[Z]);
    vec_err[Y] = -(acc_norm[X] * imu->z_vec[Z] - imu->z_vec[X] * acc_norm[Z]);
    vec_err[Z] = -(acc_norm[Y] * imu->z_vec[X] - imu->z_vec[Y] * acc_norm[X]);


#ifdef USE_MAG

		//电子罗盘赋值为float矢量
		for(u8 i = 0;i<3;i++)
		{
			mag_val_f[i] = (float)mag_val[i];
		}	
			
		if(!(mag_val[X] ==0 && mag_val[Y] == 0 && mag_val[Z] == 0))
		{
			//把载体坐标下的罗盘数据转换到地理坐标下
			a2w_3d_trans(mag_val_f,imu->w_mag);
			//计算方向向量归一化系数（模的倒数）
			float l_re_tmp = my_sqrt_reciprocal(my_pow(imu->w_mag[0]) + my_pow(imu->w_mag[1]));
			//计算南北朝向向量
			mag_2d_w_vec[1][0] = imu->w_mag[0] *l_re_tmp;
			mag_2d_w_vec[1][1] = imu->w_mag[1] *l_re_tmp;
			//计算南北朝向误差(叉乘)，地理坐标中，水平面磁场方向向量应恒为南北 (1,0)
			mag_yaw_err = vec_2_cross_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);
			//计算南北朝向向量点乘，判断同向或反向
			mag_err_dot_prudoct = vec_2_dot_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);
			//若反向，直接给最大误差
			if(mag_err_dot_prudoct<0)
			{
				mag_yaw_err = my_sign(mag_yaw_err) *1.0f;
			}			
			//
			
		}
#endif
	
		for(u8 i = 0;i<3;i++)
		{
			//死区
#ifdef USE_EST_DEADZONE	
			if(state->G_reset == 0 && state->obs_en == 0)
			{
				vec_err[i] = my_deadzone(vec_err[i],0,imu->gacc_deadzone[i]);
			}
#endif	
//(用了这个，这里是利用加速度模长判断，如果水平方向加速度太大，就不能直接把加速度数据看作重力加速度数据，这一轮计算的偏差清零不补偿了)
#ifdef USE_LENGTH_LIM			
			if(acc_norm_l>1060 || acc_norm_l<900)
			{
				vec_err[X] = vec_err[Y] = vec_err[Z] = 0;
			}
#endif
		//误差积分（用于互补滤波的pi控制器）
		vec_err_i[i] +=  LIMIT(vec_err[i],-0.1f,0.1f) *dT *ki_use;

		
	// 构造增量旋转（含融合纠正）。	
	//    d_angle[X] = (gyr[X] + (vec_err[X]  + vec_err_i[X]) * kp_use - mag_yaw_err *imu->z_vec[X] *kmp_use *RAD_PER_DEG) * dT / 2 ;
	//    d_angle[Y] = (gyr[Y] + (vec_err[Y]  + vec_err_i[Y]) * kp_use - mag_yaw_err *imu->z_vec[Y] *kmp_use *RAD_PER_DEG) * dT / 2 ;
	//    d_angle[Z] = (gyr[Z] + (vec_err[Z]  + vec_err_i[Z]) * kp_use - mag_yaw_err *imu->z_vec[Z] *kmp_use *RAD_PER_DEG) * dT / 2 ;
			
			
#ifdef USE_MAG//（用的这个，在普通互补融合滤波基础上增加了磁力计补偿）
			d_angle[i] = (gyr[i] + (vec_err[i]  + vec_err_i[i]) * kp_use + mag_yaw_err *imu->z_vec[i] *mkp_use) * dT / 2 ;
#else	//（这个没用，普通的互补融合滤波，用加速度计pi控制器输出补偿陀螺仪）
			d_angle[i] = (gyr[i] + (vec_err[i]  + vec_err_i[i]) * kp_use ) * dT / 2 ;
#endif
		}
    // 计算姿态。 姿态更新（一节龙格库塔法，wxyz初值为1000）

    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;
		
		q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z);
    imu->w *= q_norm_l;
    imu->x *= q_norm_l;
    imu->y *= q_norm_l;
    imu->z *= q_norm_l;
		

  
  /修正开关///
#ifdef USE_MAG
		if(state->M_fix_en==0)//磁力
		{
			mkp_use = 0;//不修正
			state->M_reset = 0;//罗盘修正不复位，清除复位标记
		}
		else
		{
			if(state->M_reset)//
			{
				//通过增量进行对准
				mkp_use = 10.0f;
				if(mag_yaw_err != 0 && ABS(mag_yaw_err)<0.02f)
				{
					state->M_reset = 0;//误差小于2的时候，清除复位标记
				}
			}
			else
			{
				mkp_use = state->mkp; //正常修正
			}
		}
#endif
		
		if(state->G_fix_en==0)//重力方向修正
		{
			kp_use = 0;//不修正
		}
		else
		{
			if(state->G_reset == 0)//正常修正
			{			
				kp_use = state->gkp;
				ki_use = state->gki;
			}
			else//快速修正，通过增量进行对准
			{
				kp_use = 10.0f;
				ki_use = 0.0f;
//				imu->est_speed_w[X] = imu->est_speed_w[Y] = 0;
//				imu->est_acc_w[X] = imu->est_acc_w[Y] = 0;
//				imu->est_acc_h[X] = imu->est_acc_h[Y] =0;
				
				//计算静态误差是否缩小
//				imu_reset_val += (ABS(vec_err[X]) + ABS(vec_err[Y])) *1000 *dT;
//				imu_reset_val -= 0.01f;
				imu_reset_val = (ABS(vec_err[X]) + ABS(vec_err[Y]));
				
				imu_reset_val = LIMIT(imu_reset_val,0,1.0f);
				
				if((imu_reset_val < 0.05f) && (state->M_reset == 0))
				{
					//计时
					reset_cnt += 2;
					if(reset_cnt>400)
					{
						reset_cnt = 0;
						state->G_reset = 0;//已经对准，清除复位标记
					}
				}
				else
				{
					reset_cnt = 0;
				}
			}
		}
}

姿态角就是欧拉角通过之前给出的公式计算可以得到

void calculate_RPY()
{
	///输出姿态角///
	
		t_temp = LIMIT(1 - my_pow(att_matrix[2][0]),0,1);
		
		//imu_data.pit = asin(2*q1q3 - 2*q0q2)*57.30f;
	
		if(ABS(imu_data.z_vec[Z])>0.05f)//避免奇点的运算
		{
			imu_data.pit =  fast_atan2(att_matrix[2][0],my_sqrt(t_temp))*57.30f;
			imu_data.rol =  fast_atan2(att_matrix[2][1], att_matrix[2][2])*57.30f; 
			imu_data.yaw = -fast_atan2(att_matrix[1][0], att_matrix[0][0])*57.30f; 
		}
}