飞行轨迹仿真

function Progen
%function Progen ：profile generator 飞行轨迹发生器
%根据不同运动阶段轨迹参数（segment parameters），生成相应轨迹数据，同时得到加计和陀螺的输出信息（用于纯惯性解算）
%坐标系定义：地理系取为（东北天）ENU；机体系取为右前上；子惯导坐标系取为右前上
%Parameters 
%   t_simulate  轨迹仿真时间
%   th          龙格库塔积分步长
%   t_save      轨迹数据保存步长
%Ref
%   1：Design and Analysis of Transfer Alignment Algorithm
%   2：严恭敏硕士论文
%   3：秦永元《惯性导航》
%Checked:2012/04/17
%==========================================================================
    clear
    close all
    clc
    global  deg minu    %deg角度 minu角分                        
    ConstsAndUnits;     %基本的参数设置
    th = 0.005;         %轨迹解算数据更新周期（RK4积分步长）（s）
    t_simulate = 600;  %轨迹仿真时间（s）
    t_save=0.005;        %轨迹数据保存周期（s）
%初值姿态、速度、位置设置  根据实际情况调整
    att = [0; 0; 0]*deg;                                            %[pitch（俯仰角） roll（横滚角） azimuth（航向角）]  初始姿态角（rad）  
	vb =  [0; 0; 0];                                                %[0 vby 0]       初始速度（m/s）         
	pos = [34*deg+14.76289014*minu; 108*deg+54.57983*minu; 380];	%[latitude longitude height]初始位置
    
    Length=t_simulate/t_save;                               
    profile_data = zeros（Length16）;                      %数据保存，每2.5ms保存一次  
    profile = [att;（Att2Mat（att））*vb; pos;zeros（61）];    %15维列向量 姿态、速度、位置、陀螺角增量输出、加计速度增量（此处的增量是相对零时刻的总增量）   Att2Mat作用：将姿态角转化为姿态矩阵Cb2n 
    profile_data（1:） = [0;profile]‘;                     %将0和profile列向量中的15个元素赋值给profile_data的第一行
%==========================================================================
%4阶龙格库塔（RK4）轨迹解算
    fprintf（‘\r轨迹解算进度：%5.0f s‘ 0）;           %\r:回车将当前位置移到本行的开头    %5.0f设置输出的格式
    segparam2 = getsegparam（0profile）;             %获取运动阶段轨迹参数初值   初始时刻零O时间
    for k=5:5:t_simulate*1000                     %RK4轨迹解算步长5ms    %for 循环变量=表达式1：表达式2：表达式3，表达式2为步长
        segparam0 = segparam2;             k1 = RK4（profile segparam0）;
        segparam1 = getsegparam（k-2.5profile）;     k2 = RK4（profile+th/2*k1 segparam1）;
                                                    k3 = RK4（profile+th/2*k2 segparam1）;
        segparam2 = getsegparam（kprofile）;         k4 = RK4（profile+  th*k3 segparam2）;
        profile = profile + th/6*（k1+2*k2+2*k3+k4）;           %k1到K4都是变化率，乘以时间th/6就是变化量
        if mod（kt_save*1000）==0                                 %5ms保存一次数据 
           profile_data（k/（t_save*1000）:）=[k/1000;profile]‘;
           if mod（k1000）==0
              fprintf（‘\b\b\b\b\b\b\b%5.0f s‘ k/1000）;           %解算进度显示（s） \b退格 1s显示一次
           end
        end
    end    
                                                                   %数据保存到Profile_data文件中 
    save Profile_data  profile_data                                 % save  文件名  变量
end
%@@@@@@@@@@@@@@@@@@@@@@@@@ 轨迹参数解算 @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
function dprofile = RK4（profilesegparam）
%function RK4：4th-order Runge–Kutta method 轨迹微分方程解算
%INPUTS
%   profile（1:3） 姿态角[pitch roll azimuth]
%   profile（4:6） 速度地理系投影
%   profile（7:9） 位置[latitude longitude h