mimu.m

自己仿真的四元数法的龙格库塔的姿态解算matlab仿真

N=250;
t=0.02;

%陀螺测量值
p=[zeros(1,50),pi/4*ones(1,100),zeros(1,100)]; %偏航角速率
r=zeros(1,N); %横滚角速率
q=zeros(1,N); %俯仰角速率


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

E=zeros(4,N);
E(1,1)=cos(Psi(1,1)/2)*cos(Theta(1,1)/2)*cos(Phi(1,1)/2)+sin(Psi(1,1)/2)*sin(Theta(1,1)/2)*sin(Phi(1,1)/2);
E(2,1)=cos(Psi(1,1)/2)*cos(Theta(1,1)/2)*sin(Phi(1,1)/2)-sin(Psi(1,1)/2)*sin(Theta(1,1)/2)*cos(Phi(1,1)/2);
E(3,1)=cos(Psi(1,1)/2)*sin(Theta(1,1)/2)*cos(Phi(1,1)/2)+sin(Psi(1,1)/2)*cos(Theta(1,1)/2)*sin(Phi(1,1)/2);
E(4,1)=-cos(Psi(1,1)/2)*sin(Theta(1,1)/2)*sin(Phi(1,1)/2)+sin(Psi(1,1)/2)*cos(Theta(1,1)/2)*cos(Phi(1,1)/2);

%更新
for k=2:N
   %龙哥库塔法四元数更新
    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);
    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];
    k2=omega*(E(:,k-1)+k1*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));
    %捷联阵更新
    T=[E(1,k)^2+E(2,k)^2-E(3,k)^2-E(4,k)^2,2*E(1,k)*E(4,k)+2*E(2,k)*E(3,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(1,k)*E(2,k)+2*E(3,k)*E(4,k);
       2*E(1,k)*E(3,k)+2*E(2,k)*E(4,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];
    %航向姿态更新
    if (T(3,3)>0)
       Phi(k)=atan(T(2,3)/T(3,3));
    elseif (T(3,3)<0) 
            Phi(k)=pi*sign(T(2,3))+atan(T(2,3)/T(3,3));
    end
    Theta(k)=asin(-T(1,3));
    if (T(1,1)>0)
          Psi(k)=atan(T(1,2)/T(1,1));
    elseif (T(1,1)<0)  
          Psi(k)=pi*sign(T(1,2))+atan(T(1,2)/T(1,1)); 
    end
end

%画图
subplot(1,3,1);
plot(Phi);
title('偏航角')
grid on;
subplot(1,3,2);
plot(Theta);
title('俯仰角')
grid on;
subplot(1,3,3);
plot(Psi);
title('横滚角')
grid on;