qfunlgkt.m

四元数的隆戈库塔法

function Q=Qfunlgkt()
global M
global Q0
%for i=0:3
K1=zeros(4,1);%初始化系数K1K2K3K4
%end
K2=zeros(4,1);
K3=zeros(4,1);
K4=zeros(4,1);
h=0.001;
wx1=M(1,5);
wy1=M(1,7);
wz1=M(1,6);
wx2=M(1,5);
wy2=M(1,7);
wz2=M(1,6);
W0=[0 -wx1 -wy1 -wz1;wx1 0 wz1 -wy1;wy1 -wz1 0 wx1;wz1 wy1 -wx1 0];
Wb=[0 -0.5*(wx1+wx1) -0.5*(wy1+wy1) -0.5*(wz1+wz1);0.5*(wx1+wx1) 0 0.5*(wz1+wz1) -0.5*(wy1+wy1);0.5*(wy1+wy1) -0.5*(wz1+wz1) 0 0.5*(wx1+wx1);0.5*(wz1+wz1) 0.5*(wy1+wy1) -0.5*(wx1+wx1) 0];
W1=[0 -wx2 -wy2 -wz2;wx2 0 wz2 -wy2;wy2 -wz2 0 wx2;wz2 wy2 -wx2 0];
K1=0.5*h*W0*Q0;
K2=0.5*h*Wb*(Q0+K1);
K3=0.5*h*Wb*(Q0+K2);
K4=0.5*h*W1*(Q0+K3);
Q=Q0+(K1+2*K2+2*K3+K4)/6;