|
////////////////////////////////////////////////////////////////////////////////
#define Kp 1.6f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.001f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f // half the sample period采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
// float hx, hy, hz, bx, bz;
float vx, vy, vz;// wx, wy, wz;
float ex, ey, ez;
// 先把这些用得到的值算好
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
// float q0q3 = q0*q3;
float q1q1 = q1*q1;
// float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)
return;
norm = sqrt(ax*ax + ay*ay + az*az); //acc数据归一化
ax = ax /norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity and flux (v and w) 估计重力方向和流量/变迁
vx = 2*(q1q3 - q0q2); //四元素中xyz的表示
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
exInt = exInt + ex * Ki; //对误差进行积分
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt; //将误差PI后补偿到陀螺仪,即补偿零点漂移
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt; //这里的gz由于没有观测者进行矫正会产生漂移,表现出来的就是积分自增或自减
// integrate quaternion rate and normalise //四元素的微分方程
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
printf("%f, %f, %f\r\n",Q_ANGLE.X,Q_ANGLE.Y,Q_ANGLE.Z);
输出的数据为
-0.131456, -0.013724, 0.000000
-0.263374, -0.028662, 0.000000
-0.395509, -0.044734, 0.000000
-0.528047, -0.060314, 0.000000
-0.660972, -0.075726, 0.000000
-0.794265, -0.091475, 0.000000
-0.927926, -0.107601, 0.000000
-1.061961, -0.123908, 0.000000
-1.196487, -0.140551, 0.000000
-1.331391, -0.157320, 0.000000
-1.466669, -0.174278, 0.000000
-1.602207, -0.191161, 0.000000
-1.738127, -0.208080, 0.000000
-1.874558, -0.224975, 0.000000
-2.011240, -0.242115, 0.000000
-2.148315, -0.259065, 0.000000
-2.285766, -0.276227, 0.000000
-2.423730, -0.293104, 0.000000
-2.562085, -0.309811, 0.000000
-2.700833, -0.326357, 0.000000
-2.839844, -0.342915, 0.000000
-2.979239, -0.359487, 0.000000
-3.119020, -0.376079, 0.000000
-3.259299, -0.393030, 0.000000
-3.399840, -0.409916, 0.000000
-3.540766, -0.426851, 0.000000
-3.682204, -0.443545, 0.000000
-3.823915, -0.460309, 0.000000
-3.966011, -0.476997, 0.000000
-4.108491, -0.493864, 0.000000
-4.251471, -0.510862, 0.000000
-4.394836, -0.527907, 0.000000
-4.538482, -0.544601, 0.000000
-4.682513, -0.561326, 0.000000
-4.827062, -0.577863, 0.000000
-4.971999, -0.594431, 0.000000
-5.117208, -0.610921, 0.000000
-5.262806, -0.627414, 0.000000
-5.408802, -0.643603, 0.000000
-5.555200, -0.659305, 0.000000
-5.702114, -0.674830, 0.000000
-5.849300, -0.690081, 0.000000
-5.996873, -0.705357, 0.000000
-6.144962, -0.720588, 0.000000
-6.293439, -0.735855, 0.000000
-6.442304, -0.751280, 0.000000
-6.591542, -0.767137, 0.000000
-6.740909, -0.783393, 0.000000
-6.890782, -0.799854, 0.000000
-7.041033, -0.816521, 0.000000
-7.191552, -0.833227, 0.000000
-7.342448, -0.850152, 0.000000
-7.493840, -0.867352, 0.000000
-7.645613, -0.884757, 0.000000
-7.797788, -0.901928, 0.000000
-7.950356, -0.918933, 0.000000
-8.103330, -0.935335, 0.000000
-8.256686, -0.951912, 0.000000
-8.410430, -0.968526, 0.000000
-8.564669, -0.985521, 0.000000
-8.719168, -1.002713, 0.000000
-8.874043, -1.020121, 0.000000
-9.029313, -1.037524, 0.000000
-9.184958, -1.055056, 0.000000
-9.340851, -1.073004, 0.000000
-9.497120, -1.091153, 0.000000
-9.653870, -1.109905, 0.000000
-9.811120, -1.128854, 0.000000
-9.968749, -1.148038, 0.000000
-10.126653, -1.166900, 0.000000
-10.285078, -1.185695, 0.000000
-10.443783, -1.203994, 0.000000
-10.602891, -1.221902, 0.000000
-10.762382, -1.239988, 0.000000
-10.922261, -1.257889, 0.000000
-11.082538, -1.275349, 0.000000
-11.243311, -1.293179, 0.000000
-11.404457, -1.311236, 0.000000
-11.565885, -1.328887, 0.000000
-11.727691, -1.346567, 0.000000
-11.890002, -1.364255, 0.000000
-12.052687, -1.382306, 0.000000
-12.215621, -1.400537, 0.000000
-12.378935, -1.418947, 0.000000
-12.542743, -1.437448, 0.000000
-12.706937, -1.455761, 0.000000
-12.871393, -1.474093, 0.000000
-13.036235, -1.492250, 0.000000
-13.201580, -1.510572, 0.000000
-13.367166, -1.528921, 0.000000
-13.533119, -1.547479, 0.000000
-13.699576, -1.566292, 0.000000
-13.866415, -1.585136, 0.000000
-14.033632, -1.603699, 0.000000
-14.201235, -1.622084, 0.000000
-14.369083, -1.640558, 0.000000
-14.537318, -1.658942, 0.000000
-14.705926, -1.677340, 0.000000
-14.874914, -1.695839, 0.000000
-15.044269, -1.714529, 0.000000
-15.214259, -1.732557, 0.000000
-15.382453, -1.744894, 0.000000
-15.551012, -1.757362, 0.000000
-15.719921, -1.770307, 0.000000
-15.889315, -1.783380, 0.000000
-16.059069, -1.796286, 0.000000
-16.229050, -1.809687, 0.000000
-16.399403, -1.823100, 0.000000
-16.570255, -1.836018, 0.000000
-16.741354, -1.848875, 0.000000
-16.912666, -1.864525, 0.000000
-16.790648, -1.968822, 0.000000
-16.790648, -1.968822, 0.000000
-16.790648, -1.968822, 0.000000
-16.790648, -1.968822, 0.000000
-16.790648, -1.968822, 0.000000
-16.668701, -1.861339, 0.000000
-16.523148, -1.809449, 0.000000
-16.372728, -1.785945, 0.000000
-16.372728, -1.785945, 0.000000
-16.372728, -1.785945, 0.000000
-16.222145, -1.802332, 0.000000
-16.071690, -1.818691, 0.000000
-16.071690, -1.818691, 0.000000
-16.071690, -1.818691, 0.000000
-16.071690, -1.818691, 0.000000
-15.924963, -1.858685, 0.000000
-15.924963, -1.858685, 0.000000
-15.924963, -1.858685, 0.000000
-15.803412, -1.751316, 0.000000
-15.681940, -1.644003, 0.000000
-15.546573, -1.564199, 0.000000
-15.405076, -1.501269, 0.000000
-15.269928, -1.421320, 0.000000
-15.123805, -1.377599, 0.000000
-14.977807, -1.333822, 0.000000
-14.856876, -1.226601, 0.000000
-14.736267, -1.119356, 0.000000
-14.636189, -0.984820, 0.000000
-14.553481, -0.834324, 0.000000
-14.553481, -0.834324, 0.000000
-14.553481, -0.834324, 0.000000
-14.553481, -0.834324, 0.000000
-14.553481, -0.834324, 0.000000
-14.418907, -0.754986, 0.000000
-14.418907, -0.754986, 0.000000
-14.273572, -0.796040, 0.000000
-14.126692, -0.826647, 0.000000
-13.979832, -0.857455, 0.000000
-13.839702, -0.917498, 0.000000
-13.693236, -0.948085, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.548707, -0.988774, 0.000000
-13.646944, -1.139607, 0.000000
-13.782932, -1.247371, 0.000000
-13.919066, -1.355248, 0.000000
-13.919066, -1.355248, 0.000000
-13.919066, -1.355248, 0.000000
-13.919066, -1.355248, 0.000000
-13.919066, -1.355248, 0.000000
-13.919066, -1.355248, 0.000000
-13.919066, -1.355248, 0.000000
-13.919066, -1.355248, 0.000000
-13.963211, -1.534729, 0.000000
-13.963211, -1.534729, 0.000000
-13.963211, -1.534729, 0.000000
-13.963211, -1.534729, 0.000000
-13.880636, -1.683513, 0.000000
-13.751777, -1.772001, 0.000000
-13.631468, -1.876146, 0.000000
-13.486889, -1.915820, 0.000000
-13.339078, -1.931679, 0.000000
-13.339078, -1.931679, 0.000000
-13.191006, -1.916288, 0.000000
-13.044243, -1.888035, 0.000000
-12.900252, -1.843465, 0.000000
-12.759537, -1.786226, 0.000000
-12.613331, -1.755558, 0.000000
-12.467400, -1.724816, 0.000000
-12.321623, -1.694096, 0.000000
-12.176126, -1.663251, 0.000000
-12.031150, -1.629101, 0.000000
-11.886330, -1.594977, 0.000000
-11.751529, -1.521382, 0.000000
-11.616866, -1.447850, 0.000000
-11.474373, -1.403774, 0.000000
-11.331912, -1.359736, 0.000000
-11.331912, -1.359736, 0.000000
-11.187034, -1.383453, 0.000000
-11.042317, -1.407110, 0.000000
-10.908354, -1.476759, 0.000000
-10.790461, -1.581099, 0.000000
-10.686151, -1.705517, 0.000000
-10.560135, -1.793744, 0.000000
-10.426623, -1.863064, 0.000000
-10.298854, -1.946742, 0.000000
-10.168016, -2.022523, 0.000000
-10.031547, -1.958758, 0.000000
-9.933875, -1.823682, 0.000000
-9.816784, -1.715889, 0.000000
-9.699917, -1.608078, 0.000000
-9.582911, -1.500229, 0.000000
-9.538289, -1.326900, 0.000000
-9.457185, -1.176025, 0.000000
-9.457185, -1.176025, 0.000000
-9.457185, -1.176025, 0.000000
-9.327258, -1.096145, 0.000000
-9.185512, -1.062459, 0.000000
-9.043253, -1.035163, 0.000000
-8.908201, -0.972409, 0.000000
-8.782106, -0.885030, 0.000000
-8.640348, -0.858038, 0.000000
-8.640348, -0.858038, 0.000000
-8.502763, -0.906509, 0.000000
-8.386841, -1.011151, 0.000000
-8.279217, -1.128586, 0.000000
-8.191384, -1.270194, 0.000000
-8.133173, -1.435577, 0.000000
-8.080112, -1.603920, 0.000000
-8.012108, -1.762483, 0.000000
-7.921564, -1.900785, 0.000000
-7.821742, -2.027921, 0.000000
-7.721860, -2.154962, 0.000000
-7.606580, -2.258433, 0.000000
-7.485633, -2.350675, 0.000000
-7.354560, -2.416133, 0.000000
-7.221360, -2.473731, 0.000000
-7.088193, -2.531242, 0.000000
-6.957567, -2.596191, 0.000000
-6.827087, -2.661180, 0.000000
-6.696638, -2.726179, 0.000000
-6.575797, -2.815817, 0.000000
-6.453925, -2.902483, 0.000000
-6.330185, -2.984572, 0.000000
-6.235015, -3.115091, 0.000000
-6.285958, -3.291287, 0.000000
-6.371961, -3.446283, 0.000000
-6.423087, -3.622976, 0.000000
-6.423087, -3.622976, 0.000000
-6.423087, -3.622976, 0.000000
-6.423087, -3.622976, 0.000000
-6.308908, -3.513189, 0.000000
-6.308908, -3.513189, 0.000000
-6.308908, -3.513189, 0.000000
-6.168969, -3.522132, 0.000000
-6.029190, -3.528571, 0.000000
-5.889594, -3.534991, 0.000000
-5.750250, -3.542552, 0.000000
-5.611075, -3.551486, 0.000000
-5.611075, -3.551486, 0.000000
-5.611075, -3.551486, 0.000000
-5.611075, -3.551486, 0.000000
-5.611075, -3.551486, 0.000000
-5.611075, -3.551486, 0.000000
-5.611075, -3.551486, 0.000000
-5.479783, -3.485410, 0.000000
-5.357207, -3.395059, 0.000000
-5.244268, -3.285445, 0.000000
-5.123840, -3.191123, 0.000000
-4.993218, -3.125310, 0.000000
-4.993218, -3.125310, 0.000000
-4.993218, -3.125310, 0.000000
-4.993218, -3.125310, 0.000000
-4.993218, -3.125310, 0.000000
-4.993218, -3.125310, 0.000000
-4.993218, -3.125310, 0.000000
-4.993218, -3.125310, 0.000000
-5.064902, -3.289432, 0.000000
-5.140250, -3.451183, 0.000000
-5.224064, -3.606504, 0.000000
-5.224064, -3.606504, 0.000000
-5.224064, -3.606504, 0.000000
-5.224064, -3.606504, 0.000000
-5.179358, -3.778473, 0.000000
-5.112180, -3.935903, 0.000000
-4.999271, -4.036973, 0.000000
-4.878872, -4.121905, 0.000000
-4.743280, -4.151910, 0.000000
-4.608828, -4.187781, 0.000000
-4.478706, -4.242733, 0.000000
-4.351857, -4.308254, 0.000000
-4.239819, -4.408760, 0.000000
-4.131975, -4.516673, 0.000000
-4.024858, -4.625352, 0.000000
-3.917834, -4.734130, 0.000000
-3.821880, -4.859290, 0.000000
-3.728286, -4.987709, 0.000000
-3.631336, -5.111443, 0.000000
-3.545804, -5.249485, 0.000000
-3.467370, -5.395036, 0.000000
-3.355982, -5.494143, 0.000000
-3.244697, -5.593101, 0.000000
-3.121534, -5.663051, 0.000000
-2.998394, -5.732994, 0.000000
-2.887569, -5.831328, 0.000000
-2.764698, -5.900850, 0.000000
-2.644974, -5.978319, 0.000000
-2.522390, -6.047566, 0.000000
-2.389664, -6.074150, 0.000000
-2.260262, -6.117627, 0.000000
-2.136923, -6.182515, 0.000000
-2.022280, -6.270388, 0.000000
-1.917562, -6.377777, 0.000000
-1.794387, -6.442184, 0.000000
-1.662647, -6.467952, 0.000000
-1.529127, -6.475651, 0.000000
-1.396199, -6.443541, 0.000000
-1.264479, -6.401166, 0.000000
-1.264479, -6.401166, 0.000000
-1.138465, -6.330494, 0.000000
-1.061303, -6.176907, 0.000000
-0.995642, -6.014696, 0.000000
-0.945628, -5.843599, 0.000000
-0.945628, -5.843599, 0.000000
-0.945628, -5.843599, 0.000000
-0.945628, -5.843599, 0.000000
-0.945628, -5.843599, 0.000000
-0.945628, -5.843599, 0.000000
-0.945628, -5.843599, 0.000000
-0.945628, -5.843599, 0.000000
-0.868788, -5.690161, 0.000000
-0.868788, -5.690161, 0.000000
-0.824420, -5.516807, 0.000000
-0.695473, -5.465818, 0.000000
-0.695473, -5.465818, 0.000000
-0.695473, -5.465818, 0.000000
-0.695473, -5.465818, 0.000000
-0.695473, -5.465818, 0.000000
-0.566735, -5.498673, 0.000000
-0.441962, -5.550678, 0.000000
-0.322621, -5.620169, 0.000000
-0.278182, -5.791044, 0.000000
-0.278182, -5.791044, 0.000000
-0.278182, -5.791044, 0.000000
-0.278182, -5.791044, 0.000000
-0.353115, -5.950642, 0.000000
-0.353115, -5.950642, 0.000000
-0.353115, -5.950642, 0.000000
-0.276626, -6.095060, 0.000000
-0.168938, -6.192416, 0.000000
-0.044726, -6.243210, 0.000000
0.079323, -6.293890, 0.000000
0.198331, -6.361974, 0.000000
0.322161, -6.412398, 0.000000
0.322161, -6.412398, 0.000000
0.322161, -6.412398, 0.000000
0.322161, -6.412398, 0.000000
0.449528, -6.443437, 0.000000
0.556516, -6.540131, 0.000000
0.632469, -6.683822, 0.000000
0.739159, -6.780124, 0.000000
0.815023, -6.923544, 0.000000
0.890842, -7.066806, 0.000000
0.959051, -7.217910, 0.000000
1.020718, -7.374834, 0.000000
1.044684, -7.555038, 0.000000
1.075655, -7.732170, 0.000000
1.106750, -7.909366, 0.000000
1.106750, -7.909366, 0.000000
1.133585, -8.088195, 0.000000
1.157501, -8.268241, 0.000000
1.157501, -8.268241, 0.000000
1.157501, -8.268241, 0.000000
1.157501, -8.268241, 0.000000
1.184634, -8.446960, 0.000000
1.252927, -8.596704, 0.000000
1.359325, -8.689850, 0.000000
1.471364, -8.769404, 0.000000
1.583414, -8.848911, 0.000000
1.695331, -8.928252, 0.000000
1.787901, -9.045938, 0.000000
1.888006, -9.149986, 0.000000
1.993851, -9.242202, 0.000000
2.093889, -9.345896, 0.000000
2.186151, -9.462889, 0.000000
2.230622, -9.631359, 0.000000
2.250571, -9.812746, 0.000000
2.250571, -9.812746, 0.000000
2.250571, -9.812746, 0.000000
2.236829, -10.000285, 0.000000
2.236829, -10.000285, 0.000000
2.236829, -10.000285, 0.000000
2.236829, -10.000285, 0.000000
2.219439, -10.187946, 0.000000
2.219439, -10.187946, 0.000000
2.219439, -10.187946, 0.000000
2.187331, -10.373839, 0.000000
2.116485, -10.540935, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.045312, -10.708261, 0.000000
2.151362, -10.798049, 0.000000
2.257305, -10.887650, 0.000000
2.351969, -10.998670, 0.000000
2.427439, -11.136984, 0.000000
2.464249, -11.309323, 0.000000
2.464249, -11.309323, 0.000000
2.464249, -11.309323, 0.000000
2.464249, -11.309323, 0.000000
2.412575, -11.489668, 0.000000
2.335406, -11.652706, 0.000000
2.234160, -11.785198, 0.000000
2.128287, -11.909670, 0.000000
2.032037, -12.051974, 0.000000
1.955137, -12.217128, 0.000000
1.883212, -12.386890, 0.000000
1.816899, -12.560976, 0.000000
1.749307, -12.734282, 0.000000
1.692291, -12.913945, 0.000000
1.679218, -13.102201, 0.000000
1.713584, -13.275128, 0.000000
1.774665, -13.427280, 0.000000
1.823161, -13.590077, 0.000000
1.868377, -13.755422, 0.000000
1.945109, -13.890328, 0.000000
2.045625, -13.988869, 0.000000
2.164231, -14.043316, 0.000000
2.278664, -14.110456, 0.000000
2.386050, -14.194314, 0.000000
2.493181, -14.277985, 0.000000
2.583428, -14.392410, 0.000000
2.640489, -14.546806, 0.000000
2.685616, -14.711119, 0.000000
2.761850, -14.844437, 0.000000
2.761850, -14.844437, 0.000000
2.761850, -14.844437, 0.000000
2.761850, -14.844437, 0.000000
2.818529, -14.677433, 0.000000
2.903432, -14.527827, 0.000000
2.979597, -14.371259, 0.000000
3.016975, -14.199098, 0.000000
3.128910, -14.087788, 0.000000
3.244303, -13.985609, 0.000000
3.363703, -13.896149, 0.000000
3.487816, -13.826507, 0.000000
四轴板 是静止的
怎么输出的数据一直在抖动。。。
|
阿莫论坛20周年了!感谢大家的支持与爱护!!
你熬了10碗粥,别人一桶水倒进去,淘走90碗,剩下10碗给你,你看似没亏,其实你那10碗已经没有之前的裹腹了,人家的一桶水换90碗,继续卖。说白了,通货膨胀就是,你的钱是挣来的,他的钱是印来的,掺和在一起,你的钱就贬值了。
|