GPS-INS.zip

验证有效的GPS-INS 组合导航代码，可以和我上传的GPS/BD receiver, 一起学习

%%%%%%%%%%%%%%%%%捷联惯导GPS组合导航Kalman滤波器%%%%%%%%%%%%%%%%
clc;
clear;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Wie=7.292115e-5;                 %地球自转角速
e=1/298.257;                     %地球扁律
Re=6378137;                      %地球半径
STEP=0.1;                        %惯导解算周期
G0=9.7803;                       %重力加速度初值
arc=pi/180;                      %角度转换
T=0.3;                           %滤波周期，单位秒
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load y_Ishuju;
load y_Gshuju;


S_dat_I=y_I;
S_dat_G=y_G;
%%%%%%%%%%%%%%%%%%%%数据加载%%%%%%%%%%%%%%%%%%%%%%%%%%%

I=eye（13）;

A=zeros（13）;

X00=zeros（131）;

X00（11）=（10/60）*arc;
X00（21）=（10/60）*arc;
X00（31）=（10/60）*arc;
X00（41）=10;
X00（51）=10;
X00（61）=（4/60）*arc;
X00（71）=（4/60）*arc;
X00（81）=（0.5/3600）*arc;
X00（91）=（0.5/3600）*arc;
X00（101）=（0.5/3600）*arc;
X00（111）=（1e-4）*9.81;
X00（121）=（1e-4）*9.81;
X00（131）=（1e-4）*9.81;

P00=zeros（13）;                                             %Klman滤波中估计误差方差阵初值

P00（11）=（（10/60）*arc）*（（10/60）*arc）;
P00（22）=（（10/60）*arc）*（（10/60）*arc）;
P00（33）=（（10/60）*arc）*（（10/60）*arc）;
P00（44）=10*10;
P00（55）=10*10;
P00（66）=（（4/60）*arc）*（（4/60）*arc）;
P00（77）=（（4/60）*arc）*（（4/60）*arc）;
P00（88）=（（0.5/3600）*arc）*（（0.5/3600）*arc）;
P00（99）=（（0.5/3600）*arc）*（（0.5/3600）*arc）;
P00（1010）=（（0.5/3600）*arc）*（（0.5/3600）*arc）;
P00（1111）=（（1e-4）*9.81）*（（1e-4）*9.81）;
P00（1212）=（（1e-4）*9.81）*（（1e-4）*9.81）;
P00（1313）=（（1e-4）*9.81）*（（1e-4）*9.81）;

Q0=zeros（13）;                                              %Klman滤波中过程噪声方差阵初值

Q0（11）=（（1/3）*（（0.5/3600）*arc））*（（1/3）*（（0.5/3600）*arc））;
Q0（22）=（（1/3）*（（0.5/3600）*arc））*（（1/3）*（（0.5/3600）*arc））;
Q0（33）=（（1/3）*（（0.5/3600）*arc））*（（1/3）*（（0.5/3600）*arc））;
Q0（44）=（（1/3）*（1e-4）*9.81）*（（1/3）*（1e-4）*9.81）;
Q0（55）=（（1/3）*（1e-4）*9.81）*（（1/3）*（1e-4）*9.81）;

R1=zeros（4）;                                               %Klman滤波中观测噪声方差阵初值

R1（11）=（0.3）^2;                                          
R1（22）=（0.3）^2;

R1（33）=（5/Re）^2;
R1（44）=（5/Re）^2;

H1=zeros（413）;                                            %Klman滤波中观测阵初值

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%滤波开始%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mmm=round（T/STEP）;

for j=0:1:length（S_dat_I）/mmm-2
    
    Sum=0;
    
    for DRDER=（j*mmm+1）:1:（j+1）*mmm
        
        vx_G=S_dat_G（DRDER1）;
        vy_G=S_dat_G（DRDER2）;
        vz_G=S_dat_G（DRDER3）;
        
        jd_G=S_dat_G（DRDER4）;
        wd_G=S_dat_G（DRDER5）;
        
        Vx_I=S_dat_I（DRDER1）;
        Vy_I=S_dat_I（DRDER2）;
        Vz_I=S_dat_I（DRDER3）;      
        
        Q=S_dat_I（DRDER4）;
        R=S_dat_I（DRDER5）;
        W=S_dat_I（DRDER6）;
    
        JD=S_dat_I（DRDER7）;
        WD=S_dat_I（DRDER8）;
        H=S_dat_I（DRDER9）;
    
        fx_n=S_dat_I（DRDER10）;
        fy_n=S_dat_I（DRDER11）;
        fz_n=S_dat_I（DRDER12）;
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
        q000=cos（Q/2）*cos（R/2）*cos（W/2）-sin（Q/2）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2019-02-22 11:19  GPS-INS\
     文件      376832  2018-09-17 10:07  GPS-INS\INSGPS组合导航系统0510.ppt
     文件        7468  2008-04-27 16:47  GPS-INS\INSandGPS.m
     文件      737280  2008-04-27 16:18  GPS-INS\捷联惯导0509.ppt
     文件      350208  2008-05-04 08:59  GPS-INS\组合导航若干基本问题.ppt