导航仿真程序.docx
- 文档编号:4297049
- 上传时间:2022-11-29
- 格式:DOCX
- 页数:12
- 大小:41.38KB
导航仿真程序.docx
《导航仿真程序.docx》由会员分享,可在线阅读,更多相关《导航仿真程序.docx(12页珍藏版)》请在冰豆网上搜索。
导航仿真程序
% 此为基于四元素法,角增量法的捷连惯导系统程序算法
%>飞行器飞行过程中飞行高度不变
%>航向角以逆时针为正
%>以地理系为导航坐标系
%>运行程序时需导入比力信息及陀螺议角速率信息
clearclc
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,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度(北纬)
end
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);
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];
%>姿态角计算
Seita(i+1)=asin(Ctb(2,3));%>俯仰角计算
Gama(i+1)=atan(-Ctb(1,3)/Ctb(3,3));%>横滚角计算
ifabs(Ctb(3,3))>eps
Gama(i+1)=atan(-Ctb(1,3)/Ctb(3,3));
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;
end
elseif-Ctb(1,3)>0
Gama(i+1)=pi/2;
elseGama(i+1)=-pi/2;
end
Ksai(i+1)=atan(Ctb(2,1)/Ctb(2,2));%>航向角计算
ifabs(Ctb(2,2))>eps
Ksai(i+1)=atan(Ctb(2,1)/Ctb(2,2));
ifCtb(2,2)>0
Ksai(i+1)=Ksai(i+1);
elseifCtb(2,1)>0
Ksai(i+1)=Ksai(i+1)+pi;
elseKsai(i+1)=Ksai(i+1)-pi;
end
elseifCtb(2,1)>0
Ksai(i+1)=pi/2;
elseKsai(i+1)=-pi/2;
end
end
%>将弧度换算为角度
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)%>绘制纬度变化曲线图
gridon
Xlabel('时间/秒');Ylabel('纬度L/度');title('纬度变化曲线图');
figure(3);
plot(Lamda,L)%>绘制经-纬度变化曲线图
gridon
Xlabel('经度Lamda/度');Ylabel('纬度L/度');title('经—纬度坐标曲线图');
figure(4);
plot(t,Seita)%>绘制俯仰角变化曲线图
gridon
Xlabel('时间/秒');Ylabel('俯仰角Seita/度');title('俯仰角变化曲线图');
figure(5);
plot(t,Gama)%>绘制横滚角变化曲线图
gridon
Xlabel('时间/秒');Ylabel('横滚角Gama/度');title('横滚角变化曲线图');
figure(6);
plot(t,Ksai)%>绘制航向角变化曲线
gridon
Xlabel('时间/秒');Ylabel('航向角Ksai/度');title('航向角变化曲线图');
figure(7);
plot(t,Vx)%>绘制东向速度变化曲线
gridon
Xlabel('时间/秒');Ylabel('东向速度Vx米/秒');title('东向速度变化曲线图');
figure(8);
plot(t,Vy)%>绘制北向速度变化曲线
gridon
Xlabel('时间/秒');Ylabel('北向速度Vy米/秒');title('北向速度变化曲线图');
figure(9);
plot(t,Vz)%>绘制垂直速度变化曲线
gridon
Xlabel('时间/秒');Ylabel('垂直速度Vz米/秒');title('垂直速度变化曲线图');
65行有错误,不知道能否更正
缺少了注释号 修改如下:
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,采用四阶近似算法
这里我有一分钟的数据,是自己模拟的载机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);%>用四元素表示得姿态矩阵
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:
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度(北纬)
end
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);
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);
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];
%>姿态角计算
Seita(i)=asin(Ctb(2,3));%>俯仰角计算
Gama(i)=atan(-Ctb(1,3)/Ctb(3,3));%>横滚角计算
ifabs(Ctb(3,3))>eps
Gama(i)=atan(-Ctb(1,3)/Ctb(3,3));
ifCtb(3,3)>0
Gama(i)=Gama(i);
elseif-Ctb(1,3)>0
Gama(i)=Gama(i)+pi;
elseGama(i)=Gama(i)-pi;
end
elseif-Ctb(1,3)>0
Gama(i)=pi/2;
elseGama(i)=-pi/2;
end
Ksai(i)=atan(Ctb(2,1)/Ctb(2,2));%>航向角计算
ifabs(Ctb(2,2))>eps
Ksai(i)=atan(Ctb(2,1)/Ctb(2,2));
ifCtb(2,2)>0
Ksai(i)=Ksai(i);
elseifCtb(2,1)>0
Ksai(i)=Ksai(i)+pi;
elseKsai(i)=Ksai(i)-pi;
end
elseifCtb(2,1)>0
Ksai(i)=pi/2;
elseKsai(i)=-pi/2;
end
end
%>将弧度换算为角度
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
Xlabel('经度');Ylabel('维度');title('经维度变化曲线图');
figure
plot(t,Seita)%>绘制俯仰角变化曲线图
gridon
Xlabel('时间/秒');Ylabel('俯仰角Seita/度');title('俯仰角变化曲线图');
figure
plot(t,Gama)%>绘制横滚角变化曲线图
gridon
Xlabel('时间/秒');Ylabel('横滚角Gama/度');title('横滚角变化曲线图');
figure
plot(t,Ksai)%>绘制航向角变化曲线
gridon
Xlabel('时间/秒');Ylabel('航向角Ksai/度');title('航向角变化曲线图');
figure
plot(t,Vx)%>绘制东向速度变化曲线
gridon
Xlabel('时间/秒');Ylabel('东向速度Vx米/秒');title('东向速度变化曲线图');
figure
plot(t,Vy)%>绘制北向速度变化曲线
gridon
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 导航 仿真 程序
![提示](https://static.bdocx.com/images/bang_tan.gif)