导航仿真程序Word格式.docx
《导航仿真程序Word格式.docx》由会员分享，可在线阅读，更多相关《导航仿真程序Word格式.docx（12页珍藏版）》请在冰豆网上搜索。
y通道速度
Vz
（1）=0.007106781;
z通道速度
重力加速度计算参数
g0=9.7803267714;
gk1=0.00193185138639;
gk2=0.00669437999013;
Vx=zeros（1,48001）;
Vy=zeros（1,48001）;
Vz=zeros（1,48001）;
Lamda=zeros（1,48001）;
L=zeros（1,48001）;
Seita=zeros（1,48001）;
Gama=zeros（1,48001）;
Ksai=zeros（1,48001）;
四元素初始值
e0=cos（0.5*Ksai
（1））*cos（0.5*Seita
（1））*cos（0.5*Gama
（1））-sin（0.5*Ksai
（1））*sin（0.5*Seita
（1））*sin（0.5*Gama
（1））;
e1=-cos（0.5*Ksai
（1））*sin（0.5*Seita
（1））*cos（0.5*Gama
（1））+sin（0.5*Ksai
（1））*cos（0.5*Seita
（1））*sin（0.5*Gama
（1））;
e2=-cos（0.5*Ksai
（1））*cos（0.5*Seita
（1））*sin（0.5*Gama
（1））-sin（0.5*Ksai
（1））*sin（0.5*Seita
（1））*cos（0.5*Gama
（1））;
e3=cos（0.5*Ksai
（1））*sin（0.5*Seita
（1））*sin（0.5*Gama
（1））+sin（0.5*Ksai
（1））*cos（0.5*Seita
（1））*cos（0.5*Gama
（1））;
Ctb=[e0^2+e1^2-e2^2-e3^22*（e1*e2+e0*e3）2*（e1*e3-e0*e2）;
用四元素表示得姿态矩阵
2*（e1*e2-e0*e3）e0^2-e1^2+e2^2-e3^22*（e2*e3+e0*e1）;
2*（e1*e3+e0*e2）2*（e2*e3-e0*e1）e0^2-e1^2-e2^2+e3^2];
E=[e0e1e2e3]'
;
四元素的四个元素值
fori=1:
48000
Ry（i）=Re*（1-2*e+3*e*（sin（L（i）））^2）;
计算子午圈主曲率半径
Rx（i）=Re*（1+e*（sin（L（i）））^2）;
计算卯酉圈主曲率半径
g=g0*（1+gk1*（sin（L（i）））^2）*（1-2*h/Re）/sqrt（1-gk2*（sin（L（i）））^2）;
重力加速度计算
Cbt=Ctb'
f_t=Cbt*f_INSc;
将体轴系中的比例转化到地理系
Vx（i+1）=（f_t（1,i）+2*Wie*sin（L（i））*Vy（i）+Vx（i）*Vy（i）*tan（L（i））/Rx（i））/80+Vx（i）;
x通道速度计算
Vy（i+1）=（f_t（2,i）-2*Wie*sin（L（i））*Vx（i）-Vx（i）*Vx（i）*tan（L（i））/Rx（i））/80+Vy（i）;
y通道速度计算
Vz（i+1）=（f_t（3,i）+2*Wie*cos（L（i））*Vx（i）+Vx（i）*Vx（i）/Rx（i）+Vy（i）*Vy（i）/Ry（i）-g）/80+Vz（i）;
Lamda（i+1）=Vx（i）/cos（L（i））/Rx（i）/80+Lamda（i）;
经度计算
ifLamda（i+1）>
pi
Lamda（i+1）=Lamda（i+1）-2*pi;
经度在-180度（西经）到180（东经）范围
end
L（i+1）=Vy（i）/Ry（i）/80+L（i）;
纬度计算
ifL（i+1）>
（pi/2）
L（i+1）=pi-L（i+1）;
纬度小于90度（北纬）
Wetx_t（i）=-Vy（i）/Ry（i）;
Wety_t（i）=Vx（i）/Rx（i）;
Wetz_t（i）=Vx（i）*tan（L（i））/Rx（i）;
在地理坐标系的位移角速率
Wet_t=[Wetx_t（i）Wety_t（i）Wetz_t（i）]'
Wib_b=[wib_INSc（1,i）wib_INSc（2,i）wib_INSc（3,i）]'
陀螺仪测的角速率值
Wie_t=[0Wie*cos（L（i））Wie*sin（L（i））]'
在地理坐标系的地球角速率
Wtb_b=Wib_b-Ctb*（Wie_t+Wet_t）;
姿态矩阵角速率
用角增量法计算四元素姿态矩阵
Mwtb=[0-Wtb_b
（1）-Wtb_b
（2）-Wtb_b（3）;
Wtb_b
（1）0Wtb_b（3）-Wtb_b
（2）;
Wtb_b
（2）-Wtb_b（3）0Wtb_b
（1）;
Wtb_b（3）Wtb_b
（2）-Wtb_b
（1）0]/80;
derta=sqrt（（Mwtb（1,2））^2+（Mwtb（1,3））^2+（Mwtb（1,4））^2）;
E=[eye（4）*（1-derta^2/8+derta^4/384）+（1/2-derta^2/48）*Mwtb]*E;
>
E=（cos（0.5*derta）*eye（4）+Mwtb*sin（0.5*derta）/derta）*E，采用四阶近似算法
e0=E
（1）;
e1=E
（2）;
e2=E（3）;
e3=E（4）;
姿态角计算
Seita（i+1）=asin（Ctb（2,3））;
俯仰角计算
Gama（i+1）=atan（-Ctb（1,3）/Ctb（3,3））;
横滚角计算
ifabs（Ctb（3,3））>
eps
ifCtb（3,3）>
0
Gama（i+1）=Gama（i+1）;
elseif-Ctb（1,3）>
0
Gama（i+1）=Gama（i+1）+pi;
elseGama（i+1）=Gama（i+1）-pi;
Gama（i+1）=pi/2;
elseGama（i+1）=-pi/2;
Ksai（i+1）=atan（Ctb（2,1）/Ctb（2,2））;
航向角计算
ifabs（Ctb（2,2））>
ifCtb（2,2）>
Ksai（i+1）=Ksai（i+1）;
elseifCtb（2,1）>
Ksai（i+1）=Ksai（i+1）+pi;
elseKsai（i+1）=Ksai（i+1）-pi;
Ksai（i+1）=pi/2;
elseKsai（i+1）=-pi/2;
将弧度换算为角度
Seita=Seita*180/pi;
Gama=Gama*180/pi;
Ksai=Ksai*180/pi;
L=L*180/pi;
Lamda=Lamda*180/pi;
t=0:
1/80:
600;
绘制曲线图
figure
（1）;
plot（t,Lamda）%>
绘制经度变化曲线图
gridon
Xlabel（'
时间/秒'
）;
Ylabel（'
经度Lamda/度'
title（'
经度变化曲线图'
figure
（2）;
plot（t,L）%>
绘制纬度变化曲线图
纬度L/度'
纬度变化曲线图'
figure（3）;
plot（Lamda,L）%>
绘制经-纬度变化曲线图
经—纬度坐标曲线图'
figure（4）;
plot（t,Seita）%>
绘制俯仰角变化曲线图
俯仰角Seita/度'
俯仰角变化曲线图'
figure（5）;
plot（t,Gama）%>
绘制横滚角变化曲线图
横滚角Gama/度'
横滚角变化曲线图'
figure（6）;
plot（t,Ksai）%>
绘制航向角变化曲线
航向角Ksai/度'
航向角变化曲线图'
figure（7）;
plot（t,Vx）%>
绘制东向速度变化曲线
东向速度Vx米/秒'
东向速度变化曲线图'
figure（8）;
plot（t,Vy）%>
绘制北向速度变化曲线
北向速度Vy米/秒'
北向速度变化曲线图'
figure（9）;
plot（t,Vz）%>
绘制垂直速度变化曲线
垂直速度Vz米/秒'
垂直速度变化曲线图'
65行有错误,不知道能否更正
缺少了注释号
修改如下：
这里我有一分钟的数据，是自己模拟的载机S机动，现将其传上来，顺便将这程序进行了一点点的改变，给出的结果并不理想，大家可以拿去跑一下，反正跟我真实的轨迹差别很大
此为基于四元素法，角增量法的捷连惯导系统程序算法
飞行器飞行过程中飞行高度不变
航向角以逆时针为正
以地理系为导航坐标系
运行程序时需导入比力信息及陀螺议角速率信息
clc
clear
closeall
Data=load（'
Track_S.txt'
f_INS=Data（:
1:
3）;
%加载加表数据
wib_INS=Data（:
4:
6）;
%加载陀螺数据
L0=size（Data,1）;
Wie=7.292115147e-5;
地球自传角速度
Re=6378245;
地球椭球长半径
h=30;
飞行高度
e=1/298.3;
初始经纬度
Lamda
（1）=116.344695283*pi/180;
初始经度（弧度）
L
（1）=39.975172*pi/180;
初始纬度（弧度）
初始姿态角
Seita
（1）=0.120992605*pi/180;
俯仰角（弧度）
Gama
（1）=0.010445947*pi/180;
横滚角（弧度）
Ksai
（1）=91.637207*pi/180;
航向角（弧度）
初始速度
Vx
（1）=0.000048637;
x通道速度
Vy
（1）=0.000206947;
y通道速度
Vz
（1）=0.007106781;
z通道速度
重力加速度计算参数
g0=9.7803267714;
gk1=0.00193185138639;
gk2=0.00669437999013;
Vx=zeros（1,L0）;
Vy=zeros（1,L0）;
Vz=zeros（1,L0）;
Lamda=zeros（1,L0）;
L=zeros（1,L0）;
Seita=zeros（1,L0）;
Gama=zeros（1,L0）;
Ksai=zeros（1,L0）;
四元素初始值
e0=cos（0.5*Ksai
（1））*cos（0.5*Seita
（1））*cos（0.5*Gama
（1））-sin（0.5*Ksai
（1））*sin（0.5*Seita
（1））*sin（0.5*Gama
（1））;
e1=-cos（0.5*Ksai
（1））*sin（0.5*Seita
（1））*cos（0.5*Gama
（1））+sin（0.5*Ksai
（1））*cos（0.5*Seita
（1））*sin（0.5*Gama
（1））;
e2=-cos（0.5*Ksai
（1））*cos（0.5*Seita
（1））*sin（0.5*Gama
（1））-sin（0.5*Ksai
（1））*sin（0.5*Seita
（1））*cos（0.5*Gama
（1））;
e3=cos（0.5*Ksai
（1））*sin（0.5*Seita
（1））*sin（0.5*Gama
（1））+sin（0.5*Ksai
（1））*cos（0.5*Seita
（1））*cos（0.5*Gama
（1））;
Ctb=[e0^2+e1^2-e2^2-e3^22*（e1*e2+e0*e3）2*（e1*e3-e0*e2）;
用四元素表示得姿态矩阵
E=[e0e1e2e3]'
四元素的四个元素值
fori=1:
L0
f_INSc=f_INS（i,:
）'
wib_INSc=wib_INS（i,:
Ry（i）=Re*（1-2*e+3*e*（sin（L（i）））^2）;
计算子午圈主曲率半径
Rx（i）=Re*（1+e*（sin（L（i）））^2）;
计算卯酉圈主曲率半径
g=g0*（1+gk1*（sin（L（i）））^2）*（1-2*h/Re）/sqrt（1-gk2*（sin（L（i）））^2）;
重力加速度计算
Cbt=Ctb'
f_t=Cbt*f_INSc;
将体轴系中的比例转化到地理系
Vx（i）=（f_t
（1）+2*Wie*sin（L（i））*Vy（i）+Vx（i）*Vy（i）*tan（L（i））/Rx（i））/80+Vx（i）;
x通道速度计算
Vy（i）=（f_t
（2）-2*Wie*sin（L（i））*Vx（i）-Vx（i）*Vx（i）*tan（L（i））/Rx（i））/80+Vy（i）;
y通道速度计算
Vz（i）=（f_t（3）+2*Wie*cos（L（i））*Vx（i）+Vx（i）*Vx（i）/Rx（i）+Vy（i）*Vy（i）/Ry（i）-g）/80+Vz（i）;
Lamda（i）=Vx（i）/cos（L（i））/Rx（i）/80+Lamda（i）;
经度计算
ifLamda（i）>
pi
Lamda（i）=Lamda（i）-2*pi;
经度在-180度（西经）到180（东经）范围
end
L（i）=Vy（i）/Ry（i）/80+L（i）;
纬度计算
ifL（i）>
（pi/2）
L（i）=pi-L（i）;
纬度小于90度（北纬）
Wetx_t（i）=-Vy（i）/Ry（i）;
Wety_t（i）=Vx（i）/Rx（i）;
Wetz_t（i）=Vx（i）*tan（L（i））/Rx（i）;
在地理坐标系的位移角速率
Wet_t=[Wetx_t（i）Wety_t（i）Wetz_t（i）]'
Wib_b=[wib_INSc
（1）wib_INSc
（2）wib_INSc（3）]'
陀螺仪测的角速率值
Wie_t=[0Wie*cos（L（i））Wie*sin（L（i））]'
在地理坐标系的地球角速率
Wtb_b=Wib_b-Ctb*（Wie_t+Wet_t）;
姿态矩阵角速率
用角增量法计算四元素姿态矩阵
Mwtb=[0-Wtb_b
（1）-Wtb_b
（2）-Wtb_b（3）;
derta=sqrt（（Mwtb（1,2））^2+（Mwtb（1,3））^2+（Mwtb（1,4））^2）;
E=[eye（4）*（1-derta^2/8+derta^4/384）+（1/2-derta^2/48）*Mwtb]*E;
E=（cos（0.5*derta）*eye（4）+Mwtb*sin（0.5*derta）/derta）*E，采用四阶近似算法
e0=E
（1）;
e1=E
（2）;
e2=E（3）;
e3=E（4）;
姿态角计算
Seita（i）=asin（Ctb（2,3））;
俯仰角计算
Gama（i）=atan（-Ctb（1,3）/Ctb（3,3））;
横滚角计算
eps
Gama（i）=Gama（i）;
0
Gama（i）=Gama（i）+pi;
elseGama（i）=Gama（i）-pi;
Gama（i）=pi/2;
elseGama（i）=-pi/2;
Ksai（i）=atan（Ctb（2,1）/Ctb（2,2））;
航向角计算
Ksai（i）=Ksai（i）;
Ksai（i）=Ksai（i）+pi;
elseKsai（i）=Ksai（i）-pi;
Ksai（i）=pi/2;
elseKsai（i）=-pi/2;
将弧度换算为角度
Seita=Seita*180/pi;
Gama=Gama*180/pi;
Ksai=Ksai*180/pi;
L=L*180/pi;
Lamda=Lamda*180/pi;
t=0.01:
0.01:
L0*0.01;
绘制曲线图
figure
plot（L,Lamda）%>
绘制经度变化曲线图
gridon
经度'
维度'
经维度变化曲线图'
绘制俯仰角变化曲线图
绘制横滚角变化曲线图
绘制航向角变化曲线
绘制东向速度变化曲线
绘制北向速度变化曲线
升级会员 