惯性导航卡尔曼滤波仿真

惯性导航卡尔曼滤波仿真，包括姿态解算，扩展卡尔曼滤波应用等

%=======本程序为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