飞机惯性导航Matlab语言实现

飞机惯性导航Matlab语言实现 %这是研究惯性导航的最好代码。记得自己添加测试数据 % 此为基于四元素法，角增量法的捷连惯导系统程序算法 % > 飞行器飞行过程中飞行高度不变 % > 航向角以逆时针为正 % > 以地理系为导航坐标系 % > 运行程序时需导入比力信息及陀螺议角速率信息 clc clear close all Data = load('Data1.txt'); f_INS = Data(:,2:4);% 加载加 表 关于同志近三年现实表现材料材料类招标技术评分表图表与交易pdf视力表打印pdf用图表说话 pdf 数据 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))*si n(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))*c os(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^2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2); %> 用四元素表示得姿态矩阵 2*(e1*e2-e0*e3) e0^2-e1^2+e2^2-e3^2 2*(e2*e3+e0*e1); 2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0^2-e1^2-e2^2+e3^2]; E = [e0 e1 e2 e3]';%> 四元素的四个元素值 for i = 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);% > z通道速度计算 Lamda(i) = Vx(i)/cos(L(i))/Rx(i)/80+Lamda(i);% > 经度计算 if Lamda(i)>pi Lamda(i) = Lamda(i)-2*pi;% >经度在-180度(西经)到180(东经)范围 end L(i) = Vy(i)/Ry(i)/80+L(i); %> 纬度计算 if L(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 = [0 Wie*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) 0 Wtb_b(3) -Wtb_b(2); Wtb_b(2) -Wtb_b(3) 0 Wtb_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;% (cos(0.5*derta)*eye(4)+Mwtb*sin(0.5*derta)/derta)*E，采用四阶 近似算法 > E = e0 = E(1);e1 = E(2);e2 = E(3);e3 = E(4); Ctb = [e0^2+e1^2-e2^2-e3^2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2);% > 用四元素表示得姿态矩阵 2*(e1*e2-e0*e3) e0^2-e1^2+e2^2-e3^2 2*(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)); %> 横滚角计算 if abs(Ctb(3,3))>eps Gama(i) = atan(-Ctb(1,3)/Ctb(3,3)); if Ctb(3,3)>0 ——————————————————————————————————————————————— Gama(i) = Gama(i); elseif -Ctb(1,3)> 0 Gama(i) = Gama(i)+pi; else Gama(i) = Gama(i)-pi; end elseif -Ctb(1,3)> 0 Gama(i) = pi/2; else Gama(i) = -pi/2; end Ksai(i) = atan(Ctb(2,1)/Ctb(2,2));% > 航向角计算 if abs(Ctb(2,2))>eps Ksai(i) = atan(Ctb(2,1)/Ctb(2,2)); if Ctb(2,2)>0 Ksai(i) = Ksai(i); elseif Ctb(2,1)> 0 Ksai(i) = Ksai(i)+pi; else Ksai(i) = Ksai(i)-pi; end elseif Ctb(2,1)>0 Ksai(i) = pi/2; else Ksai(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)% > 绘制经度变化曲线图 grid on Xlabel('经度');Ylabel('维度');title('经维度变化曲线图'); figure plot(t,Seita)% > 绘制俯仰角变化曲线图 grid on Xlabel('时间/秒');Ylabel('俯仰角Seita/度');title('俯仰角变化曲线图'); figure plot(t,Gama)% > 绘制横滚角变化曲线图 grid on Xlabel('时间/秒');Ylabel('横滚角Gama/度');title('横滚角变化曲线图'); figure plot(t,Ksai) %> 绘制航向角变化曲线 ——————————————————————————————————————————————— grid on Xlabel('时间/秒');Ylabel('航向角Ksai/度');title('航向角变化曲线图'); figure plot(t,Vx)% > 绘制东向速度变化曲线 grid on Xlabel('时间/秒');Ylabel('东向速度Vx 米/秒');title('东向速度变化曲线图'); figure plot(t,Vy)% > 绘制北向速度变化曲线 grid on Xlabel('时间/秒');Ylabel('北向速度Vy 米/秒');title('北向速度变化曲线图'); figure plot(t,Vz)% > 绘制垂直速度变化曲线 grid on Xlabel('时间/秒');Ylabel('垂直速度Vz 米/秒');title('垂直速度变化曲线图'); ———————————————————————————————————————————————