又得麻烦坛友了，matlab 四元数更新姿态程序问题。

s15200380596

程序目的是输入三轴角速度，输出三轴角度。捷联阵和欧拉角转换参考秦永元老师的惯性导航。
但输出的角度与实际各轴角度有很大差别（各轴实际角度已知，LZ想用matlab实现得到角度值）
不知道各位看了有什么意见，谢谢指点。

N=10;%数据个数。
t=0.02;%时间步长
%陀螺测量值

r=[-22.0337       -20.1416        -17.6392        -15.2588           -13.4888        -     12.6953        -12.5732        -11.7798        -10.1318        -7.6294        ]; %偏航角速率 wz
q=[-21.4233         -22.1558        -21.4844        -19.043            -16.1743        -14.0381        -12.3291        -10.376            -7.9346            -4.2114        ]; %横滚角速率 wy
p=[ 18.6157         16.9678     14.8926        13.0005            11.5967            10.1318           9.1553            8.9722            9.5825             11.4136]; %俯仰角速率 wx

%初始化
Phi=zeros(1,N);
Theta=zeros(1,N);
Psi=zeros(1,N);
E=zeros(4,N);

%四元数初值
E(1,1)=1;
E(2,1)=0;
E(3,1)=0;
E(4,1)=0;
%更新
for k=2:N
   %龙哥库塔法四元数更新
   %                  W矩阵为[ 0  -p  -q  -r
   %                               p  0   r   -q
    %                              q  -r  0    p
    %                              r  q  -p    0]
    omega=1/2*[0,-p(k-1),-q(k-1),-r(k-1);p(k-1),0,r(k-1),-q(k-1);q(k-1),-r(k-1),0,p(k-1);r(k-1),q(k-1),-p(k-1),0];
    k1=omega*E(:,k-1);%k1=1/2*W*Q
    omega=1/4*[0,-p(k)-p(k-1),-q(k)-q(k-1),-r(k)-r(k-1);
                       p(k)+p(k-1),0,r(k)+r(k-1),-q(k)-q(k-1);
                       q(k)+q(k-1),-r(k)-r(k-1),0,p(k)+p(k-1);
                      r(k)+r(k-1),q(k)+q(k-1),-p(k)-p(k-1),0];  %取t时刻和t+T时刻的角速度平均值当做t+T/2时刻的角速度
           
    k2=omega*(E(:,k-1)+k1*t/2);%k2=1/2(q(t)+k1*t/2)*W(t+T/2)
    k3=omega*(E(:,k-1)+k2*t/2);
    omega=1/2*[0,-p(k),-q(k),-r(k);p(k),0,r(k),-q(k);q(k),-r(k),0,p(k);r(k),q(k),-p(k),0];
    k4=omega*(E(:,k-1)+k3*t);
    E(:,k)=E(:,k-1)+t/6*(k1+2*k2+2*k3+k4);
   
    %归一化

    E(:,k)=E(:,k)/sqrt(E(:,k)'*E(:,k));
   
    %捷联阵更新
    %捷联矩阵为[ q0^2+q1^2-q2^2-q3^2     2(q1*q2-q0*q3)                2(q1*q2+q0*q2)
    %                2(q1*q2+q0*q3)                q0^2-q1^2+q2^2-q3^2     2(q2*q3-q0*q1)
    %                 2(q1*q3-q0*q2)                2(q2*q3+q0*q1)               q0^2-q1^2-q2^2+q3^2]

    T=[E(1,k)^2+E(2,k)^2-E(3,k)^2-E(4,k)^2,2*E(2,k)*E(3,k)-2*E(1,k)*E(4,k),2*E(2,k)*E(4,k)+2*E(1,k)*E(3,k);
       2*E(2,k)*E(3,k)+2*E(4,k)*E(1,k),E(1,k)^2-E(2,k)^2+E(3,k)^2-E(4,k)^2,2*E(3,k)*E(4,k)-2*E(1,k)*E(2,k);
       2*E(2,k)*E(4,k)-2*E(1,k)*E(3,k),2*E(3,k)*E(4,k)+2*E(1,k)*E(2,k),E(1,k)^2-E(2,k)^2-E(3,k)^2+E(4,k)^2];
   
    %航向姿态更新

   Phi(k)=((atan(T(1,2)/T(2,2))))*180/pi;
   Theta(k)=((asin ((T(3,2)))))*180/pi;
   Psi(k)=((atan(-T(3,1)/T(3,3))))*180/pi;
end