这一段是网上比较流行的四元数解算程序
shuju=load('data.txt')
ax=shuju(:,1)/32768*16*9.8
ay=shuju(:,2)/32768*16*9.8
az=shuju(:,3)/32768*16*9.8
gx=shuju(:,4)/32768*2000
gy=shuju(:,5)/32768*2000
gz=shuju(:,6)/32768*2000
n=length(ax)
ax=ax'
ay=ay'
az=az'
gx=gx'
gy=gy'
gz=gz'
q0=1
q1=0
q2=0
q3=0
exInt=0
eyInt=0
ezInt=0
Kp=100
Ki=0.002
halfT=0.00045
t=1:1:n
for i=1:n
norm = sqrt(ax(i)*ax(i) + ay(i)*ay(i) + az(i)*az(i));
ax(i) = ax(i) / norm;
ay(i) = ay(i) / norm;
az (i)= az(i) / norm;
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
ex = (ay(i)*vz - az(i)*vy);
ey = (az(i)*vx - ax(i)*vz);
ez = (ax(i)*vy - ay(i)*vx);
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
gx(i) = gx (i)+ Kp*ex + exInt;
gy(i) = gy(i) + Kp*ey + eyInt;
gz(i) = gz(i) + Kp*ez + ezInt;
q0 = q0 + (-q1*gx(i) - q2*gy(i) - q3*gz(i))*halfT;
q1 = q1 + (q0*gx(i) + q2*gz(i) - q3*gy(i))*halfT;
q2 = q2 + (q0*gy(i) - q1*gz(i) + q3*gx(i))*halfT;
q3 = q3 + (q0*gz(i) + q1*gy(i) - q2*gx(i))*halfT;
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
Pitch (i) =( asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3);
Roll(i) = (atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3);
Yaw (i)=( atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3);
end
但是静止状态下，加速度会提供当前的pitch与roll角，而角速度为0，因此解算出的pitch与roll角均为0°，因此对于任意初始姿态，在静止一段时间后，解算出的姿态角均会发生极大偏移，有没有大神能够指点一下如何解决这个问题