trajectory.m

惯性导航精解算程序

function trajectory()%用来生成精确轨迹数据
%%初始化
close all;
clear all;
fprintf('\n请输入轨迹类型：\n1.匀速直线运动;\n2.加速直线运动;\n3.匀速圆周运动(顺时针)\n');
trajectory_type = input('选择相应的轨迹类型：');
if trajectory_type>3|trajectory_type<1
    error('输入参数错误');
    clear trajectory_type;
end 
caiyangT = input('请输入采样时间：');
totaltime = input('请输入仿真时间：');
longitude0 = input('请输入初始经度：');

longitude0 = longitude0*pi/180;
latitude0 = input('请输入初始纬度：');
latitude0 =latitude0*pi/180;
if trajectory_type == 1
    line_v = input('请输入运动速度：');
    line_fai = input('请输入偏航角(0～2π)：');
end
if trajectory_type == 2
    line_v = input('请输入初始运动速度:');
    line_fai = input('请输入偏航角(0～2π):');
    line_ac = input('请输入加速度:')
end
if trajectory_type == 3
    circle_r = input('请输入圆半径：');
    circle_w = input('请输入圆周角速率(小时/圈)：');
    circle_fai0 = input('请输入初始偏航角(0～2π)');
    circle_w = 2*pi/(3600*circle_w);%角速率
end
global Ra e wie;
Ra = 6378140;%6000000%地球参考椭球长轴半径
e = 1/298.257;%0%地球椭球的椭圆度
wie = 15.04088/3600/180*pi; %0%%地球自转角速度
% fprintf('地球参考椭球长轴半径:6378140\n地球椭球的椭圆度:1/298.257\n');
simT = [0:caiyangT:totaltime]';%仿真的时间
step =size(simT,1);%采样的点数

%%
%生成真实匀速直线运动轨迹
if trajectory_type ==1
    
    EL = [0 0 line_fai]';%初始姿态角 欧拉角 theta俯仰角 gama滚动角 fai航向角
    LL = [latitude0 longitude0 0]'; %初始经纬度,先是纬度再是经度
    vL = [line_v*sin(line_fai) line_v*cos(line_fai) 0]';
    
    aL = [0 0 0]';%加速度 
    wL = [0 0 0]';%地理坐标系下的欧拉角速度
end


%%
if trajectory_type ==2%生成真实加速直线运动轨迹
    EL = [0 0 line_fai]';%初始姿态角 欧拉角 theta俯仰角 gama滚动角 fai航向角
    LL = [latitude0 longitude0 0]'; %初始经纬度，先是纬度再是经度 
    vL = [line_v*sin(line_fai) line_v*cos(line_fai) 0]';
    if(line_fai>=0&line_fai<pi*2)
        aL = [line_ac*sin(line_fai) line_ac*cos(line_fai) 0]';%加速度
        else
        error('航向角在(0～2π)之间');
    end
    
    wL = [0 0 0]';%地理坐标系下的欧拉角速度
end
%%
if trajectory_type ==3 %匀速圆周运动生成轨迹
    EL = [0 0 circle_fai0]';%初始姿态角 欧拉角 theta俯仰角 gama滚动角 fai航向角
    LL = [latitude0 longitude0 0]'; %初始经纬度
    vL = [circle_r*circle_w*sin(circle_fai0) circle_r*circle_w*cos(circle_fai0) 0]';%地理坐标系下初始速度啊
    aL = [circle_w^2*circle_r*cos(circle_fai0) -circle_w^2*circle_r*sin(circle_fai0) 0]';%地理坐标系下的加速度
    wL = [0 0 circle_w]';%地理坐标系下的欧拉角速度
    
end
%%
hWaitBar = waitbar(0,'Please wait...');
fprintf('正在产生真实航迹数据……\n');
Storedata_v=zeros(step,3);
Storedata_l=zeros(step,3);
Storedata_a=zeros(step,3);
Storedata_e=zeros(step,3);
Storedata_w=zeros(step,3);
if trajectory_type==1|trajectory_type==2%生成直线运动的数据，主要是速度和位置的变化；
for ii=1:step
    if ii==1
        Storedata_v(1,:) = vL';
        Storedata_l(1,:) = LL';
    else

        Storedata_v(ii,:) = Storedata_v(ii-1,:)+aL'*caiyangT;
        latitude = Storedata_l(ii-1,1);
        RN = Ra*(1-2*e+3*e*sin(latitude)^2);%参考椭球子午面内的曲率半径
        RE = Ra*(1+e*sin(latitude)^2);%参考椭球子午面的法线平面内的曲率半径
        dot_latitude = Storedata_v(ii-1,2)/RN;%纬度变化率
        dot_longitude = Storedata_v(ii-1,1)/(RE*cos(latitude));
        dot_v = [dot_latitude dot_longitude 0];
        Storedata_l(ii,:) = Storedata_l(ii-1,:)+dot_v*caiyangT;
    end
    

waitbar(ii/step,hWaitBar,[num2str( fix(ii/step*100) ),'%'])
end
    Storedata_a = ones(step,1)*aL';
    Storedata_e = ones(step,1)*EL';
    Storedata_w = ones(step,1)*wL';
close(hWaitBar);
end


%%
if trajectory_type==3%生成匀速圆周运动轨迹
    
    vv = circle_r*circle_w;%速率
    aa = circle_w^2*circle_r;%加速度的大小
for ii=1:step
    if ii==1
        Storedata_v(1,:) = vL';
        Storedata_l(1,:) = LL';
        Storedata_a(1,:) = aL';
        Storedata_e(1,:) = EL';
        Storedata_w(1,:) = wL';
    else
%         Storedata_v(ii,:) = Storedata_v(ii-1,:)+Storedata_a(ii-1,:)*caiyangT;
%         latitude = Storedata_l(ii-1,1);
%         RN = Ra*(1-2*e+3*e*sin(latitude)^2);%参考椭球子午面内的曲率半径
%         RE = Ra*(1+e*sin(latitude)^2);%参考椭球子午面的法线平面内的曲率半径
%         dot_latitude = Storedata_v(ii-1,2)/RN;%纬度变化率
%         dot_longitude = Storedata_v(ii-1,1)/(RE*cos(latitude));
%         dot_v = [dot_latitude dot_longitude 0];
%         Storedata_l(ii,:) = Storedata_l(ii-1,:)+dot_v*caiyangT;
%         Storedata_e(ii,:) = Storedata_e(ii-1,:)+Storedata_w(ii-1,:)*caiyangT;
%         if(Storedata_e(ii,3))>=2*pi
%             Storedata_e(ii,3)=Storedata_e(ii,3)-2*pi;
%         end
%         Storedata_w(ii,:) = wL';
%         Storedata_a(ii,:) = [Storedata_w(ii,3)^2*circle_r*cos(Storedata_e(ii,3)) -Storedata_w(ii,3)^2*circle_r*sin(Storedata_e(ii,3)) 0];
    Storedata_e(ii,:) = Storedata_e(ii-1,:)+Storedata_w(ii-1,:)*caiyangT;
        if(Storedata_e(ii,3))>=2*pi
            Storedata_e(ii,3)=Storedata_e(ii,3)-2*pi;
        end
        f1 = Storedata_e(ii,3);
    Storedata_v(ii,:) = [vv*sin(Storedata_e(ii,3)) vv*cos(Storedata_e(ii,3)) 0];   
    latitude = Storedata_l(ii-1,1);   
    RN = Ra*(1-2*e+3*e*sin(latitude)^2);%参考椭球子午面内的曲率半径
    RE = Ra*(1+e*sin(latitude)^2);%参考椭球子午面的法线平面内的曲率半径

    
    
    
    
    
    
    
    
    
    
    
    Storedata_l(ii,:) = [Storedata_l(1,1)+circle_r*sin(f1)/RN Storedata_l(1,2)+circle_r*(1-cos(f1))/(RE*cos(latitude)) 0];
    Storedata_w(ii,:) = wL';
    Storedata_a(ii,:) =[aa*cos(Storedata_e(ii,3)) -aa*sin(Storedata_e(ii,3)) 0];
    end
    
waitbar(ii/step,hWaitBar,[num2str( fix(ii/step*100) ),'%'])

end
close(hWaitBar);
end
%%
% 捷联惯导系统需要飞行轨迹产生的五组十五个轨迹数据
% 位置：纬度，经度，高度:Storedata_l 纬度经度以弧度为单位
% 地理系下载体相对于地球系的速度:Storedata_v 东向，北向，天向
% 地理系下载体相对于地球系的加速度 :Storedata_a 东向，北向，天向
% 姿态角：俯仰角、横滚角、航向角 :Storedata_e 
% 机体下的坐标系相对于地理系的转动角速率:Storedata_w 
% 轨迹的类型：
  % 1.匀速直线运动
  % 2.加速直线运动
  % 3.匀速圆周运动
% 采样的时间 simT
% 采样的点数 step
% 仿真的时间 totaltime
parameter = zeros(1,15);
parameter(1) = trajectory_type;

parameter(2) = caiyangT;
parameter(3) = step;

Storedata = [Storedata_l Storedata_v Storedata_a Storedata_e Storedata_w];
Storedata = [parameter;Storedata];%存储的数据的第一行表示轨迹类型，采样时间等参数

if trajectory_type ==1
    save Storedata_line.mat Storedata -ASCII -DOUBLE -MAT
elseif trajectory_type ==2
    save Storedata_line2.mat Storedata -ASCII -DOUBLE -MAT
elseif trajectory_type ==3
    save Storedata_circle.mat Storedata -ASCII -DOUBLE -MAT
end