function?DR
fearth=1/298.257;?%定值：橢圓度
Re=6378137.0000;%定值：橢圓半長軸
navdt=1;????%推算周期0.5秒
Ang_Hud=3.14159265358979/180.0;%角度、弧度轉換
Hud_Ang=1/Ang_Hud;
xc=?fopen（‘DR_Result.txt‘‘wt‘）;
D=?load（‘common_seg.txt‘）;%慣導
D1=load（‘Navi_Result.txt‘）;%GPS或USBL
auv_lon=D1（11）;
auv_lan=D1（12）;
[Row_numRank]=size（D）;
for?i=1:Row_num???
Headi=D（i10）-0.0253;%AUV當前航向角，單位度
for_speed=D（i13）*0.9773;%AUV當前前向速度，單位米/秒
ror_speed=-D（i14）*0.9773;%AUV當前右向速度單位米/秒
s1=sin（Headi*Ang_Hud）;
c=co