INS/GPS 惯性卫星组合导航。输入惯导

惯性卫星组合导航。输入惯导，gps数据输出导航结果

clc
close all
clear all
format long
%% 数据读取
load（‘IMU.dat‘）;
load（‘GPS.dat‘）; 
f_INSc=IMU（1:end-16:8）‘;
wib_INSc=IMU（1:end-13:5）‘*pi/180/3600;
P_GPS=GPS（2:end3:5）‘;
V_GPS=GPS（2:end6:8）‘;
n=length（IMU）;N=length（GPS）;%数据个数
%% 初值设定
L=34.6521606250000*pi/180;                                %初始纬度
J=109.249623717000*pi/180;                            %初始经度
h=362.269000000000;                                              %初始高度（m）
Wie=7.292115147e-5;                                %地球自转角速度（rad/s）
hangxiang=303.10881*pi/180;                                    %初始航向角
fuyang=0.25516*pi/180;                                    %初始俯仰角
henggun=1.76037*pi/180;                                    %初始横滚角
T=0.01;%采样周期（s）
Re=6378245 ;                                   
e=1/298.3 ;                                    
Vx=0;                                   %初始东向水平速度（m/s）
Vy=0.00900000000000000;                 %初始北向水平速度（m/s）
Vz=0.0350000000000000;                  %初始天向速度（m/s）
g0=9.7803267714;                               
gk1=0.00193185138639;                            
gk2=0.00669437999013;
W=zeros（3n-1）;                                    %用于记录实时三轴指令角速度
V=zeros（3n-1）;                                    %用于记录实时东北天速度
attitude=zeros（3n-1）;                             %姿态角
Alt=zeros（3N-1）;
pos=zeros（3N-1）;
vel=zeros（3N-1）;
q=zeros（41）;                                      %四元数
q（1）=cos（hangxiang/2）*cos（fuyang/2）*cos（henggun/2）-sin（hangxiang/2）*sin（fuyang/2）*sin（henggun/2）;
q（2）=cos（hangxiang/2）*sin（fuyang/2）*cos（henggun/2）-sin（hangxiang/2）*cos（fuyang/2）*sin（henggun/2）;
q（3）=cos（hangxiang/2）*cos（fuyang/2）*sin（henggun/2）+sin（hangxiang/2）*sin（fuyang/2）*cos（henggun/2）;
q（4）=cos（hangxiang/2）*sin（fuyang/2）*sin（henggun/2）+sin（hangxiang/2）*cos（fuyang/2）*cos（henggun/2）;
PP=zeros（15N-1）;
X0=zeros（151）;
P0=eye（1515）;
X=zeros（15N-1）;
 %% 捷联结算
for i=1:N-1
    for t=（i-1）*5+1:（i-1）*5+5;
        Cnb=[q（1）^2+q（2）^2-q（3）^2-q（4）^2  2*（q（2）*q（3）+q（1）*q（4））      2*（q（2）*q（4）-q（1）*q（3））;
          2*（q（2）*q（3）-q（1）*q（4））      q（1）^2-q（2）^2+q（3）^2-q（4）^2  2*（q（3）*q（4）+q（1）*q（2））;
          2*（q（2）*q（4）+q（1）*q（3））      2*（q（3）*q（4）-q（1）*q（2））      q（1）^2-q（2）^2-q（3）^2+q（4）^2;];

        g=g0*（1+gk1*sin（L）^2）*（1-2*h/Re）/sqrt（1-gk2*sin（L）^2）;
        ft=Cnb‘*f_INSc（:t）*g;                         %本体系下的比力转换到导航系
        Rxt=（1-e*（sin（L）^2））/Re;                      
        Ryt=（1+2*e-3*e*（sin（L）^2））/Re;                 
        Dvx=ft（1）+2*Wie*sin（L）*Vy+Vx*Vy*Rxt*tan（L）-（2*Wie*cos（L）+Vx*Rxt）*Vz; %东向加速度
        Dvy=ft（2）-2*Wie*sin（L）*Vx-Vx^2*Rxt*tan（L）-Vy*Ryt*Vz;                 %北向加速度
        Dvz=ft（3）+（2*Wie*cos（L）+Vx*Rxt）*Vx+Vy*Ryt*Vy-g;                      %天向加速度
        Vx=Vx+Dvx*T;                                   %东向速度
        Vy=Vy+Dvy*T;                                   %北向速度
        Vz=Vz+Dvz*T;                                   %北向速度
        V（:t）=[VxVyVz]‘;                            %实时速度记录
       

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件     8099809  2020-09-28 17:39  GPS.dat
     文件    68403987  2020-09-28 17:39  IMU.dat
     文件       10299  2020-09-28 17:39  ins-gps.m