stance_dff.m

来自「采用四阶龙格库塔方法进行微分方程解算,用matlab编写的源代码,主要用于四元素」· M 代码 · 共 57 行

function fourelement_matrix=stance_dff(fourelement_matrix,w_ebx,w_ebx2,w_ebx3,w_eby,w_eby2,w_eby3,w_ebz,w_ebz2,w_ebz3,time)%函数定义行
%利用4阶龙格库塔法求解实时姿态阵
%采用四元素方法
%输入：前一时刻四元素和角速度 T/2时刻角速度 计算周期/2
%输出：当前时刻四元素值
%w_eb 姿态速率
P0=fourelement_matrix(1);
P1=fourelement_matrix(2);
P2=fourelement_matrix(3);
P3=fourelement_matrix(4);
K10=0.5*(-w_ebx*P1-w_eby*P2-w_ebz*P3);
K11=0.5*(w_ebx*P0+w_ebz*P2-w_eby*P3);
K12=0.5*(w_eby*P0-w_ebz*P1+w_ebx*P3);
K13=0.5*(w_ebz*P0+w_eby*P1-w_ebx*P2);
A0=P0+K10/2;
A1=P1+K11/2;
A2=P2+K12/2;
A3=P3+K13/2;
K20=0.5*(-w_ebx2*A1-w_eby2*A2-w_ebz2*A3);
K21=0.5*(w_ebx2*A0+w_ebz2*A2-w_eby2*A3);
K22=0.5*(w_eby2*A0-w_ebz2*A1+w_ebx2*A3);
K23=0.5*(w_ebz2*A0+w_eby2*A1-w_ebx2*A2);
B0=P0+K20/2;
B1=P1+K21/2;
B2=P2+K22/2;
B3=P3+K23/2;
K30=0.5*(-w_ebx2*B1-w_eby2*B2-w_ebz2*B3);
K31=0.5*(w_ebx2*B0+w_ebz2*B2-w_eby2*B3);
K32=0.5*(w_eby2*B0-w_ebz2*B1+w_ebx2*B3);
K33=0.5*(w_ebz2*B0+w_eby2*B1-w_ebx2*B2);
C0=P0+K30/2;
C1=P1+K31/2;
C2=P2+K32/2;
C3=P3+K33/2;
K40=0.5*(-w_ebx3*C1-w_eby3*C2-w_ebz3*C3);
K41=0.5*(w_ebx3*C0+w_ebz3*C2-w_eby3*C3);
K42=0.5*(w_eby3*C0-w_ebz3*C1+w_ebx3*C3);
K43=0.5*(w_ebz3*C0+w_eby3*C1-w_ebx3*C2);
P0=P0+time*(K10+2*K20+2*K30+K40)/6;
P1=P1+time*(K11+2*K21+2*K31+K41)/6;
P2=P2+time*(K12+2*K22+2*K32+K42)/6;
P3=P3+time*(K13+2*K23+2*K33+K43)/6;
%PPP=Sqrtm(P0*P0+P1*P1+P2*P2+P3*P3);%四元素进行正交化处理
PPP=1;
fourelement_matrix(1)=P0/PPP;
fourelement_matrix(2)=P1/PPP;
fourelement_matrix(3)=P2/PPP;
fourelement_matrix(4)=P3/PPP;