%这是研究惯性导航的最好代码。

记得自己添加测试数据% 此为基于四元素法，角增量法的捷连惯导系统程序算法% > 飞行器飞行过程中飞行高度不变% > 航向角以逆时针为正% > 以地理系为导航坐标系% > 运行程序时需导入比力信息及陀螺议角速率信息clcclearclose allData = 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^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: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);% > 经度计算if Lamda(i)>piLamda(i) = Lamda(i)-2*pi;% >经度在-180度(西经)到180(东经)范围endL(i) = Vy(i)/Ry(i)/80+L(i); %> 纬度计算if L(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 = [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;% > 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^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))>epsGama(i) = atan(-Ctb(1,3)/Ctb(3,3));if Ctb(3,3)>0Gama(i) = Gama(i);elseif -Ctb(1,3)> 0Gama(i) = Gama(i)+pi;else Gama(i) = Gama(i)-pi;endelseif -Ctb(1,3)> 0Gama(i) = pi/2;else Gama(i) = -pi/2;endKsai(i) = atan(Ctb(2,1)/Ctb(2,2));% > 航向角计算if abs(Ctb(2,2))>epsKsai(i) = atan(Ctb(2,1)/Ctb(2,2));if Ctb(2,2)>0Ksai(i) = Ksai(i);elseif Ctb(2,1)> 0Ksai(i) = Ksai(i)+pi;else Ksai(i) = Ksai(i)-pi;endelseif Ctb(2,1)>0Ksai(i) = pi/2;else Ksai(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)% > 绘制经度变化曲线图grid onXlabel('经度');Ylabel('维度');title('经维度变化曲线图');figureplot(t,Seita)% > 绘制俯仰角变化曲线图grid onXlabel('时间/秒');Ylabel('俯仰角Seita/度');title('俯仰角变化曲线图');figureplot(t,Gama)% > 绘制横滚角变化曲线图grid onXlabel('时间/秒');Ylabel('横滚角Gama/度');title('横滚角变化曲线图'); figureplot(t,Ksai) %> 绘制航向角变化曲线grid onXlabel('时间/秒');Ylabel('航向角Ksai/度');title('航向角变化曲线图'); figureplot(t,Vx)% > 绘制东向速度变化曲线grid onXlabel('时间/秒');Ylabel('东向速度Vx 米/秒');title('东向速度变化曲线图'); figureplot(t,Vy)% > 绘制北向速度变化曲线grid onXlabel('时间/秒');Ylabel('北向速度Vy 米/秒');title('北向速度变化曲线图'); figureplot(t,Vz)% > 绘制垂直速度变化曲线grid onXlabel('时间/秒');Ylabel('垂直速度Vz 米/秒');title('垂直速度变化曲线图');。