四轴上MPU6050输出的 角度
////////////////////////////////////////////////////////////////////////////////#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
四轴板 是静止的
怎么输出的数据一直在抖动。。。
到后来输出的数据变成了
42.400253,-4.191398, 0.000000
42.429253,-4.133758, 0.000000
42.429253,-4.133758, 0.000000
42.429253,-4.133758, 0.000000
42.429253,-4.133758, 0.000000
42.457767,-4.057551, 0.000000
42.484085,-3.912763, 0.000000
42.512112,-3.820593, 0.000000
42.540497,-3.745285, 0.000000
42.569031,-3.704885, 0.000000
42.597496,-3.668670, 0.000000
42.625999,-3.635552, 0.000000
42.654297,-3.602604, 0.000000
42.682438,-3.556211, 0.000000
42.710510,-3.500008, 0.000000
42.710510,-3.500008, 0.000000
42.710510,-3.500008, 0.000000
42.710510,-3.500008, 0.000000
42.710510,-3.500008, 0.000000
42.710510,-3.500008, 0.000000
四元数不对。感觉有滞后。 初始化对准的时候没动吧，或者你计算的时候是不是有误，还有就是你换个板子试试，可能是传感器不大好使了 楼主你的咋样了 我的也是这个情况
页:
[1]