GPS-INS组合导航Matlab程序.zip

Matlab仿真程序，用于INS/GPS模拟和仿真。GPS和INS组合导航Matlab仿真源码（包含实验数据），下载可直接运行

%function [accelerometer gyro]=acc_gyro（trajectory_typecaiyangTtotaltime）
%%生成陀螺仪加速度计的输出数据
%与trajectory配合使用
%读取真实数据
clear all;
clc;
close all;
trajectory_type=1;
caiyangT=0.1;
totaltime=600;

if trajectory_type==1;
    load Storedata_line ;
elseif trajectory_type==2
    load Storedata_line2;
elseif trajectory_type==3
    load Storedata_circle;
end
% 第一行表示真实轨迹类型，采样时间等参数
% 轨迹的类型：% 1.匀速直线运动  % 2.加速直线运动  % 3.匀速圆周运动
  
% 采样的时间 caiyangT
% 采样的点数 step
% 仿真的时间 totaltime
parameter = Storedata（11:3）;
true_type = parameter（1）;
true_caiyangT = parameter（2）;
true_step = parameter（3）;
if（trajectory_type~=true_type）
    error（‘轨迹类型不匹配‘）;
end
if（caiyangT    fprintf（‘真实数据采样时间为：‘）;disp（true_caiyangT）;
    error（‘采样时间时间应小于真实真实采样时间‘）;
end
if（totaltime>true_caiyangT*true_step）
    error（‘仿真时间超过真实数据仿真时间‘）;
end
%   捷联惯导系统需要飞行轨迹产生的五组十五个轨迹数据
%   1~3列位置：纬度，经度，高度:Storedata_l 纬度经度以弧度为单位
%   4~6列 地理系下载体相对于地球系的速度:Storedata_v 东向，北向，天向
%   7~9列地理系下载体相对于地球系的加速度 :Storedata_a 东向，北向，天向
%   10~12列姿态角：俯仰角、横滚角、航向角 :Storedata_e 
%   13~15列 机体下的坐标系相对于地理系的转动角速率:Storedata_w 
[row line]= size（Storedata）;%得到Storedata的行数和列数
Storedata1 = Storedata（2:row1:15）;%第一行为Storedata的特征量舍去
Location = Storedata1（1:floor（caiyangT/true_caiyangT）:true_step1:3）;%floor（x）：向左取整
Velocity = Storedata1（1:floor（caiyangT/true_caiyangT）:true_step4:6）;
Acceleration = Storedata1（1:floor（caiyangT/true_caiyangT）:true_step7:9）;
Attitude = Storedata1（1:floor（caiyangT/true_caiyangT）:true_step10:12）;

Angle_ve = Storedata1（1:floor（caiyangT/true_caiyangT）:true_step13:15）;
%%
%生成加速度计，陀螺仪数据

global Ra e wie;
Ra = 6378140;%6000000%地球参考椭球长轴半径
e = 1/298.257;%0%地球椭球的椭圆度
wie = 15.04088/3600/180*pi; %0%%地球自转角速度
hWaitBar = waitbar（0‘Please wait...‘）;

fprintf（‘正在生成加速度计陀螺仪数据……\n‘）;
simT = [0:caiyangT:totaltime]‘;%仿真的时间
step =size（simT1）;%采样的点数
accelerometer1=zeros（step3）;
gyro1=zeros（step3）;
for i=1:step    
    Ve = Velocity（i1）;%东向的速度
    Vn = Velocity（i2）;%北向的速度
    Vu = Velocity（i3）;%天向的速度
    latitude = Location（i1）;%纬度
    RN = Ra*（1-2*e+3*e*sin（latitude）^2）;%参考椭球子午面内的曲率半径，沿南北方向
    RE = Ra*（1+e*sin（latitude）^2）;%参考椭球子午面的法线平面内的曲率半径，沿东西方向
    g = 9.7803267714*（1+0.00193185138639*sin（latitude）^2）/sqrt（1-0.00669437999013*sin（latitude）^2）;
    gn = [0 0 -g]‘;%地理系上的重力
    dot_latitude = Vn/RN;
    dot_longitude = Ve/（RE*cos（latitude））;
    %俯仰角定义为载体纵轴与纵向水平轴之间的夹角，规定以纵向水平轴为起点，向上为正，向下为负，定义域为-90°~+90°
    %横滚角定义为载体纵向对称面与纵向铅垂面之间的夹角，规定从铅垂面算起，右倾为正，左倾为负，定义域为-180°~+180°
    %航向角定义为载体纵轴在水平面的投影与地理子午线之间的夹角，规定以地理北向为起点，偏东方向为正，定义域为0~360°
    theta = Attitude（i1）;%俯仰角
    gama = Attitude（i2）;%滚动角
    fai = Attitude（i3）;%航向角
    %导航坐标系（地理坐标系）到体坐标系的坐标变换矩阵
    %%%？？？？？？？？？元素对吗，cbn还是cnb？？？？
    %%%这一下原Cb_n被我替换成Cn_b不知道对其他子程序有没有影响
    %        Cb_n = [sin（fai）*sin（theta）*sin（gama）+cos（fai）*cos（gama）   cos（fai）*sin（theta）*sin（gama）-sin（fai）*cos（gama）   -cos（theta）*sin（gama）;
    %        sin（fai）*cos（theta）                              

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-04-28 09:30  整套程序\
     文件        7516  2017-04-20 09:14  整套程序\acc_gyro.m
     文件      188372  2019-12-01 19:32  整套程序\acc_gyro_line.mat
     文件       91008  2019-12-01 19:32  整套程序\acc_line_fb.mat
     文件        7589  2019-12-01 19:35  整套程序\GPSdata_line.mat
     文件         200  2019-12-01 19:35  整套程序\GPSdata_location0.mat
     文件        7743  2017-04-20 11:33  整套程序\guandaochengxucopy2.m
     文件       97329  2019-12-01 19:32  整套程序\gyro_line_wibb.mat
     文件       65716  2019-12-01 19:35  整套程序\Storedata_line.mat
     文件        8949  2017-04-20 11:29  整套程序\trajectory.m
     文件         292  2017-04-28 09:30  整套程序\各文件信息.txt
     文件         270  2017-04-20 09:22  整套程序\各文件信息.txt.bak