IMU的代码的引自https://storage.googleapis.com/google-code-archive-downloads/v2/code.google.com/imumargalgorithm30042010sohm/IMU.zip
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;
参考https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
引用AN5022 Quaternion Algebra.pdf
或者
出自《整合 GNSS 與 INS 量測資訊的地平面運動軌跡估測》鄭 期 元的论文
下面把四元数换算成方向余弦中的第三行的三个元素。刚好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
http://www.anobbs.com/forum.php?mod=viewthread&tid=1185
