在网上看到的四元数姿态解算

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az/*, float mx, float my, float mz*/) {
unsigned long now,timeChange;
static unsigned long lastTime=0;
  now = micros();                                 //us
  timeChange =now - lastTime;
  if(timeChange>=SampleTime){
    lastTime = now;
  
  float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez,lastex,lastey,lastez;

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;         

norm = sqrt(ax*ax + ay*ay + az*az);      
ax = ax / norm;                                                           
ay = ay / norm;
az = az / norm;
/*norm = sqrt(mx*mx + my*my + mz*mz);                                      
mx = mx / norm;
my = my / norm;
mz = mz / norm;        

hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);  
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);        
bx = sqrt((hx*hx) + (hy*hy));                                             
bz = hz;      */  

vx = 2*(q1q3 - q0q2);                                                     
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;

/* wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);                     
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  */

ex = (ay*vz - az*vy) ;/*+ (my*wz - mz*wy);*/
ey = (az*vx - ax*vz);/* + (mz*wx - mx*wz);*/
ez = (ax*vy - ay*vx);/* + (mx*wy - my*wx);*/

exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;

gx = gx + Kp*ex + exInt+exD;
gy = gy + Kp*ey + eyInt+eyD;
gz = gz + Kp*ez + ezInt+ezD;

exD=Kd * (ex - lastex)*1000/timeChange;
eyD=Kd * (ey - lastey)*1000/timeChange;
ezD=Kd * (ez - lastez)*1000/timeChange;


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;  

norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);   
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;

lastex = ex;
  lastey = ey;
   lastez = ez;

yaw= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;
pitch= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
  }
}

最后的角度计算是自己为了测试加上的，我用的是集成的10 DOF IMU,得到的姿态只有在静止时比较准确和稳定，在动态时有很大波动，没有规律，请问是需要滤波吗？