1 //=====================================================================================================
 2 // IMU.c
 3 // S.O.H. Madgwick
 4 // 25th September 2010
 5 //=====================================================================================================
 6 // Description:
 7 //
 8 // Quaternion implementation of the ‘DCM filter‘ [Mayhony et al].
 9 //
10 // User must define ‘halfT‘ as the (sample period / 2), and the filter gains ‘Kp‘ and ‘Ki‘.
11 //
12 // Global variables ‘q0‘, ‘q1‘, ‘q2‘, ‘q3‘ are the quaternion elements representing the estimated
13 // orientation.  See my report for an overview of the use of quaternions in this application.
14 //
15 // User must call ‘IMUupdate()‘ every sample period and parse calibrated gyroscope (‘gx‘, ‘gy‘, ‘gz‘)
16 // and accelerometer (‘ax‘, ‘ay‘, ‘ay‘) data.  Gyroscope units are radians/second, accelerometer 
17 // units are irrelevant as the vector is normalised.
18 //
19 //=====================================================================================================
20 
21 //----------------------------------------------------------------------------------------------------
22 // Header files
23 
24 #include "IMU.h"
25 #include <math.h>
26 
27 //----------------------------------------------------------------------------------------------------
28 // Definitions
29 
30 #define Kp 2.0f            // proportional gain governs rate of convergence to accelerometer/magnetometer
31 #define Ki 0.005f        // integral gain governs rate of convergence of gyroscope biases
32 #define halfT 0.5f        // half the sample period
33 
34 //---------------------------------------------------------------------------------------------------
35 // Variable definitions
36 
37 float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
38 float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
39 
40 //====================================================================================================
41 // Function
42 //====================================================================================================
43 
44 void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
45     float norm;
46     float vx, vy, vz;
47     float ex, ey, ez;         
48     
49     // normalise the measurements
50     norm = sqrt(ax*ax + ay*ay + az*az);       
51     ax = ax / norm;
52     ay = ay / norm;
53     az = az / norm;      
54     
55     // estimated direction of gravity
56     vx = 2*(q1*q3 - q0*q2);
57     vy = 2*(q0*q1 + q2*q3);
58     vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
59     
60     // error is sum of cross product between reference direction of field and direction measured by sensor
61     ex = (ay*vz - az*vy);
62     ey = (az*vx - ax*vz);
63     ez = (ax*vy - ay*vx);
64     
65     // integral error scaled integral gain
66     exInt = exInt + ex*Ki;
67     eyInt = eyInt + ey*Ki;
68     ezInt = ezInt + ez*Ki;
69     
70     // adjusted gyroscope measurements
71     gx = gx + Kp*ex + exInt;
72     gy = gy + Kp*ey + eyInt;
73     gz = gz + Kp*ez + ezInt;
74     
75     // integrate quaternion rate and normalise
76     q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
77     q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
78     q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
79     q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
80     
81     // normalise quaternion
82     norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
83     q0 = q0 / norm;
84     q1 = q1 / norm;
85     q2 = q2 / norm;
86     q3 = q3 / norm;
87 }
88 
89 //====================================================================================================
90 // END OF CODE
91 //====================================================================================================

 1 //=====================================================================================================
 2 // IMU.h
 3 // S.O.H. Madgwick
 4 // 25th September 2010
 5 //=====================================================================================================
 6 //
 7 // See IMU.c file for description.
 8 // 
 9 //=====================================================================================================
10 #ifndef IMU_h
11 #define IMU_h
12 
13 //----------------------------------------------------------------------------------------------------
14 // Variable declaration
15 
16 extern float q0, q1, q2, q3;    // quaternion elements representing the estimated orientation
17 
18 //---------------------------------------------------------------------------------------------------
19 // Function declaration
20 
21 void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az);
22 
23 #endif
24 //=====================================================================================================
25 // End of file
26 //=====================================================================================================

将加速度的原始数据，归一化，得到单位加速度  
 // normalise the measurements
   norm = sqrt(ax*ax + ay*ay + az*az);       
   ax = ax / norm;
   ay = ay / norm;
   az = az / norm; 

下面把四元数换算成方向余弦中的第三行的三个元素。刚好vx,vy,vz 其实就是上一次的欧拉角（四元数）的机体坐标参考系换算出来的重力的单位向量【没弄懂】。

    // estimated direction of gravity
    vx = 2*(q1*q3 - q0*q2);
    vy = 2*(q0*q1 + q2*q3);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

ax、ay、az是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。
ax、ay、az是测量得到的重力向量，
 vx、vy、vz是陀螺积分后的姿态来推算出的重力向量【没明白】，它们都是机体坐标参照系上的重力向量。 
那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。 
向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。
 这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。
（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
// error is sum of cross product between reference direction of field and direction measured by sensor
    ex = (ay*vz - az*vy);
    ey = (az*vx - ax*vz);
    ez = (ax*vy - ay*vx);