# 浅析IMU代码

 1 //=====================================================================================================
2 // IMU.c
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 //----------------------------------------------------------------------------------------------------
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
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
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);

可以参考的文章

http://blog.csdn.net/qq_21842557/article/details/50993809