组合导航松组合间接法的MATLAB程序

从惯导卫导数据轨迹产生，然后将轨迹数据用以产生陀螺仪和加速度计数据，然后用陀螺仪和加速度计数据进行惯导解算并验证仿真的陀螺仪和加速度数据的有效性，之后用仿真的GPS和INS数据做松组合。

%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）                              

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      57572  2015-06-07 16:56  轨迹生成\1.png

     文件      50513  2015-06-07 16:56  轨迹生成\2.png

     文件     116696  2015-08-10 09:15  轨迹生成\20150806仿真结果.docx

     文件      53605  2015-06-07 16:56  轨迹生成\3.png

     文件      47618  2015-06-07 16:55  轨迹生成\4.png

     文件      50732  2015-06-07 16:55  轨迹生成\5.png

     文件       7516  2017-04-20 09:14  轨迹生成\acc_gyro.m

     文件     188346  2017-04-21 15:15  轨迹生成\acc_gyro_line.mat

     文件      90990  2017-04-21 15:15  轨迹生成\acc_line_fb.mat

     文件       7589  2017-04-21 15:15  轨迹生成\GPSdata_line.mat

     文件        200  2017-04-21 15:15  轨迹生成\GPSdata_location0.mat

     文件      14987  2015-04-20 20:40  轨迹生成\guandaochengxu.m

     文件       7743  2017-04-20 11:33  轨迹生成\guandaochengxucopy2.m

     文件      97322  2017-04-21 15:15  轨迹生成\gyro_line_wibb.mat

     文件      15636  2015-08-19 10:41  轨迹生成\kf\up_of_song20150808.m

     文件     401695  2017-04-18 11:37  轨迹生成\New Folder\acc_gyro_line.mat

     文件     196763  2017-04-18 11:37  轨迹生成\New Folder\acc_gyro_line_fb.mat

     文件     204864  2017-04-18 11:37  轨迹生成\New Folder\acc_gyro_line_wib.mat

     文件      11419  2016-04-21 14:58  轨迹生成\song_0306_1.m

     文件      65716  2017-04-21 15:15  轨迹生成\Storedata_line.mat

     文件      34151  2015-10-24 11:50  轨迹生成\Storedata_line_error.mat

     文件      54832  2008-03-09 13:45  轨迹生成\trace\Figure\trace0.fig

     文件      94672  2008-03-03 14:49  轨迹生成\trace\Figure\trace1.fig

     文件      94816  2008-03-03 14:45  轨迹生成\trace\Figure\trace11.fig

     文件     121584  2007-12-05 16:59  轨迹生成\trace\Figure\trace12.fig

     文件      91376  2007-12-05 14:06  轨迹生成\trace\Figure\trace21.fig

     文件     118056  2007-12-05 14:06  轨迹生成\trace\Figure\trace22.fig

     文件      12768  2007-12-05 17:00  轨迹生成\trace\Figure\trace31.fig

     文件      18824  2007-12-05 17:00  轨迹生成\trace\Figure\trace32.fig

     文件      39656  2007-12-05 14:19  轨迹生成\trace\Figure\trace41.fig

............此处省略97个文件信息