航位推算DR算法实现

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