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???