gpsnoiseout.m

本压缩文件包含GPS量测数据仿真和惯性导航解算以及卡尔曼滤波在导航解算中的应用

%=======本程序为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(NumofGPSout,1);
dNmissileU=20*randn(NumofGPSout,1); 	%三个方向GPS位置误差的设定
dNmissileE=20*randn(NumofGPSout,1);
dVn=5*randn(NumofGPSout,1);
dVu=5*randn(NumofGPSout,1);             %三个方向GPS速度误差的设定
dVe=5*randn(NumofGPSout,1);
%--------------------将经纬度转换成初始时刻当地地理坐标系下---------------
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);