chengxu2.m

这是我自己编写的一段基于MATLAB的轨迹仿真程序

clear all
close all

%初始化

Dr=2*pi/360;
B0=5*Dr;
B1=5*Dr;
B2=5*Dr;
ommiga0=2*pi/10;
ommiga1=2*pi/10;
ommiga2=2*pi/10;
ommiga3=2*pi/7200
Wie=360/(24*60*60)*Dr
i=0;
phi0=45*Dr;
lamda0=126*Dr;
h0=3000;
A0=3*Dr;
A1=5*Dr;
A2=2000;
Re=6371020;
g=9.8;
alpha=0;

%--------------------------------------------------------------------------

for t=0:0.5:120
    i=i+1;
    theta=B0*cos(ommiga0*t);
    gamma=B1*cos(ommiga1*t);
    psi=B2*cos(ommiga2*t);
    d_theta=-B0*ommiga0*sin(ommiga0*t);
    d_gamma=-B1*ommiga1*sin(ommiga1*t);
    d_psi=-B2*ommiga2*sin(ommiga2*t);
    W_tb_b=[cos(gamma) 0 -sin(gamma)*cos(theta);0 1 -sin(theta);sin(gamma) 0 cos(gamma)*cos(theta)]*[d_theta; d_gamma; d_psi];


    phi=phi0+A0*cos(ommiga3*t);
    lamda=lamda0+A1*cos(ommiga3*t);
    h=h0+A2*cos(ommiga3*t);
    d_phi=-A0*ommiga3*sin(ommiga3*t);
    d_lamda=-A1*ommiga3*sin(ommiga3*t);
    d_h=-A2*ommiga3*sin(ommiga3*t);
    d_d_phi=-A0*ommiga3^2*cos(ommiga3*t);
    d_d_lamda=-A1*ommiga3^2*cos(ommiga3*t);
    d_d_h=-A2*ommiga3^2*cos(ommiga3*t);

    x=(Re+h)*cos(phi)*cos(lamda);
    d_x=d_h*cos(phi)*cos(lamda)-d_phi*(Re+h)*sin(phi)*cos(lamda)-d_lamda*(Re+h)*cos(phi)*sin(lamda);
    d_d_x1=d_d_h*cos(phi)*cos(lamda)-d_h*d_phi*sin(phi)*cos(lamda)-d_h*d_phi*cos(phi)*sin(lamda);
    d_d_x2=-d_d_phi*(Re+h)*sin(phi)*cos(lamda)-d_h*d_phi*sin(phi)*cos(lamda)-(Re+h)*(d_phi)^2*cos(phi)*cos(lamda)+(Re+h)*d_phi*d_lamda*sin(phi)*sin(lamda);
    d_d_x3=-d_d_lamda*(Re+h)*cos(phi)*sin(lamda)-d_h*d_lamda*cos(phi)*sin(lamda)+(Re+h)*d_phi*d_lamda*sin(phi)*sin(lamda)-(Re+h)*(d_lamda)^2*cos(phi)*cos(lamda);
    d_d_x=d_d_x1+d_d_x2+d_d_x3;

    y=(Re+h)*cos(phi)*sin(lamda);
    d_y=d_h*cos(phi)*sin(lamda)-d_phi*(Re+h)*sin(phi)*sin(lamda)+d_lamda*(Re+h)*cos(phi)*cos(lamda);
    d_d_y1=d_d_h*cos(phi)*sin(lamda)-d_h*d_phi*sin(phi)*sin(lamda)+d_h*d_phi*cos(phi)*cos(lamda);
    d_d_y2=-d_d_phi*(Re+h)*sin(phi)*sin(lamda)-d_h*d_phi*sin(phi)*sin(lamda)-(Re+h)*(d_phi)^2*cos(phi)*sin(lamda)-(Re+h)*d_phi*d_lamda*sin(phi)*cos(lamda);
    d_d_y3=d_d_lamda*(Re+h)*cos(phi)*cos(lamda)+d_h*d_lamda*cos(phi)*cos(lamda)-(Re+h)*d_phi*d_lamda*sin(phi)*cos(lamda)-(Re+h)*(d_lamda)^2*cos(phi)*sin(lamda);
    d_d_y=d_d_y1+d_d_y2+d_d_y3;

    z=(Re+h)*sin(phi);
    d_z=d_h*sin(phi)+d_phi*(Re+h)*cos(phi);
    d_d_z1=d_d_h*sin(phi)+d_h*d_phi*cos(phi);
    d_d_z2=d_d_phi*(Re+h)*cos(phi)+d_h*d_phi*cos(phi)-(d_phi)^2*(Re+h)*sin(phi);
    d_d_z=d_d_z1+d_d_z2;

    C_e_t=[-sin(lamda) cos(lamda) 0;-sin(phi)*cos(lamda) -sin(phi)*sin(lamda) cos(phi);cos(phi)*cos(lamda) cos(phi)*sin(lamda) sin(phi)];
    V_et_t=C_e_t*[d_x;d_y;d_z];
    v_etx_t=V_et_t(1);
    v_ety_t=V_et_t(2);
    v_etz_t=V_et_t(3);
    a_r_t=C_e_t*[d_d_x;d_d_y;d_d_z];
    a_t=a_r_t-[-(v_etx_t*v_ety_t)/(Re+h)*tan(phi)+(v_etx_t*v_etz_t)/(Re+h);(v_etx_t)^2/(Re+h)*tan(phi)+(v_ety_t*v_etz_t)/(Re+h);-(v_etx_t)^2/(Re+h)-(v_ety_t)^2/(Re+h)];

    W_ie_t=[0;Wie*cos(phi);Wie*sin(phi)];
    W_et_t=[-v_ety_t/(Re+h);v_etx_t/(Re+h);v_etx_t/(Re+h)*tan(phi)];
    W_it_t=W_ie_t+W_et_t;

    c00=cos(gamma)*cos(psi)-sin(gamma)*sin(theta)*sin(psi);
    c01=cos(gamma)*sin(psi)+sin(gamma)*sin(theta)*cos(psi);
    c02=-sin(gamma)*cos(theta);
    c10=-cos(theta)*sin(psi);
    c11=cos(gamma)*cos(psi);
    c12=sin(theta);
    c20=sin(gamma)*cos(psi)+cos(gamma)*sin(theta)*sin(psi);
    c21=sin(gamma)*sin(psi)-cos(gamma)*sin(theta)*cos(psi);
    c22=cos(gamma)*cos(theta);
    C_t_b=[c00 c01 c02;c10 c11 c12;c20 c21 c22];

    W_ib_b=C_t_b*W_it_t+W_tb_b;

    w_iex_t=W_ie_t(1);
    w_iey_t=W_ie_t(2);
    w_iez_t=W_ie_t(3);
    w_etx_t=W_et_t(1);
    w_ety_t=W_et_t(2);
    w_etz_t=W_et_t(3);
    f_t=a_t+[0 -(2*w_iez_t+w_etz_t) (2*w_iey_t+w_ety_t);(2*w_iez_t+w_etz_t) 0 -(2*w_iex_t+w_etx_t); -(2*w_iey_t+w_ety_t) (2*w_iex_t+w_etx_t) 0]*V_et_t+[0;0;g];
    f_b=C_t_b*f_t;
    gyro(:,i)=W_ib_b;
    Acce(:,i)=f_b;
end
figure(1)
subplot(311)
plot(gyro(1,:))
subplot(312)
plot(gyro(2,:))
subplot(313)
plot(gyro(3,:))

C_b_t=[-sin(alpha)*sin(phi)*cos(lamda)-cos(alpha)*sin(lamda) -sin(alpha)*sin(phi)*sin(lamda)+cos(alpha)*cos(lamda) sin(alpha)*cos(phi);
       -cos(alpha)*sin(phi)*cos(lamda)+sin(alpha)*sin(lamda) -cos(alpha)*sin(phi)*sin(lamda)-sin(alpha)*cos(lamda) cos(alpha)*cos(phi);
       cos(phi)*cos(lamda) cos(phi)*sin(lamda) sin(phi)];
psi_G=psi+alpha
T11=cos(gamma)*cos(psi_G)-sin(gamma)*sin(theta)*sin(psi_G);
T12=cos(gamma)*sin(psi_G)+sin(gamma)*sin(theta)*cos(psi_G);
T13=-sin(gamma)*cos(theta);
T21=-cos(theta)*sin(psi_G);
T22=cos(gamma)*cos(psi_G);
T23=sin(theta);
T31=sin(gamma)*cos(psi_G)+cos(gamma)*sin(theta)*sin(psi_G);
T32=sin(gamma)*sin(psi_G)-cos(gamma)*sin(theta)*cos(psi_G);
T33=cos(gamma)*cos(theta);
T=[T11 T12 T13;T21 T22 T23;T31 T32 T33];

q0^2+q1^2-q2^2-q3^2=T11;
q0^2-q1^2+q2^2-q3^2=T22;
q0^2-q1^2-q2^2+q3^2=T33;
q0^2+q1^2+q2^2+q3^2=1;
|q1|=1/2*(1+T11-T22-T33)^1/2;
|q2|=1/2*(1-T11+T22-T33)^1/2;
|q3|=1/2*(1-T11-T22+T33)^1/2;
|q0|=1/2*(1-T11-T22-T33)^1/2;
signq0=+;
signq1=sign(T32-T23);
signq2=sign(T13-T31);
signq3=sign(T21-T12);
 W_ie_p=[0;Wie*cos(phi);Wie*sin(phi)];
 W_ep_p=0;