MATLAB 轨迹模拟程序

function trajectory
global Re e g0 wie 
Re = 6378160;   e = 1/298.3;   wie = 7.2921151467e-5;   g0 = 9.7803267714;
th = 0.002;       %龙格-库塔步长
deg = pi/180; min = deg/60;

    att = pi/180*[0; 0; 0];	 %[pitch roll azimuth]  
	vb = [0; 0; 0];          %[0 vby 0]       
	pos = [34*deg+14.76289014*min; 108*deg+54.57983*min; 380];  %[latitude longitude height]

    %rk（1:3） att（pitch roll azimuth）;   rk（2:6） vn（vnE vnN vnU）;   rk（7:9） pos（lti lgi hgt）
    %rk（10:12） wm;  rk（13:15） vm;         
    rk = [att; Att2Mat（att）*vb; pos; zeros（61）];
    
    fid = fopen（‘e:/ygm/vehicle/trace2.bin‘‘wb‘）;  %4 good
    wat2 = getwat（0）;
    for k=2:2:1000*1000
        wat0 = wat2;        k1 = funrk（rk         wat0）;
        wat1 = getwat（k-1）; k2 = funrk（rk+th/2*k1 wat1）;
                            k3 = funrk（rk+th/2*k2 wat1）;
        wat2 = getwat（k）;   k4 = funrk（rk+  th*k3 wat2）;
        rk = rk + th/6*（k1+2*k2+2*k3+k4）;
        if mod（k10）==0                               %10 ms
            fwrite（fid [rk; k4（10:15）] ‘real*8‘）;   %保存数据 k4（10:15）: [wb fb]
            if mod（k1000）==0
                step=k/1000                          %进度显示
            end
        end
    end
    fclose（fid）;
    
function drk = funrk（rk wat）        
global Re e g0 wie 
    %通用变量计算
	si = sin（rk（1））; ci = cos（rk（1））; sj = sin（rk（2））; cj = cos（rk（2））; sk = sin（rk（3））; ck = cos（rk（3））;
	slti = sin（rk（7））; clti = cos（rk（7））; tlti = slti/clti; slti2 = slti^2; slti4 = slti2^2;
    RM = Re*（1-2*e+3*e*slti2）; RN = Re*（1+e*slti2）; RMh = RM + rk（9）; RNh = RN + rk（9）;
    wnie = wie * [0; clti; slti];	wnen = [-rk（5）/RMh; rk（4）/RNh; rk（4）/RNh*tlti];
    Cnb = [ cj*ck+si*sj*sk ci*sk  sj*ck-si*cj*sk;
           -cj*sk+si*sj*ck ci*ck -sj*sk-si*cj*ck;
           -ci*sj          si     ci*cj ];
	%姿态增量
	datt = wat（1:3）;
	%速度增量
	Cnt = [ ck ci*sk -si*sk; 
           -sk ci*ck -si*ck; 
            0  si     ci ];
	dvn = Cnt*wat（4:6）;
	%位置增量
    dpos = [rk（5）/RMh; rk（4）/（RNh*clti）; rk（6）];
	%角增量
    wnin = wnie + wnen;
	a2w = [ cj 0  sj*ci; 
            0  1 -si; 
            sj 0 -cj*ci ];
    wbnb = a2w*wat（1:3）;
	dwm = Cnb‘*wnin + wbnb;
	%比力增量
    g = g0*（1+5.27094e-3*slti2+2.32718e-5*slti4） - 3.086e-6*rk（9）;
	gn = [0; 0; -g];
  	dvm = Cnb‘ * （dvn+cross（2*wnie+wnenrk（4:6））-gn）;
    %rk增量
    drk = [datt; dvn; dpos; dwm; dvm];
    
function wat = getwat（k）          %轨迹设置
%姿态角变化率 wat（1:3） [wPitch wRoll wAzimuth]; 轨迹加速度 wat（4:6） [atx aty atz]
%阶段   动作      起始时间（s）     航向azimuth（deg）  俯仰pitch（deg）     速度（m/s）
%阶段   动作              持续时间（s）       倾斜roll（deg）   加速度（m/s^2）   
%       停止       0      100     0         0       0       0          0
        if k<=100*1000
            wat=[0; 0; 0;    0; 0; 0];
%       加速       100    20      0         0       0       1          0
        elseif k<=120*1000
            wat=[0; 0; 0;     0; 1; 0];
%       匀速       120    80      0         0       0