飞控陀螺仪，磁力计，加速计，四元数姿态结算

2023-05-16

MPU6050主要包含陀螺仪和加速度计。陀螺仪主要测量角速度，即可以测出某一时间段物体转过的角度。加速度计测量的是物体的加速度，重力加速度即物体受重力作用的情况下具有的加速度，物体静止时，加速度计测出来的加速度等于重力加速度g，约等于9.8米每平方秒，重力加速度总是竖直向下，通过计算重力加速度在X,Y的分量，则可以计算出物体相对于水平面的倾斜角度。

我们先来看看如何用欧拉角描述旋转(坐标变换)：

绕Z轴旋转即滚转角，绕Y轴旋转即偏航角，绕X旋转即俯仰角。

这里需要注意绕X,Y,Z轴方向旋转的先后顺序不一样，得出余弦矩阵的顺序（也就是公式）也不一样。并且在一组旋转里X,Y,Z不可交换(Euler旋转)，所以有先后顺序之分（程序中）。旋转变换有12种表示方式，分别为：XYZ、XZY、XYX、XZX、YXZ、YZX、YXY、YZY、ZXY、ZYX、ZXZ、ZYZ。这里还有一点需要注意，那就是"Gimbal Lock"。图中（ZYX，以下均为此顺序）的绕Z旋转会导致YX轴变化，但是Y影响X不会影响Z，X不会影响ZY。因此，如果旋转Y±90°，那么Z轴方向不变，但是会改变X轴方向，导致YX轴同向。此时保持Y值不变，那么改变Z或者X的效果相同。

Gimbal Lock总结就是，其源自Euler旋转原理，此原理旋转变量不可交换，所以有先后之分，所以可以改变后两个轴而第一轴方向不变，所以产生轴共线，即Gimbal Lock。

上图中得出旋转后的欧拉角公式，但是无人机的姿态结算中不能用欧拉角公式计算，一方面是因为欧拉角微分方程中包含了大量的三角运算，这给实时结算带来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“Gimbal Lock”。所以欧拉角方法只适合水平姿态变化不大的情况，而不适用于飞行器的姿态确定。

下面对四元数姿态进行结算：

重力加速度归一化。即将加速度计的三维向量（ax,ay,az）转换为单位向量，因为是单位矢量到参考性的投影，所以要把加速度计数据单位化。归一化只是改变这三个向量的长度，方向不改变，也就是只改变了相同的倍数，只是为了与单位四元数对应。ax,ay,az是机体坐标参照系上，加速度测出来的重力向量。
四元数换成方向余弦中的第三行的三个元素。将当前姿态重力在三个轴的分量分离出来，把四元数换算成方向余弦中的第三行的三个元素(vx,vy,vz)。惯性测量器件测量的都是关于b系的值。vx,vy,vz是陀螺仪的值积分后的姿态来推算出的重力向量。
向量叉积得出姿态误差。将ax,ay,az和vx,vy,vz对应的进行向量叉积（向量外积、叉乘）。分别得出ex,ey,ez。这个叉积向量仍然是位于机体坐标系，并和积分误差成正比，正好矫正陀螺。
对误差进行积分。将ax,ay,az和vx,vy,vz的误差进行积分消除误差。
进行滤波。陀螺仪测的值不断的进行更新，相应的积分误差也不断的修正，最后将积分误差反馈给陀螺仪，修正陀螺仪的值。
四元数微分方程。通过一阶龙格库塔法更新四元数。
四元数归一化。对四元数的单位化，单位化的四元数可以表示一个完整的旋转，只有单位四元数才可以表示旋转，这就是四元数表示旋转的约束条件。
四元数转欧拉角。

程序如下：


 
 
   
   
   
   
    
    
     
     float Kp = 
     
     0.4f; 
     
     // 比例增益
    
    
   
   

   
   
   
   
    
    
     
     float Ki = 
     
     0.001f; 
     
     // 积分增益
    
    
   
   

   
   
   
   
    
    
     
     float exInt = 
     
     0.f; 
     
     // 重力在X轴上的分量
    
    
   
   

   
   
   
   
    
    
     
     float eyInt = 
     
     0.0f;
    
    
   
   

   
   
   
   
    
    
     
     float	ezInt = 
     
     0.0f;
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     //四元数
    
    
   
   

   
   
   
   
    
    
     
     static 
     
     float q0 = 
     
     1.0f;
    
    
   
   

   
   
   
   
    
    
     
     static 
     
     float q1 = 
     
     0.0f;
    
    
   
   

   
   
   
   
    
    
     
     static 
     
     float q2 = 
     
     0.0f;
    
    
   
   

   
   
   
   
    
    
     
     static 
     
     float q3 = 
     
     0.0f;
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     float Rot_matrix[
     
     3][
     
     3];
    
    
   
   

   
   
   
   
    
    
     
     Vector3f angle1;
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     void	IMU_Updata(Vector3f acc,Vector3f gyro,Attitude *state,float dt)
    
    
   
   

   
   
   
   
    
    
     
     {
    
    
   
   

   
   
   
   

   
   
   
   
    
        
     
     float norm;
    
    
   
   

   
   
   
   
    
        
     
     float ex,ey,ez;
    
    
   
   

   
   
   
   
    
        
     
     float halfT	= dt/
     
     2.0f;
    
    
   
   

   
   
   
   
    
        
     
     //重力加速度在X,Y,Z方向的分量
    
    
   
   

   
   
   
   
    
        
     
     static 
     
     float verxZ,veryZ,verzZ;
    
    
   
   

   
   
   
   
    
    
     
         angle1.x = 
     
     0.0f;
    
    
   
   

   
   
   
   
    
    
     
         angle1.y = 
     
     0.0f;
    
    
   
   

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

   
   
   
   

   
   
   
   
    
        
     
     //1、获取原始值
    
    
   
   

   
   
   
   
    
        
     
     //角速度转弧速度
    
    
   
   

   
   
   
   
    
    
     
         gyro.x = gyro.x	* DEG2RAD;
    
    
   
   

   
   
   
   
    
    
     
         gyro.y = gyro.y	* DEG2RAD;
    
    
   
   

   
   
   
   
    
    
     
         gyro.z = gyro.z	* DEG2RAD;
    
    
   
   

   
   
   
   

   
   
   
   
    
        
     
     //2、加速度计量化
    
    
   
   

   
   
   
   
    
        
     
     if((acc.x!=
     
     0.0f)||(acc.y!=
     
     0.0f)||(acc.z!=
     
     0.0f))
    
    
   
   

   
   
   
   
    
    
     
         {
    
    
   
   

   
   
   
   
    
    
     
     	norm =	sqrtf3(acc.x , acc.y , acc.z);
    
    
   
   

   
   
   
   
    
    
     
     	acc.x = acc.x /norm ;
    
    
   
   

   
   
   
   
    
    
     
     	acc.y = acc.y /norm ;
    
    
   
   

   
   
   
   
    
    
     
     	acc.z = acc.z /norm ;
    
    
   
   

   
   
   
   

   
   
   
   

   
   
   
   
    
    	
     
     //3、叉乘
    
    
   
   

   
   
   
   
    
    
     
     	ex = (acc.y * verzZ) - (veryZ * acc.z);
    
    
   
   

   
   
   
   
    
    
     
     	ey = (acc.x * verzZ) - (verxZ * acc.z);
    
    
   
   

   
   
   
   
    
    
     
     	ez = (acc.x * veryZ) - (verxZ * acc.y);
    
    
   
   

   
   
   
   

   
   
   
   
    
    	
     
     //4、积分误差滤波
    
    
   
   

   
   
   
   
    
    
     
     	gyro.x += ex	* Kp + exInt;
    
    
   
   

   
   
   
   
    
    
     
     	gyro.y += ey	* Kp + eyInt;
    
    
   
   

   
   
   
   
    
    
     
     	gyro.z += ez	* Kp + ezInt;	
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     }
    
    
   
   

   
   
   
   

   
   
   
   
    
        
     
     float q0_last =q0;
    
    
   
   

   
   
   
   
    
        
     
     float q1_last =q1;
    
    
   
   

   
   
   
   
    
        
     
     float q2_last =q2;
    
    
   
   

   
   
   
   
    
        
     
     float q3_last =q3;
    
    
   
   

   
   
   
   

   
   
   
   
    
        
     
     //解四元微分方程
    
    
   
   

   
   
   
   
    
    
     
         q0 += (-q1_last*gyro.x - q2_last * gyro.y - q3_last *gyro.z)*halfT;
    
    
   
   

   
   
   
   
    
    
     
         q1 += (	q0_last*gyro.x + q3_last * gyro.y - q2_last *gyro.z)*halfT;
    
    
   
   

   
   
   
   
    
    
     
         q2 += (-q3_last*gyro.x + q0_last * gyro.y + q1_last *gyro.z)*halfT;
    
    
   
   

   
   
   
   
    
    
     
         q3 += (	q2_last*gyro.x - q1_last * gyro.y + q0_last *gyro.z)*halfT;
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
         norm = sqrtf4(q0,q1,q2,q3);
    
    
   
   

   
   
   
   
    
    
     
         q0 = q0/norm;
    
    
   
   

   
   
   
   
    
    
     
         q1 = q1/norm;
    
    
   
   

   
   
   
   
    
    
     
         q2 = q2/norm;
    
    
   
   

   
   
   
   
    
    
     
         q3 = q3/norm;
    
    
   
   

   
   
   
   

   
   
   
   

   
   
   
   
    
    
     
         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;
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
         verxZ = 
     
     2.0f * (q1q3 - q0q2); 
    
    
   
   

   
   
   
   
    
    
     
         veryZ = 
     
     2.0f * (q2q3 + q0q1);
    
    
   
   

   
   
   
   
    
    
     
         verzZ = q0q0 - q1q1 - q2q2 + q3q3;
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     0][
     
     0] = q0q0 + q1q1 - q2q2 - q3q3;
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     0][
     
     1] = 
     
     2.0f * (q1q2 - q0q3);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     0][
     
     2] = 
     
     2.0f * (q1q3 + q0q2);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     1][
     
     0] = 
     
     2.0f * (q1q2 + q0q3);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     1][
     
     1] = q0q0 - q1q1 + q2q2 - q3q3;
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     1][
     
     2] = 
     
     2.0f * (q2q3 - q0q1);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     2][
     
     0] = 
     
     2.0f * (q1q3 - q0q2);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     2][
     
     1] = 
     
     2.0f * (q2q3 + q0q1);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     2][
     
     2] = q0q0 - q1q1 - q2q2 + q3q3;
    
    
   
   

   
   
   
   

   
   
   
   
    
        
     
     //四元数转换为欧拉角输出
    
    
   
   

   
   
   
   
    
    
     
         state->pitch = 
     
     asin(Rot_matrix[
     
     1][
     
     2]);
    
    
   
   

   
   
   
   
    
    
     
         state->roll	 = 
     
     atan2(-Rot_matrix[
     
     0][
     
     2],Rot_matrix[
     
     2][
     
     2]);
    
    
   
   

   
   
   
   
    
    
     
         state->yaw   = 
     
     atan2(-Rot_matrix[
     
     1][
     
     0], Rot_matrix[
     
     1][
     
     1]);
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     }

运算宏定义如下：


 
 
   
   
   
   
    
    
     
     #define MM2M 			0.001f
    
    
   
   

   
   
   
   
    
    
     
     #define DEG2RAD 	0.017453292519943295769236907684886f	//角度转弧度
    
    
   
   

   
   
   
   
    
    
     
     #define RAD2DEG 	57.295779513082320876798154814105f	//弧度转角度
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     #define sq(a)		((a) * (a))	//平方、
    
    
   
   

   
   
   
   
    
    
     
     #define	sqrtf4(a,b,c,d)	__sqrtf(sq(a) + sq(b) + sq(c) + sq(d))
    
    
   
   

   
   
   
   
    
    
     
     #define sqrtf3(a,b,c)	__sqrtf(sq(a) + sq(b) + sq(c))
    
    
   
   

   
   
   
   
    
    
     
     #define	sqrtf2(a,b)	__sqrtf(sq(a) + sq(b))

在加速度计矫正的基础上+磁力计：
在三维空间中，根据重力加速度，加速度为我们提供一个水平位置的参考，但是无法获得方向的参考，这时就需要磁力计，它能给人们提供一个正北方向的绝对参考。

在上述姿态结算的基础上进行磁力计矫正：

把磁力计的数据进行归一化处理（进行量化）。
根据当前四元数姿态值估算各重力分量vx,vy,vz，再根据vx,vy,vz预估磁场的方向wx,wy,wz。
再根据wx,wy,wz对磁力计测出的值进行误差矫正。
把加速度计和磁力计修正后的陀螺仪数据整合到四元数中。
最后进行角度运算。


 
 
   
   
   
   
    
    
     
     float KpDef = 
     
     0.5f;
    
    
   
   

   
   
   
   
    
    
     
     float KiDef	=	
     
     0.025f;
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     float q0 = 
     
     1.0f,q1 = 
     
     0.0f,q2 = 
     
     0.0f,q3 = 
     
     0.0f;
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     float integralFBx = 
     
     0.0f,integralFBy = 
     
     0.0f,integralFBz = 
     
     0.0f;
    
    
   
   

   
   
   
   

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

   
   
   
   

   
   
   
   
    
    
     
     static 
     
     float Rot_matrix[
     
     3][
     
     3];	
     
     //方向余弦矩阵
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     //使用加速度传感器测量重力向量，使用磁力计测量方向
    
    
   
   

   
   
   
   
    
    
     
     void AHRS_Uptate(Vector3f acc,Vector3f gyro,Vector3i mag,Attitude *state,float dt)
    
    
   
   

   
   
   
   
    
    
     
     {
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
         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;
    
    
   
   

   
   
   
   
    
    
     
     //大地坐标系(e)  --> 机体坐标系(b)
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     0][
     
     0] = q0q0 + q1q1 - q2q2 - q3q3;
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     0][
     
     1] = 
     
     2.f * (q1q2 + q0q3);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     0][
     
     2] = 
     
     2.f * (q1q3 - q0q2);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     1][
     
     0] = 
     
     2.f * (q1q2 - q0q3);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     1][
     
     1] = q0q0 - q1q1 + q2q2 - q3q3;
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     1][
     
     2] = 
     
     2.f * (q2q3 + q0q1);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     2][
     
     0] = 
     
     2.f * (q1q3 + q0q2);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     2][
     
     1] = 
     
     2.f * (q2q3 - q0q1);
    
    
   
   

   
   
   
   
    
    
     
         Rot_matrix[
     
     2][
     
     2] = q0q0 - q1q1 - q2q2 + q3q3;
    
    
   
   

   
   
   
   

   
   
   
   
    
        
     
     float ex = 
     
     0.0f, ey = 
     
     0.0f, ez = 
     
     0.0f;
    
    
   
   

   
   
   
   
    
        
     
     float norm;
    
    
   
   

   
   
   
   

   
   
   
   
    
        
     
     if((mag.x != 
     
     0) || (mag.y != 
     
     0) || (mag.z !=
     
     0))
    
    
   
   

   
   
   
   
    
    
     
         {
    
    
   
   

   
   
   
   
    
    	
     
     //磁力计量化
    
    
   
   

   
   
   
   
    
    
     
     	norm = sqrtf3(mag.x,mag.y,mag.z);
    
    
   
   

   
   
   
   
    
    
     
     	mag.x /= norm;
    
    
   
   

   
   
   
   
    
    
     
     	mag.y /= norm;
    
    
   
   

   
   
   
   
    
    
     
     	mag.z /= norm;
    
    
   
   

   
   
   
   

   
   
   
   
    
    	
     
     //地球磁场的参考方向
    
    
   
   

   
   
   
   
    
    	
     
     float hx = Rot_matrix[
     
     0][
     
     0] * mag.x + Rot_matrix[
     
     1][
     
     0] * mag.y + Rot_matrix[
     
     2][
     
     0] * mag.z ;
     
     //计算出x轴的方向
    
    
   
   

   
   
   
   
    
    	
     
     float hy = Rot_matrix[
     
     0][
     
     1] * mag.x + Rot_matrix[
     
     1][
     
     1] * mag.y + Rot_matrix[
     
     2][
     
     1]	* mag.z	;
     
     //计算出Y轴的方向
    
    
   
   

   
   
   
   
    
    	
     
     float by = sqrtf2(hx,hy);	
     
     //预估磁场的方向
    
    
   
   

   
   
   
   
    
    	
     
     float bz = Rot_matrix[
     
     0][
     
     2] * mag.x + Rot_matrix[
     
     1][
     
     2] * mag.y + Rot_matrix[
     
     2][
     
     2] * mag.z	;
     
     //计算出Z轴方向
    
    
   
   

   
   
   
   

   
   
   
   
    
    	
     
     //估计磁场的方向
    
    
   
   

   
   
   
   
    
    	
     
     float wx = Rot_matrix[
     
     0][
     
     1] *	by + Rot_matrix[
     
     0][
     
     2] * bz;
    
    
   
   

   
   
   
   
    
    	
     
     float wy = Rot_matrix[
     
     1][
     
     1]	* by + Rot_matrix[
     
     1][
     
     2] * bz;
    
    
   
   

   
   
   
   
    
    	
     
     float wz = Rot_matrix[
     
     2][
     
     1]	* by + Rot_matrix[
     
     2][
     
     2]	*	bz;
    
    
   
   

   
   
   
   

   
   
   
   
    
    	
     
     //误差是估计方向和场矢量测量方向的乘积
    
    
   
   

   
   
   
   
    
    
     
     	ex = mag.y * wz - mag.z * wy;
    
    
   
   

   
   
   
   
    
    
     
     	ey = mag.z * wx - mag.x * wz;
    
    
   
   

   
   
   
   
    
    
     
     	ez = mag.x * wz - mag.z * wx;
    
    
   
   

   
   
   
   

   
   
   
   
    
    	
     
     if((acc.x !=
     
     0)||(acc.y !=
     
     0)||(acc.z !=
     
     0))
    
    
   
   

   
   
   
   
    
    
     
     	{
    
    
   
   

   
   
   
   
    
    	 
     
     //加速度值量化
    
    
   
   

   
   
   
   
    
    
     
     	norm = sqrtf3(acc.x,acc.y,acc.z);
    
    
   
   

   
   
   
   
    
    
     
     	acc.x /= norm;
    
    
   
   

   
   
   
   
    
    
     
     	acc.y	/= norm;
    
    
   
   

   
   
   
   
    
    
     
     	acc.z	/= norm;
    
    
   
   

   
   
   
   

   
   
   
   
    
    	
     
     //根据当前四元数的姿态值估算出各重力分量Vx,Vy,Vz
    
    
   
   

   
   
   
   
    
    	
     
     float vx = Rot_matrix[
     
     0][
     
     2];
    
    
   
   

   
   
   
   
    
    	
     
     float vy = Rot_matrix[
     
     1][
     
     2];
    
    
   
   

   
   
   
   
    
    	
     
     float vz = Rot_matrix[
     
     2][
     
     2];
    
    
   
   

   
   
   
   

   
   
   
   
    
    	
     
     //使用叉积来计算重力误差
    
    
   
   

   
   
   
   
    
    
     
     	ex += acc.y * vz - acc.z * vy;
    
    
   
   

   
   
   
   
    
    
     
     	ey += acc.z * vx - acc.x * vz;
    
    
   
   

   
   
   
   
    
    
     
     	ez += acc.x * vy - acc.y * vx;
    
    
   
   

   
   
   
   
    
    
     
     	}
    
    
   
   

   
   
   
   

   
   
   
   
    
            
     
     //只有当从加速度计或磁力计收集有效数据时才应用反馈
    
    
   
   

   
   
   
   
    
    	
     
     if(ex != 
     
     0.0f && ey != 
     
     0.0f && ez !=
     
     0.0f)
    
    
   
   

   
   
   
   
    
    
     
     	{
    
    
   
   

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

   
   
   
   
    
    	
     
     if(KiDef > 
     
     0.0f)
    
    
   
   

   
   
   
   
    
    
     
     	{
    
    
   
   

   
   
   
   
    
    
     
     	    integralFBx += KiDef * ex * dt;
    
    
   
   

   
   
   
   
    
    
     
     	    integralFBy += KiDef * ey *	dt;
    
    
   
   

   
   
   
   
    
    
     
     	    integralFBz += KiDef * ez *	dt;
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     	    gyro.x += integralFBx;
    
    
   
   

   
   
   
   
    
    
     
     	    gyro.y += integralFBy;
    
    
   
   

   
   
   
   
    
    
     
     	    gyro.z += integralFBz;
    
    
   
   

   
   
   
   
    
    
     
     	}
    
    
   
   

   
   
   
   
    
    	
     
     else
    
    
   
   

   
   
   
   
    
    
     
     	{
    
    
   
   

   
   
   
   
    
    
     
     	    integralFBx = 
     
     0.0f;
    
    
   
   

   
   
   
   
    
    
     
     	    integralFBy = 
     
     0.0f;
    
    
   
   

   
   
   
   
    
    
     
     	    integralFBz = 
     
     0.0f;
    
    
   
   

   
   
   
   
    
    
     
     	}
    
    
   
   

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

   
   
   
   

   
   
   
   
    
    
     
     	    gyro.x += KpDef * ex;
    
    
   
   

   
   
   
   
    
    
     
     	    gyro.y += KpDef * ey;
    
    
   
   

   
   
   
   
    
    
     
     	    gyro.z += KpDef *	ez;
    
    
   
   

   
   
   
   
    
    
     
     	}
    
    
   
   

   
   
   
   
    
    	
     
     float dq0 = 
     
     0.5f*(-q1 * gyro.x - q2 * gyro.y - q3 * gyro.z);
    
    
   
   

   
   
   
   
    
    	
     
     float dq1 = 
     
     0.5f*( q0 * gyro.x + q2 * gyro.z - q3 * gyro.y);
    
    
   
   

   
   
   
   
    
    	
     
     float dq2 = 
     
     0.5f*( q0 * gyro.y - q1 * gyro.z + q3 * gyro.x);
    
    
   
   

   
   
   
   
    
    	
     
     float dq3 = 
     
     0.5f*( q0 * gyro.z + q1 * gyro.y - q2 * gyro.x); 
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     	q0 += dt * dq0;
    
    
   
   

   
   
   
   
    
    
     
     	q1 += dt * dq1;
    
    
   
   

   
   
   
   
    
    
     
     	q2 += dt * dq2;
    
    
   
   

   
   
   
   
    
    
     
     	q3 += dt * dq3;
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     	norm = sqrtf4(q0, q1, q2, q3);
    
    
   
   

   
   
   
   
    
    
     
     	q0 /= norm;
    
    
   
   

   
   
   
   
    
    
     
     	q1 /= norm;
    
    
   
   

   
   
   
   
    
    
     
     	q2 /= norm;
    
    
   
   

   
   
   
   
    
    
     
     	q3 /= norm;		
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     	Rot_matrix[
     
     0][
     
     0] = q0q0 + q1q1 - q2q2 - q3q3;
    
    
   
   

   
   
   
   
    
    
     
     	Rot_matrix[
     
     0][
     
     1] = 
     
     2.f * (q1q2 + q0q3);
    
    
   
   

   
   
   
   
    
    
     
     	Rot_matrix[
     
     0][
     
     2] = 
     
     2.f * (q1q3 - q0q2);
    
    
   
   

   
   
   
   
    
    
     
     	Rot_matrix[
     
     1][
     
     0] = 
     
     2.f * (q1q2 - q0q3);
    
    
   
   

   
   
   
   
    
    
     
     	Rot_matrix[
     
     1][
     
     1] = q0q0 - q1q1 + q2q2 - q3q3;
    
    
   
   

   
   
   
   
    
    
     
     	Rot_matrix[
     
     1][
     
     2] = 
     
     2.f * (q2q3 + q0q1);
    
    
   
   

   
   
   
   
    
    
     
     	Rot_matrix[
     
     2][
     
     0] = 
     
     2.f * (q1q3 + q0q2);
    
    
   
   

   
   
   
   
    
    
     
     	Rot_matrix[
     
     2][
     
     1] = 
     
     2.f * (q2q3 - q0q1);
    
    
   
   

   
   
   
   
    
    
     
     	Rot_matrix[
     
     2][
     
     2] = q0q0 - q1q1 - q2q2 + q3q3;
    
    
   
   

   
   
   
   

   
   
   
   
    
    	
     
     //四元数转换为欧拉角输出
    
    
   
   

   
   
   
   
    
    
     
     	state->pitch = 
     
     asin(Rot_matrix[
     
     1][
     
     2]);
    
    
   
   

   
   
   
   
    
    
     
     	state->roll	 = 
     
     atan2(-Rot_matrix[
     
     0][
     
     2],Rot_matrix[
     
     2][
     
     2]);
    
    
   
   

   
   
   
   
    
    
     
     	state->yaw   = 
     
     atan2(-Rot_matrix[
     
     1][
     
     0], Rot_matrix[
     
     1][
     
     1]);	
    
    
   
   

   
   
   
   
    
    
     
         }
    
    
   
   

   
   
   
   
    
    
     
     }

运算宏定义如下：


 
 
   
   
   
   
    
    
     
     #define MM2M 		0.001f
    
    
   
   

   
   
   
   
    
    
     
     #define DEG2RAD 	0.017453292519943295769236907684886f	// 角度转弧度
    
    
   
   

   
   
   
   
    
    
     
     #define RAD2DEG 	57.295779513082320876798154814105f	// 弧度转角度 
    
    
   
   

   
   
   
   

   
   
   
   
    
    
     
     #define sq(a)		((a) * (a))	//平方、
    
    
   
   

   
   
   
   
    
    
     
     #define	sqrtf4(a,b,c,d)	__sqrtf(sq(a) + sq(b) + sq(c) + sq(d))
    
    
   
   

   
   
   
   
    
    
     
     #define sqrtf3(a,b,c)	__sqrtf(sq(a) + sq(b) + sq(c))
    
    
   
   

   
   
   
   
    
    
     
     #define	sqrtf2(a,b)	__sqrtf(sq(a) + sq(b))

以上仅为本人的理解，仅供参考，如有细节上的错误希望大神指点指点！

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

飞控陀螺仪，磁力计，加速计，四元数姿态结算的相关文章

关于我使用的安卓View Binding方式

方案有 xff1a ButterKnife findViewById View Binding 下面看下View Binding在下面的场景怎么使用 Activities Fragments Inflate Bind RecyclerVie
SpringBoot

resources application properties xff1a Spring Boot应用的配置文件 xff1b 可以修改一些默认设置 xff1b server port 61 8081 改变默认端口号使用SpringMVC
[转]机器学习之第一天

一机器学习简介 xff08 一 xff09 简单介绍 1 人工智能之父图灵图灵测试 xff08 1950 xff09 2 马文李闵斯基 xff1a 把人工智能推向了全世界 xff1b 3 人工智能 xff08 1950 xff09
[转]【微服务架构

GitLab 新建 myshop 项目使用 ssh 克隆到本地将代码复制进 myshop 提交到本地仓库提交到远程仓库从 GitLab 拉取源代码到 Docker 容器中生成 SSH KEY ssh keygen t rsa C
[转]Spring Cloud Netflix服务搭建详解（一）：服务的注册与发现

文章目录简介导航 xff1a 创建统一的依赖管理概述创建依赖管理项目服务注册与发现概述创建服务注册中心Applicationapplication yml操作界面创建服务提供者概述POMApplicationapplication
[转]Spring Cloud Netflix服务搭建详解（二）：熔断器和Zuul网关

文章目录使用熔断器防止服务雪崩概述Ribbon 中使用熔断器和熔断器仪表盘在 pom xml 中增加依赖启动类上增加 64 EnableHystrix 注解在 Service 中增加 64 HystrixCommand 注解测试熔断器
[转]Spring Cloud Netflix服务搭建详解（三）：分布式配置中心和服务监控追踪

文章目录分布式配置中心分布式配置中心服务端创建项目pom文件Applicationapplication yml测试附 xff1a HTTP 请求地址和资源文件映射分布式配置中心客户端概述Applicationapplication
SSM众筹网站手写分页,同步

jsp main jsp 发出请求 lt a href 61 34 APP PATH user index do 34 gt lt i class 61 34 glyphicon glyphicon user 34 gt lt i gt 用
【java毕业设计】基于Spring Boot+mysql的音乐网站与分享平台系统设计与实现（程序源码）-音乐网站与分享平台

基于Spring Boot 43 mysql的音乐网站与分享平台系统设计与实现 xff08 程序源码 43 毕业论文 xff09 大家好 xff0c 今天给大家介绍基于Spring Boot 43 mysql的音乐网站与分享平台系统设计与实
spring boot + vue + 图片验证码登录实例( 从菜鸟到小白 )

spring boot 实现验证数据库登录 43 图片验证码登录 1 先创建个数据库数据库大概就是这个样子 xff08 表名先叫 user xff09 2 在项目里面写好dao entity 等等一系列东西我这里就不一一列出来了 xff
SSM众筹网站手写分页，异步

jsp main jsp 发出请求 lt a href 61 34 APP PATH user toIndex do 34 gt lt i class 61 34 glyphicon glyphicon user 34 gt lt i gt
模糊查询-注意事项

1 动态查询语句 2 SQL中占位符不能在单引号中 xff0c 否则 xff0c 会以进行查询数据 39 param 39 39 39 3 SQL中不能使用加号进行字符串拼接 xff0c 加号是用来做运算的 39 39 43 39 D 3
克龙表达式

Cron表达式详解 1 基本格式用空格分开的7个部分 xff0c 按顺序依次为秒 xff1a 0 59 分钟 xff1a 0 59 小时 xff1a 0 23 日期 xff1a 1 31 xff0c 但是需要考虑月的实际天数 day o
The method xxx of type xxx must override a superclass method

原因是1 5及以下不认识 64 override 修改编译版本为1 6及以上步骤1 2 步骤2 3
删除Activiti表单

DROP TABLE IF EXISTS ACT RE DEPLOYMENT DROP TABLE IF EXISTS ACT GE PROPERTY DROP TABLE IF EXISTS ACT HI ACTINST DROP TAB
表单重置的2种方式

1 34 resetBtn 34 click function 34 advertForm 34 0 reset 2 34 resetBtn 34 click function document getElementById 34 adve
mybatis的一些笔记

目录结论 xff1a mybatis是与数据库交互的框架 MyBatis 01 HelloWorld MyBatis 02 config MyBatis 03 mapper sql映射文件的增删改查 1 单个参数 xff1a mybati
SpringMVC笔记

目录 QuickStart 重要注解 64 RequestMapping 1 把数据封装到bean中且bean中有引用类型user name的值与bean 的参数必须完全一致 xff0c 自动封装 xff0c 引用的类型用级联user u
解决ubuntu下c++标准库缺少conio.h

该头文件是用于检测键盘输入的 xff0c 在windows下 xff0c c 43 43 标准库是自带的 xff0c 但是在Linux下没有该头文件 xff0c 可以通过手动的方式进行添加步骤如下 xff1a 1 打开终端 xff0c s
重装系统之gpt分区与mbr分区

重装ubunutu系统时往往会碰到不能将系统装在所选分区的情况 xff0c 一般是因为硬盘原本分区不是gpt分区重装windows系统时往往会碰到不能将系统装在所选分区的情况 xff0c 一般是因为硬盘原本分区不是mbr分区 gpt

随机推荐

Debian的GNOME汉化(美化)简易教程(转)

Debian的GNOME汉化美化简易教程转 64 more 64 Debian的GNOME汉化美化简易教程 fcitx开机启动 tahoma英文显示 simsun中文显示 openoffice字体调整这是我自己汉化GNOME的心
Centos8 系统下安装jdk1.8

查看是否已经安装java java version 检查 yum 中有没有 java1 8 包 yum list java 1 8 安装jdk yum install java 1 8 0 openjdk y
3.3:如何在Python中创建文件？

一在Python中创建文件的主要步骤包括 1 使用open 函数打开一个文件对象 2 使用文件对象的write 方法向文件中写入内容 3 关闭文件对象以释放文件资源二 open 函数有三个参数 1 文件0名需要创建的文件的名称和路径
Android设置暗码简要流程

设置暗码 1 Phone对暗码的简要处理流程路径 packages services Telephony src com android phone SpecialCharSequenceMgr java phone中对暗码的处理 spa
[教程]Ubuntu20.04安装Node.js

前言这两天在装一个工具软件的时候 xff0c 要求先安装Node js xff0c 我也不懂这是用来干什么的 xff0c 只知道跟JavaScript有关不懂没关系 xff0c 装就完事了 xff0c 管它呢本文将介绍3种在Ubunt
Ubuntu使用管理员（root）身份登录系统

Ubuntu使用管理员 xff08 root xff09 身份登录系统一 Ubuntu安装好后 xff0c Ubuntu系统默认root用户是不能登录的 xff0c 密码也是空的如果想要使用root用户登录 xff0c 必须先为root
Android使用代码进行界面布局和改变图标、标题、名称、主界面

一代码进行界面布局 lt xml version 61 34 1 0 34 encoding 61 34 utf 8 34 gt 二 Android Studio改变图标标题名称主界面注意 xff1a 64 表示是在哪个目录或者是
Linux（Ubuntu）系统如何安装Python

Linux 系统是为编程而生的 xff0c 因此绝大多数的 Linux 发行版 xff08 Ubuntu CentOS 等 xff09 都默认自带了 Python 有的 Linux 发行版甚至还会自带两个版本的 Python xff0c 例
如何用python实现多线程爬虫

当单线程python爬虫已经不能满足企业需求时 xff0c 很多程序员会进行改代码或者增加服务器数量 xff0c 这样虽说也能达到效果 xff0c 但是对于人力物力也是一笔不小的消耗如果是技术牛点的 xff0c 正常都会自己重新改写多线程
## Hive分析疫情数据

拿到的数据部分如下 xff1a 4月27日黑龙江境外输入不详 0 45 0 黑龙江卫健委 https m thepaper cn newsDetail forward 7160075 4月27日内蒙古境外输入不详 0 8 0 央
python --根据windows窗口名称、进程pid打开窗口(pygetwindow详解)

pygetwindow详解简介 pygetwindow是一个Python库 xff0c 用于获取操作和管理当前打开的窗口它提供了一些常用的窗口操作方法 xff0c 包括获取窗口句柄 xff0c 获取窗口位置和大小 xff0c 移动和调
Redhat Linux advance Server V2.１无法进入桌面(转)

Redhat Linux advance Server V2 xff11 无法进入桌面转 64 more 64 在本地调试安装了个Redhat Linux advance Server V2 1 xff0c 一共有5张光盘 xff0c 我
Mac上类似于xshell的远程工具：finalshell 和 royal tsx

FinalShell 国产国产国产自己研发的是一体化的的服务器网络管理软件不仅是ssh客户端还是功能强大的开发运维工具充分满足开发运维需求特色功能免费海外服务器远程桌面加速 ssh加速本地化命令输入框支持自动补全
css查找元素注意事项

一 CSS ID 选择器查找元素 1 注意 xff1a 如果元素的ID不唯一 xff0c 或者是动态的或者name以及linktext属性值也不唯一我们就需要考虑用Xpath来查找元素了 xff0c 然后再对元素执行操作不管用什么方式
OS2.3.7:多生产者，多消费者问题

文章目录 0 问题描述1 问题分析2 实现3 总结 0 问题描述桌子上有一只盘子 xff0c 每次只能向其中放入一个水果爸爸专向盘子中放苹果 xff0c 妈妈专向盘子中放橘子 xff0c 儿子专等着吃盘子中的橘子 xff0c 女儿专等着
java 方法名类名命名规范

一命名规范 1 项目名全部小写 2 包名全部小写 3 类名首字母大写 xff0c 如果类名由多个单词组成 xff0c 每个单词的首字母都要大写大驼峰 xff0c 如 xff1a public class MyFirstClass 4 变
Qt arm环境安装

一相关工作准备 Qt opensource 和 Qt everywhere 下载链接版本为5 9 8 arm linux gcc下载链接版本为4 8 3 tslib 下载链接版本为1 21 ps 可以不安装Qt opensou
STM32驱动ST7789V2 tft屏幕

一简介本次教程使用的是1 54寸240 240像素的tft屏幕 xff0c 其接口协议为SPI协议在使用的过程中仅需要四根数据即可驱动点亮屏幕然后硬件使用的是STM32F103C8T6核心板 xff0c 用的是SPI2 一般购买屏幕
linux设置复杂度策略、登录超时处理功能

1 在字符终端下 xff0c 实现某一用户连续错误登陆N次后 xff0c 就锁定该用户X分钟 pam tally2 执行 vi etc pam d login 在 PAM 1 0 下新起一行 xff0c 加入 auth required p
飞控陀螺仪，磁力计，加速计，四元数姿态结算

MPU6050主要包含陀螺仪和加速度计陀螺仪主要测量角速度 xff0c 即可以测出某一时间段物体转过的角度加速度计测量的是物体的加速度 xff0c 重力加速度即物体受重力作用的情况下具有的加速度 xff0c 物体静止时 xff0c 加速

飞控陀螺仪，磁力计，加速计，四元数姿态结算

飞控陀螺仪，磁力计，加速计，四元数姿态结算 的相关文章

随机推荐

热门标签

飞控陀螺仪，磁力计，加速计，四元数姿态结算的相关文章