%=======本程序為GPS輸出的仿真程序（輸出參數包括運載體的位置、速度信息）=========
clear?all;
close?all;
clc;
deg_rad=pi/180;???%由度轉化成弧度
rad_deg=180/pi;???%由弧度轉化成度
%--------------------------讀取理想數據------------------------------
fid_read=fopen（‘IMUout.txt‘‘r‘）;?????????%軌跡發生器輸出
fid_write=fopen（‘GPSout.txt‘‘w‘）;???????%GPS量測值

[AllData?NumofAllData]=fscanf（fid_read‘%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g‘[17?inf]）;
AllData=AllData‘;???%AllDate?矩陣?有17列?每列為一類數據??行數為每一類數據的個數
NumofEachData=fix（NumofAllData/17）;

%?[AllData?NumofAllData]=fscanf（fid_read‘%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g?%g‘[16?inf]）;
%?AllData=AllData‘;???%AllDate?矩陣?有16列?每列為一類數據??行數為每一類數據的個數
%?NumofEachData=round（NumofAllData/16）;


TimeSINS=AllData（:1）;????%時間序列
longitude=AllData（:2）;???%經度???單位：弧度
latitude=AllData（:3）;????%緯度???單位：弧度
High=AllData（:4）;??%高度???單位：米

Vn=AllData（:5）;
Vu=AllData（:7）;
Ve=-AllData（:6）;

TimeEach=TimeSINS（2）-TimeSINS（1）;?????%采樣時間
NUM=20;
TimeGPS=TimeEach*NUM;??????????%GPS周期
TimeAll=（NumofEachData-1）*TimeEach;????%仿真總時間
NumofGPSout=fix（（NumofEachData）/NUM）;???%GPS?的數據輸出頻率為10
%---------------------------常值定義區-------------------------------
g0=9.78;
Re=6378245.0;?%地球長軸?《慣性導航系統》??P28
e=1/298.3;
%----------------------------GPS誤差的生成----------------------------
dNmissileN=20*randn（NumofGPSout1）;
dNmissileU=20*randn（NumofGPSout1）;?	%三個方向GPS位置誤差的設定
dNmissileE=20*randn（NumofGPSout1）;
dVn=5*randn（NumofGPSout1）;
dVu=5*randn（NumofGPSout1）;?????????????%三個方向GPS速度誤差的設定
dVe=5*randn（NumofGPSout1）;
%--------------------將經緯度轉換成初始時刻當地地理坐標系下---------------
for?ii=1:NumofGPSout
????RM0=Re*（1.0-2.0*e+3.0*e*sin（latitude（1+（ii-1）*NUM））^2）+High（1+（ii-1）*NUM）;????%地球理想長短軸
????RN0=Re*（1.0+e*sin（latitude（1+（ii-1）*NUM））^2）+High（1+（ii-1）*NUM）;
????GPS_lat（ii）=latitude（1+（ii-1）*NUM）+dNmissileN（ii）/RM0;
????GPS_lon（ii）=longitude（1+（ii-1）*NUM）+dNmissileE（ii）/（RN0*cos（latitude（1+（ii-1）*NUM）））;
????GPS_High（ii）=High（1+（ii-1）*NUM）+dNmissileU（ii）;
????
????dlatitude（ii）=GPS_lat（ii）-latitude（（ii-1）*NUM+1）;
????dlongitude（ii）=GPS_lon（ii）-longitude（（ii-1）*NUM+1）;
????dHigh（ii）=GPS_High（ii）-High（（ii-1）*NUM+1）;
????
????GPS_Vn（ii）=Vn（（ii-1）*NUM+1）+dVn（ii）;
????GPS_Vu（ii）=Vu（（ii-1）*NUM+1）+dVu（ii）;
????GPS_Ve（ii）=Ve（（ii-1）*NUM+1）+dVe（ii）;

????TimeGPS（ii）=TimeSINS（（ii-1）*NUM+1）;
????fprintf（fid_write‘%f?%25.16g?%25.16g?%25.16g?%25.16g?%25.16g?%25.16g\n‘TimeGPS（ii）...
????GPS_lon（ii）GPS_lat（ii）GPS_High（ii）GPS_Vn（ii）GPS_Vu（ii）GPS_Ve（ii））;
end

fclose（fid_read）;
fclose（fid_write）;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件??????10009??2007-10-06?18:59??Navigation.m

?????文件??????10980??2007-10-01?16:07??SINS_Out2.m

?????文件???????2773??2007-10-19?20:40??GPSNoiseOut.m

?????文件???????9612??2007-10-15?11:15??Kalmanfilter.m

-----------?---------??----------?-----??----

????????????????33374????????????????????4