平台惯性导航Matlab位置解算程序

平台惯性导航Matlab位置解算程序，可以直接使用。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%           指北方位惯性导航系统           %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear                                                                %初始化

load（‘fw‘）;

Re=6378245;                                                          %定义常量
e=1/298.3;
g=[0 0 9.78049*Re^2/（Re+500）^2];                                     %500米高度的重力加速度
longitude=[116*pi/180 zeros（15000）];                                %经度longitude
latitude=[39.9*pi/180 zeros（15000）];                                %纬度latitude
velocity=[0 200 0];                                                %速度velocity
wie=2*pi/24/3600;                                                    %wie为地球相对惯性空间的自转角速度

for i=2:5001
  Rx=Re/（1-e*sin（latitude（i-1））^2）;                                  %Rx为卯酉圈的曲率半径
  Ry=Re/（1+2*e-3*e*sin（latitude（i-1））^2）;                            %Ry为子午圈的曲率半径
  ax=f（1i）‘+2*wie*sin（latitude（i-1））*velocity（2） ...                %加速度分量ax
  +velocity（1）*velocity（2）*tan（latitude（i-1））/Rx;
  ay=f（2i）‘-2*wie*sin（latitude（i-1））*velocity（1） ...                %加速度分量ay
  -velocity（1）^2*tan（latitude（i-1））/Rx;  
  average=velocity;
  velocity=velocity+[ax ay 0]*0.1;
  average=（average+velocity）/2;                                      %速度
  longitude（i）=longitude（i-1）+average（1）/Rx*sec（latitude（i-1））*0.1;
  latitude（i）=latitude（i-1）+average（2）/Ry*0.1;                       %经度longitude纬度latitude
end

longitude=longitude*180/pi;                                          %将弧度转化为角度
latitude=latitude*180/pi;

title（‘位置曲线‘）;                                                   %作图
xlabel（‘经度‘）;
ylabel（‘纬度   ‘‘Rotation‘0）;
grid on
hold on
plot（longitudelatitude‘r‘‘LineWidth‘3）;
plot（longitude（100）latitude（100）‘r^‘‘MarkerFaceColor‘‘r‘）;       %标明方向
plot（longitude（1000）latitude（1000）‘r<‘‘MarkerFaceColor‘‘r‘）;
plot（longitude（2000）latitude（2000）‘rv‘‘MarkerFaceColor‘‘r‘）;
plot（longitude（3000）latitude（3000）‘r<‘‘MarkerFaceColor‘‘r‘）;
plot（longitude（4900）latitude（4900）‘rv‘‘MarkerFaceColor‘‘r‘）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件     219770  2005-10-19 10:15  inertia\fw.mat

     文件       2201  2009-07-27 14:41  inertia\pingtai.m

     文件      45056  2009-07-27 14:42  inertia\文档.doc

     目录          0  2010-10-21 19:08  inertia

----------- ---------  ---------- -----  ----

               267027                    4