姿态解算的一点疑问

603133791

1.         //Attitude estimation
   
2.         AttitudeFilter(&gyro, &acc, &acc_filter, MixConstant);
   
3.         AttitudeFilter(&gyro, &mag, &mag_filter, 0.950f);//那为什么数字罗盘测出来的数据也要这么干？
   
4.        
   
5.         RPY = CalculateRollPitchYaw(&acc_filter, &mag_filter); //acc_filter较为准确的重力加速度向量

复制代码

1. //MPU6050->gyrox-->pitch
   
2. //                   gyroy-->roll
   
3. //         gyroz-->yaw
   
4. inline void AttitudeFilter(vector_f *gyro, vector_i *acc, vector_i *acc_filter, float mixConstant)
   
5. {//complementation filter
   
6.         //calculate acc vector based on last value and gyroscope value
   
7.         vector_f acc_gyro;
   
8.         vector_f delta_theta;
   
9.         int32_t acceleration;
   
10.         float mixConstant1;
    
11.         acc_gyro.x = acc_filter->x;
    
12.         acc_gyro.y = acc_filter->y;
    
13.         acc_gyro.z = acc_filter->z;
    
14.         delta_theta.y = gyro->x * dt;
    
15.         delta_theta.x = gyro->y * dt;
    
16.         delta_theta.z = gyro->z * dt;//陀螺仪的值乘以dt，这个在方向余弦旋转变换矩阵中有出现过。
    
17.         rotateV(&acc_gyro, &delta_theta);
    
18.        
    
19. //        acceleration = acc->x*acc->x + acc->y*acc->y + acc->z*acc->z;
    
20.        
    
21.         //if acceleration<8m/s2 or acceleration>12m/s2, don't use acceleration information
    
22. //        if(acceleration<64000000 || acceleration>144000000)
    
23. //                mixConstant = 1.0f;
    
24.        
    
25.         //Complementary filter
    
26.         mixConstant1 = 1.0f - mixConstant;
    
27.         acc_filter->x = acc_gyro.x * mixConstant + acc->x * mixConstant1;//acc_filter的值是陀螺仪和上一次姿态估算出的值。而acc就是此次加速度计的值，在这两个值作一个加权。只要时间足够上的话，最后的值还是等于加速度计的值，也就是准确的值。
    
28.         acc_filter->y = acc_gyro.y * mixConstant + acc->y * mixConstant1;
    
29.         acc_filter->z = acc_gyro.z * mixConstant + acc->z * mixConstant1;
    
30.         //此时acc_gyro的值就是经过了旋转矩阵处理之后的值。是通过上次的估计出这次的数据，灵气来源是陀螺仪。而后和加速度计的值来一个加权平均。
    
31. }
    
32. 
    
33. 
    
34. //Calculate roll/pitch/yaw
    
35. //Roll:ret.x        -180~180 deg
    
36. //Pitch:ret.y        -90~90 deg
    
37. //yaw:ret.z                -180~180 deg
    
38. inline vector_f CalculateRollPitchYaw(vector_i *acc, vector_i *mag)
    
39. {
    
40.         vector_f temp;
    
41. 
    
42.         temp.x = atan2f(acc->x, acc->z);
    
43.         temp.y = asinf(acc->y/sqrtf(acc->x*acc->x + acc->y*acc->y + acc->z*acc->z));
    
44.         temp.z = -atan2f(mag->y*cos(temp.y) - mag->z*sin(temp.y),
    
45.                                         -mag->y*sin(temp.x)*sin(temp.y) + mag->x*cos(temp.x) - mag->z*sin(temp.x)*cos(temp.y)) * 57.30f;
    
46.         temp.x *= 57.30f;
    
47.         temp.y *= 57.30f;
    
48.         return temp;
    
49. }

复制代码

通过陀螺仪测量得到的数据作为旋转矩阵来估计重力矢量，再和加速度计测得的重力矢量进行一下加权，得到最后的重力加速度的矢量值。

这是我自己理解的，但是我不懂得为什么电子罗盘得到的数值要和陀螺仪的数据进行融合。而且最后求出姿态角（个人认为是欧拉角），那个解算的过程是怎样的，求解的过程是怎样的，我不是很理解啊。

老衲人称秃驴

因为陀螺仪积分会发散，需要加速计校正，个人理解

603133791

但就是不知道具体的过程为什么是这样的，比如公式为什么是这样的。这好像是参考MWC的。