acc_gyro.m

惯性导航精解算程序

function [accelerometer gyro]=acc_gyro(trajectory_type,caiyangT,totaltime)
%%生成陀螺仪加速度计的输出数据
%读取真实数据

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(1,1:3);
true_type = parameter(1);
true_caiyangT = parameter(2);
true_step = parameter(3);
if(trajectory_type~=true_type)
    error('轨迹类型不匹配');
end
if(caiyangT<true_caiyangT)
    fprintf('真实数据采样时间为：');disp(true_caiyangT);
    error('采样时间时间应小于真实真实采样时间');
end
if(totaltime>true_caiyangT*true_step)
    error('仿真时间超过真实数据仿真时间');
end
% 捷联惯导系统需要飞行轨迹产生的五组十五个轨迹数据
% 位置：纬度，经度，高度:Storedata_l 纬度经度以弧度为单位
% 地理系下载体相对于地球系的速度:Storedata_v 东向，北向，天向
% 地理系下载体相对于地球系的加速度 :Storedata_a 东向，北向，天向
% 姿态角：俯仰角、横滚角、航向角 :Storedata_e 
% 机体下的坐标系相对于地理系的转动角速率:Storedata_w 
[row line]= size(Storedata);
Storedata1 = Storedata(2:row,1:15);
Location = Storedata1(1:floor(caiyangT/true_caiyangT):true_step,1:3);
Velocity = Storedata1(1:floor(caiyangT/true_caiyangT):true_step,4:6);
Acceleration = Storedata1(1:floor(caiyangT/true_caiyangT):true_step,7:9);
Attitude = Storedata1(1:floor(caiyangT/true_caiyangT):true_step,10:12);

Angle_ve = Storedata1(1:floor(caiyangT/true_caiyangT):true_step,13: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(simT,1);%采样的点数
accelerometer1=zeros(step,3);
gyro1=zeros(step,3);
for i=1:step    
    Ve = Velocity(i,1);%东向的速度
    Vn = Velocity(i,2);%北向的速度
    Vu = Velocity(i,3);%天向的速度
    latitude = Location(i,1);%纬度
    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(i,1);%俯仰角
    gama = Attitude(i,2);%滚动角
    fai = Attitude(i,3);%航向角
    %导航坐标系(地理坐标系)到体坐标系的坐标变换矩阵
    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)                                cos(fai)*cos(theta)              sin(theta);
            cos(fai)*sin(gama)-sin(fai)*sin(theta)*cos(gama)   -sin(fai)*sin(gama)-cos(fai)*sin(theta)*cos(gama)    cos(theta)*cos(gama) ];
    Cb_n = Cb_n/(Cb_n'*Cb_n)^0.5;%保持正交化
    wL = Angle_ve(i,:)';%wL=[dot_theta dot_gama dot_fai] 地理坐标系下欧拉角速度
    SWb_nb = [cos(gama) 0 sin(gama)*cos(theta);%体系相对导航系的角速度在体系上的分量%参照捷联惯性_里程仪_伪卫星车载组合导航系统研究
              0         1      -sin(theta)  ;
              sin(gama) 0     -cos(theta)*cos(gama)]*wL;%参照捷联惯性_里程仪_伪卫星车载组合导航系统研究23
    SWn_ie = [0;wie*cos(latitude);wie*sin(latitude)];%地球自转角速度在导航系上的分量,wie地球自转角速度
    SWn_en = [-dot_latitude;dot_longitude*cos(latitude);dot_longitude*sin(latitude)];%导航系相对地固系的角速度在导航系上的分量
    SWb_ib = SWb_nb + Cb_n*(SWn_ie+SWn_en);%陀螺标称输出，车体相对惯性系的角速度在体系上的分量
    aL = Acceleration(i,:)';
      
    SFn = aL + cross(2*SWn_ie+SWn_en ,(Velocity(i,:))') - gn;%车体相对惯性系加速度在地理坐标系上的分量，标称值%参照捷联惯性_里程仪_伪卫星车载组合导航系统研究36
    SFb = Cb_n*SFn;%%车体相对惯性系加速度在体坐标系上的分量，标称值
    accelerometer1(i,:) = SFb';%加速度计的标称输出
    gyro1(i,:) = SWb_ib';%陀螺仪的标称输出
    waitbar(i/step,hWaitBar,[num2str( fix(i/step*100) ),'%'])
end

close(hWaitBar);
Fe_chang = 0.0*10^-3 ; 
Fe_randn = 0*0.5e-5*g;
Frandn = Fe_chang+ Fe_randn*randn(step,3);%加在加速度上的噪声
accelerometer = accelerometer1+Frandn;
We_chang = 0.00*pi/180/3600;
We_randn =  0*pi/180/3600;
Wrandn = We_chang + We_randn*randn(step,3);%加在陀螺数据上的噪声
gyro = gyro1+Wrandn;
%%
%存储加速度计和陀螺仪数据，第一行记录轨迹类型，采样时间，采样点数的参数以及初始位置
parameter2 = zeros(5,6);
parameter2(1,1) = trajectory_type;
parameter2(1,2) = caiyangT;
parameter2(1,3) = totaltime/caiyangT;%采样的点数;
parameter2(1,4:6) = Location(1,:);

%第二行记录初始速度,加速度
parameter2(2,1:3) = Velocity(1,:);

parameter2(2,4:6) = Acceleration(1,:);

%第三行记录初始姿态,角速度
parameter2(3,1:3) = Attitude(1,:);
parameter2(3,4:6) = Angle_ve(1,:);

%记录加速度计和陀螺仪的误差大小，分为常值噪声和随机噪声
parameter2(4,1:2) = [Fe_chang Fe_randn];
parameter2(5,1:2) = [We_chang We_randn];
acc_gyro_data =[parameter2;accelerometer gyro];

if trajectory_type ==1
    save acc_gyro_line.mat acc_gyro_data -ASCII -DOUBLE -MAT;
elseif trajectory_type ==2
    save acc_gyro_line2.mat acc_gyro_data -ASCII -DOUBLE -MAT;
elseif trajectory_type ==3
    save acc_gyro_circle.mat acc_gyro_data -ASCII -DOUBLE -MAT;
end