惯性导航精解算程序（挺好的）

惯性导航精解算程序，test_error是主运行程序。

function [accelerometer gyro]=acc_gyro（trajectory_typecaiyangTtotaltime）
%%生成陀螺仪加速度计的输出数据
%读取真实数据

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
% 捷联惯导系统需要飞行轨迹产生的五组十五个轨迹数据
% 位置：纬度，经度，高度:Storedata_l 纬度经度以弧度为单位
% 地理系下载体相对于地球系的速度:Storedata_v 东向，北向，天向
% 地理系下载体相对于地球系的加速度 :Storedata_a 东向，北向，天向
% 姿态角：俯仰角、横滚角、航向角 :Storedata_e 
% 机体下的坐标系相对于地理系的转动角速率:Storedata_w 
[row line]= size（Storedata）;
Storedata1 = Storedata（2:row1:15）;
Location = Storedata1（1:floor（caiyangT/true_caiyangT）:true_step1:3）;
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）;%航向角
    %导航坐标系（地理坐标系）到体坐标系的坐标变换矩阵
    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_ga

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

     文件       5920  2007-09-19 17:37  sin\acc_gyro.m

     文件        764  2007-09-19 17:37  sin\circle_lgkt1.m

     文件       1401  2007-09-19 17:37  sin\circle_lgkt2.m

     文件       1095  2007-09-19 17:37  sin\circle_lgkt4.m

     文件         39  2007-09-19 17:37  sin\error_correct.m

     文件       6092  2007-09-19 17:37  sin\error_function.m

     文件       6011  2007-09-19 17:37  sin\error_function2.m

     文件       4421  2007-09-19 17:37  sin\error_plot.m

     文件        920  2007-09-19 17:37  sin\ImuAliment2.m

     文件       6321  2007-09-19 17:37  sin\Imusolve.m

     文件       3282  2007-09-19 17:37  sin\Imusolve2.m

     文件       6128  2007-09-19 17:37  sin\jing_alignmet2.m

     文件         18  2010-08-06 09:53  sin\readme.txt

     文件        970  2007-09-19 17:37  sin\Storecircle_ture.m

     文件    1357200  2007-09-19 17:37  sin\Storedata_circle.txt

     文件       3770  2007-09-19 17:37  sin\Storedata_line.txt

     文件       3059  2007-09-19 17:37  sin\test_error.m

     文件       6919  2007-09-19 17:37  sin\trajectory.m

     目录          0  2010-08-06 09:53  sin

----------- ---------  ---------- -----  ----

              1414330                    19