飞机惯性导航Matlab语言实现

%这是研究惯性导航的最好代码。记得自己添加测试数据%此为基于四元素法，角增量法的捷连惯导系统程序算法%飞行器飞行过程中飞行高度不变%航向角以逆时针为正%以地理系为导航坐标系%运行程序时需导入比力信息及陀螺议角速率信息clcclearcloseallData=load('Data1.txt');f_INS=Data(:,2:4);%加载加表数据wib_INS=Data(:,5:7);%加载陀螺数据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(i0.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:L0f_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);%z通道速度计算Lamda(i)=Vx(i)/cos(L(i))/Rx(i)/80+Lamda(i);%经度计算ifLamda(i)piLamda(i)=Lamda(i)-2*pi;%经度在-180度(西经)到180(东经)范围endL(i)=Vy(i)/Ry(i)/80+L(i);%纬度计算ifL(i)(pi/2)L(i)=pi-L(i);%纬度小于90度(北纬)endWetx_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))epsGama(i)=atan(-Ctb(1,3)/Ctb(3,3));ifCtb(3,3)0Gama(i)=Gama(i);elseif-Ctb(1,3)0Gama(i)=Gama(i)+pi;elseGama(i)=Gama(i)-pi;endelseif-Ctb(1,3)0Gama(i)=pi/2;elseGama(i)=-pi/2;endKsai(i)=atan(Ctb(2,1)/Ctb(2,2));%航向角计算ifabs(Ctb(2,2))epsKsai(i)=atan(Ctb(2,1)/Ctb(2,2));ifCtb(2,2)0Ksai(i)=Ksai(i);elseifCtb(2,1)0Ksai(i)=Ksai(i)+pi;elseKsai(i)=Ksai(i)-pi;endelseifCtb(2,1)0Ksai(i)=pi/2;elseKsai(i)=-pi/2;endend%将弧度换算为角度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;%绘制曲线图figureplot(L,Lamda)%绘制经度变化曲线图gridonXlabel('经度');Ylabel('维度');title('经维度变化曲线图');figureplot(t,Seita)%绘制俯仰角变化曲线图gridonXlabel('时间/秒');Ylabel('俯仰角Seita/度');title('俯仰角变化曲线图');figureplot(t,Gama)%绘制横滚角变化曲线图gridonXlabel('时间/秒');Ylabel('横滚角Gama/度');title('横滚角变化曲线图');figureplot(t,Ksai)%绘制航向角变化曲线gridonXlabel('时间/秒');Ylabel('航向角Ksai/度');title('航向角变化曲线图');figureplot(t,Vx)%绘制东向速度变化曲线gridonXlabel('时间/秒');Ylabel('东向速度Vx米/秒');title('东向速度变化曲线图');figureplot(t,Vy)%绘制北向速度变化曲线gridonXlabel('时间/秒');Ylabel('北向速度Vy米/秒');title('北向速度变化曲线图');figureplot(t,Vz)%绘制垂直速度变化曲线gridonXlabel('时间/秒');Ylabel('垂直速度Vz米/秒');title('垂直速度变化曲线图');