function?Progen
%function?Progen?：profile?generator?飛行軌跡發生器
%根據不同運動階段軌跡參數（segment?parameters），生成相應軌跡數據，同時得到加計和陀螺的輸出信息（用于純慣性解算）
%坐標系定義：地理系取為（東北天）ENU；機體系取為右前上；子慣導坐標系取為右前上
%Parameters?
%???t_simulate??軌跡仿真時間
%???th??????????龍格庫塔積分步長
%???t_save??????軌跡數據保存步長
%Ref
%???1：Design?and?Analysis?of?Transfer?Alignment?Algorithm
%???2：嚴恭敏碩士論文
%???3：秦永元《慣性導航》
%Checked:2012/04/17
%==========================================================================
????clear
????close?all
????clc
????global??deg?minu????%deg角度?minu角分????????????????????????
????ConstsAndUnits;?????%基本的參數設置
????th?=?0.005;?????????%軌跡解算數據更新周期（RK4積分步長）（s）
????t_simulate?=?600;??%軌跡仿真時間（s）
????t_save=0.005;????????%軌跡數據保存周期（s）
%初值姿態、速度、位置設置??根據實際情況調整
????att?=?[0;?0;?0]*deg;????????????????????????????????????????????%[pitch（俯仰角）?roll（橫滾角）?azimuth（航向角）]??初始姿態角（rad）??
	vb?=??[0;?0;?0];????????????????????????????????????????????????%[0?vby?0]???????初始速度（m/s）?????????
	pos?=?[34*deg+14.76289014*minu;?108*deg+54.57983*minu;?380];	%[latitude?longitude?height]初始位置
????
????Length=t_simulate/t_save;???????????????????????????????
????profile_data?=?zeros（Length16）;??????????????????????%數據保存，每2.5ms保存一次??
????profile?=?[att;（Att2Mat（att））*vb;?pos;zeros（61）];????%15維列向量?姿態、速度、位置、陀螺角增量輸出、加計速度增量（此處的增量是相對零時刻的總增量）???Att2Mat作用：將姿態角轉化為姿態矩陣Cb2n?
????profile_data（1:）?=?[0;profile]‘;?????????????????????%將0和profile列向量中的15個元素賦值給profile_data的第一行
%==========================================================================
%4階龍格庫塔（RK4）軌跡解算
????fprintf（‘\r軌跡解算進度：%5.0f?s‘?0）;???????????%\r:回車將當前位置移到本行的開頭????%5.0f設置輸出的格式
????segparam2?=?getsegparam（0profile）;?????????????%獲取運動階段軌跡參數初值???初始時刻零O時間
????for?k=5:5:t_simulate*1000?????????????????????%RK4軌跡解算步長5ms????%for?循環變量=表達式1：表達式2：表達式3，表達式2為步長
????????segparam0?=?segparam2;?????????????k1?=?RK4（profile?segparam0）;
????????segparam1?=?getsegparam（k-2.5profile）;?????k2?=?RK4（profile+th/2*k1?segparam1）;
????????????????????????????????????????????????????k3?=?RK4（profile+th/2*k2?segparam1）;
????????segparam2?=?getsegparam（kprofile）;?????????k4?=?RK4（profile+??th*k3?segparam2）;
????????profile?=?profile?+?th/6*（k1+2*k2+2*k3+k4）;???????????%k1到K4都是變化率，乘以時間th/6就是變化量
????????if?mod（kt_save*1000）==0?????????????????????????????????%5ms保存一次數據?
???????????profile_data（k/（t_save*1000）:）=[k/1000;profile]‘;
???????????if?mod（k1000）==0
??????????????fprintf（‘\b\b\b\b\b\b\b%5.0f?s‘?k/1000）;???????????%解算進度顯示（s）?\b退格?1s顯示一次
???????????end
????????end
????end????
???????????????????????????????????????????????????????????????????%數據保存到Profile_data文件中?
????save?Profile_data??profile_data?????????????????????????????????%?save??文件名??變量
end
%@@@@@@@@@@@@@@@@@@@@@@@@@?軌跡參數解算?@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
function?dprofile?=?RK4（profilesegparam）
%function?RK4：4th-order?Runge–Kutta?method?軌跡微分方程解算
%INPUTS
%???profile（1:3）?姿態角[pitch?roll?azimuth]
%???profile（4:6）?速度地理系投影
%???profile（7:9）?位置[latitude?longitude?h