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
