捷联惯导仿真程序-MATLAB源代码

捷联惯导仿真程序-MATLAB源代码 %飞行轨迹仿真 %加速度计输出，陀螺输出,及捷联惯导系统输出 %捷联惯导系统 %姿态矩阵初值 %位置矩阵 %四元数计算

clear;
%经度 l=120*pi/180;经度 al=45*pi/180;高度 h=100;初始速度 v0=[0;200;0];
%子午圈曲率半径 rn=re*（1-2*e+3*e*sin（al）^2）;卯酉圈曲率半径 rm=re*（1+e*sin（al）^2）;
re=6378393;
e=1/298.3;
%飞行轨迹仿真 
tf=1*200;
delt=0.02;
for t=1:tf
    l（1）=120*pi/180;
    al（1）=45*pi/180;
    h（1）=100;
    v（:1）=[0;200;0];
    y（1）=0;
    y0（t）=pi/200;
    y（t+1）=y（t）+y0（t）*delt;
    at（:t）=[0;0;y0（t）*v（2t）];
    a（:t）=[100;0cos（y（t））-sin（y（t））;0sin（y（t））cos（y（t））]*at（:t）;
    v（1t+1）=v（1t）+a（1t）*delt;
    v（2t+1）=v（2t）+a（2t）*delt;
    v（3t+1）=v（3t）+a（3t）*delt;
    rn（t+1）=re*（1-2*e+3*e*sin（al（t））^2）;
    rm（t+1）=re*（1+e*sin（al（t））^2）;
    al（t+1）= al（t）+v（2t+1）./（rn（t+1）+h（t））*delt;
    l（t+1）= l（t）+v（1t+1）./（rm（t+1）+h（t））./cos（al（t））*delt;
    h（t+1）=h（t）+v（3t+1）*delt;
end
for t=tf+1:tf*2
    y0（t）=0;
    y（t）=y（tf）;
    at（:t）=[0;0;0];
    a（:t）=at（:t）;
    v（1t+1）=v（1t）+a（1t）*delt;
    v（2t+1）=v（2t）+a（2t）*delt;
    v（3t+1）=v（3t）+a（3t）*delt;
    rn（t+1）=re*（1-2*e+3*e*sin（al（t））^2）;
    rm（t+1）=re*（1+e*sin（al（t））^2）;
    al（t+1）= al（t）+v（2t+1）./（rn（t+1）+h（t））*delt;
    l（t+1）= l（t）+v（1t+1）./（rm（t+1）+h（t））./cos（al（t））*delt;
    h（t+1）=h（t）+v（3t+1）*delt;
end
for t=tf*2:tf*3
    y0（t）=-pi/200;
    y（t+1）=y（t）+y0（t）*delt;
    at（:t）=[0;0;y0（t）*v（2t）];
    a（:t）=[100;0cos（y（t））-sin（y（t））;0sin（y（t））cos（y（t））]*at（:t）;
    v（1t+1）=v（1t）+a（1t）*delt;
    v（2t+1）=v（2t）+a（2t）*delt;
    v（3t+1）=v（3t）+a（3t）*delt;
    rn（t+1）=re*（1-2*e+3*e*sin（al（t））^2）;
    rm（t+1）=re*（1+e*sin（al（t））^2）;
    al（t+1）= al（t）+v（2t+1）./（rn（t+1）+h（t））*delt;
    l（t+1）= l（t）+v（1t+1）./（rm（t+1）+h（t））./cos（al（t））*delt;
    h（t+1）=h（t）+v（3t+1）*delt;
end
for t=tf*3+1:tf*9
    at（:t）=[0;0;0];
    a（:t）=at（:t）;
    v（:t）=v（:tf*3+1）;
    v（1t+1）=v（1t）+a（1t）*delt;
    v（2t+1）=v（2t）+a（2t）*delt;
    v（3t+1）=v（3t）+a（3t）*delt;
    y0（t）=0;
    y（t）=y（tf*3+1）;
    rn（t+1）=re*（1-2*e+3*e*sin（al（t））^2）;
    rm（t+1）=re*（1+e*sin（al（t））^2）;
    al（t+1）= al（t）+v（2t+1）./（rn（t+1）+h（t））*delt;
    l（t+1）= l（t）+v（1t+1）./（rm（t+1）+h（t））./cos（al（t））*delt;
    h（t+1）=h（t）+v（3t）*delt;
end
for t=tf*9:tf*10
    y0（t）=-pi/200;
    y（t+1）=y（t）+y0（t）*delt;
    at（:t）=[0;0;y0（t）*v（2t）];
    a（:t）=[100;0cos（y（t））-sin（y（t））;0sin（y（t））cos（y（t））]*at（:t）;
    v（1t+1）=v（1t）+a（1t）*delt;
    v（2t+1）=v（2t）+a（2t）*delt;
    v（3t+1）=v（3t）+a（3t）*delt;
    rn（t+1）=re*（1-2*e+3*e*sin（al（t））^2）;
    rm（t+1）=re*（1+e*sin（al（t））^2）;
    al（t+1）= al（t）+v（2t+1）./（rn（t+1）+h（t））*delt;
    l（t+1）= l（t）+v（1t+1）./（rm（t+1）+h（t））./cos（al（t））*delt;
    h（t+1）=h（t）+v（3t+1）*delt;
end
for t=tf*10+1:tf*11
    y0（t）=0;
    y（t）=y（tf*10+1）;
    at（:t）=[0;0;0];
    a（:t）=at（:t）;
    v（1t+1）=v（1t）+a（1t）*delt;
    v（2t+1）=v（2t）+a（2t）*delt;
    v（3t+1）=v（3t）+a（3t）*delt;
    rn（t+1）=re*（1-2*e+3*e*sin（al（t））^2）;
    rm（t+1）=re*（1+e*sin（al（t））^2）;
    al（t+1）= al（t）+v（2t+1）./（rn（t+1）+h（t））*delt;
    l（t

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

     文件       9111  2007-01-11 22:08  捷联惯导仿真程序-MATLAB源代码\sins.m

     目录          0  2010-04-21 12:00  捷联惯导仿真程序-MATLAB源代码

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

                 9111                    2