MiniFly微型四轴飞行器差值ex,ey,ez的计算

Lyanzh

请问各位大神，MiniFly微型四轴飞行器里的差值ex,ey,ez为什么是这样计算
        /*加速计读取的方向与重力加速计方向的差值，用向量叉乘计算*/
        ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
        ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
        ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);

源代码如下

1. void imuUpdate(Axis3f acc, Axis3f gyro, state_t *state , float dt)        /*数据融合 互补滤波*/
   
2. {
   
3.         float normalise;
   
4.         float ex, ey, ez;
   
5.         float halfT = 0.5f * dt;
   
6.         float accBuf[3] = {0.f};
   
7.         Axis3f tempacc = acc;
   
8.        
   
9.         gyro.x = gyro.x * DEG2RAD;        /* 度转弧度 */
   
10.         gyro.y = gyro.y * DEG2RAD;
    
11.         gyro.z = gyro.z * DEG2RAD;
    
12. 
    
13.         /* 加速度计输出有效时,利用加速度计补偿陀螺仪*/
    
14.         if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
    
15.         {
    
16.                 /*单位化加速计测量值*/
    
17.                 normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
    
18.                 acc.x *= normalise;
    
19.                 acc.y *= normalise;
    
20.                 acc.z *= normalise;
    
21. 
    
22.                 /*加速计读取的方向与重力加速计方向的差值，用向量叉乘计算*/
    
23.                 ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
    
24.                 ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
    
25.                 ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);
    
26.                
    
27.                 /*误差累计，与积分常数相乘*/
    
28.                 exInt += Ki * ex * dt;
    
29.                 eyInt += Ki * ey * dt;
    
30.                 ezInt += Ki * ez * dt;
    
31.                
    
32.                 /*用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量*/
    
33.                 gyro.x += Kp * ex + exInt;
    
34.                 gyro.y += Kp * ey + eyInt;
    
35.                 gyro.z += Kp * ez + ezInt;
    
36.         }
    
37.         /* 一阶近似算法，四元数运动学方程的离散化形式和积分 */
    
38.         float q0Last = q0;
    
39.         float q1Last = q1;
    
40.         float q2Last = q2;
    
41.         float q3Last = q3;
    
42.         q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
    
43.         q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
    
44.         q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
    
45.         q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;
    
46.        
    
47.         /*单位化四元数*/
    
48.         normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    
49.         q0 *= normalise;
    
50.         q1 *= normalise;
    
51.         q2 *= normalise;
    
52.         q3 *= normalise;
    
53.        
    
54.         imuComputeRotationMatrix();        /*计算旋转矩阵*/
    
55.        
    
56.         /*计算roll pitch yaw 欧拉角*/
    
57.         state->attitude.pitch = -asinf(rMat[2][0]) * RAD2DEG;
    
58.         state->attitude.roll = atan2f(rMat[2][1], rMat[2][2]) * RAD2DEG;
    
59.         state->attitude.yaw = atan2f(rMat[1][0], rMat[0][0]) * RAD2DEG;
    
60.        
    
61.         if (!isGravityCalibrated)        /*未校准*/
    
62.         {               
    
63. //                accBuf[0] = tempacc.x* rMat[0][0] + tempacc.y * rMat[0][1] + tempacc.z * rMat[0][2];        /*accx*/
    
64. //                accBuf[1] = tempacc.x* rMat[1][0] + tempacc.y * rMat[1][1] + tempacc.z * rMat[1][2];        /*accy*/
    
65.                 accBuf[2] = tempacc.x* rMat[2][0] + tempacc.y * rMat[2][1] + tempacc.z * rMat[2][2];        /*accz*/
    
66.                 calBaseAcc(accBuf);                /*计算静态加速度*/                               
    
67.         }
    
68. }

复制代码

lycreturn

请看这个https://blog.csdn.net/Leyvi_Hsing/article/details/54293690

Lyanzh

lycreturn 发表于 2019-12-31 10:29
请看这个https://blog.csdn.net/Leyvi_Hsing/article/details/54293690

太感谢了！