惯性导航作业一、数据说明：1：惯导系统为指北方位的捷连系统。

初始经度为116.344695283度、纬度为39.975172度,高度h为30米。

初速度v0=[-9.993908270;0.000000000;0.348994967]。

2：jlfw中为600秒的数据，陀螺仪和加速度计采样周期分别为为1/100秒和1/100秒。

3：初始姿态角为[2 1 90]（俯仰，横滚，航向,单位为度）,jlfw.mat中保存的为比力信息f_INSc(单位m/s^2)、陀螺仪角速率信息wib_INSc(单位rad/s)，排列顺序为一~三行分别为X、Y、Z向信息.4: 航向角以逆时针为正。

5:地球椭球长半径re=6378245;地球自转角速度wie=7.292115147e-5;重力加速度g=g0*(1+gk1*c33^2)*(1-2*h/re)/sqrt(1-gk2*c33^2)；g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin(lat纬度);二、作业要求：1：可使用MATLAB语言编程，用MATLAB编程时可使用如下形式的语句读取数据：load D:\...文件路径...\jlfw,便可得到比力信息和陀螺仪角速率信息。

用角增量法。

2：(1) 以系统经度为横轴，纬度为纵轴（单位均要转换为：度）做出系统位置曲线图；(2) 做出系统东向速度和北向速度随时间变化曲线图（速度单位：m/s，时间单位：s）；(3) 分别做出系统姿态角随时间变化曲线图（俯仰，横滚，航向,单位转换为：度，时间单位：s）；以上结果均要附在作业报告中。

3：在作业报告中要写出“程序流程图、现阶段学习小结”，写明联系方式。

(注意程序流程图不是课本上的惯导解算流程，而是你程序分为哪几个模块、是怎样一步步执行的,什么位置循环等,让别人根据该流程图能够编出相应程序) (学习小结按条写，不用写套话) 4：作业以纸质报告形式提交，附源程序。

三、基本原理和公式1、初始姿态矩阵的确定：根据初始姿态角求四元数：0123cos cos cos sin sin sin 222222cossin cos sin cos sin 222222cos cos sin sin sin cos 222222cossin sin sin cos cos 222222abab abab ab ab abab q q q q ψψθγθγψψθγθγψψθγθγψψθγθγ=-=-=+=+再根据四元数求方向余弦矩阵的初始矩阵：()()()()()()222201231203130222221203012323012222130223010123222222b t q q q q q q q q q q q q C q q q q q q q q q q q q q q q q q q q q q q q q ⎡⎤+--+-⎢⎥=--+-+⎢⎥⎢⎥+---+⎣⎦2、指北方位系统的运动解算：“平台”指令角速度为：()()cos sin tan()ty yt tt x it ie xt tx ie xt V R V L R V L L R ωωω⎡⎤-⎢⎥⎢⎥⎢⎥⎢⎥=+⎢⎥⎢⎥⎢⎥+⎢⎥⎣⎦加速度计获得的比力信息bib f 为载体坐标系中各个轴向的比力，而我们需要的比力t it f 为地理坐标系中各个轴向的比力，它们之间应用矩阵t b C 做变换：()()1t t bit b ibT t b b bttf C f C CC -=⨯==根据比力信息可以求出各个方向上的加速度：()()()()(2sin tan())(2cos )(2sin tan())(2cos )t t t t tt x x x ibxie y ie zxt xt t t y tt tt x y ibyie x zxt ytt ty t tt t x z ibz ie x y xt ytV V V fL L V L V R R V V V fL L V V R R V V V f L V V gR R ωωωω•••=++-+=-++=+++-因此可以求得速度为：t tt t xx x t t t t y y y V V dt V V V dt V ••=+=+⎰⎰载体所在位置的地理纬度L 、经度λ可由下列方程求得：000)sec(λλ+=+=⎰⎰dt L R V L dt R V L txtxttytyt3、四元数姿态矩阵的更新：b b b t tb ib t it C ωωω=-式中，bib ω为陀螺所测得的角速度。

用毕卡逼近法更新b t C 的值，T 为采样时间bib T θω∆=⨯[]0000x y z xz y y z x z yxθθθθθθθθθθθθθ-∆-∆∆⎡⎤⎢⎥∆∆-∆⎢⎥∆=⎢⎥∆-∆∆⎢⎥∆∆-∆⎢⎥⎣⎦ 22220x y z θθθθ∆=∆+∆+∆()()()()[]()2420001118384248q n I q n θθθθ⎧⎫⎛⎫⎛⎫∆∆∆⎪⎪+=-++-∆ ⎪ ⎪⎨⎬ ⎪ ⎪⎪⎪⎝⎭⎝⎭⎩⎭4、姿态角的求解：姿态角与姿态矩阵的关系：cos cos sin sin sin cos sin sin sin cos sin cos cos sin cos cos sin sin cos cos sin sin sin sin cos sin cos cos cos ab abab abb t ababab abab abC γψγθψγψγθψγθθψθψθγψγθψγψγθψγθ-+-⎡⎤⎢⎥=-⎢⎥⎢⎥+--⎣⎦式中θ,γ,ab ψ分别为俯仰角，滚转角和偏航角，以逆时针为正方向，而课本上是以顺时针为正，因此需要对课本上的公式进行修改，将ab ψ-代入原公式可得现公式。

如果记111213212223313233b t T T T C T T T T T T ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦则由以上两式即可解算出姿态角：()1231133312122sin tan tan T T T T T θγϕ---=⎛⎫=- ⎪⎝⎭⎛⎫=⎪⎝⎭四、程序流程图五、结果六、小结这次作业是捷联惯导的解算，是利用上次作业的结果对数据进行处理。

和上次不同，这次遇到了较多的问题。

首先，对捷联惯导的基本原理理解的不够深刻，比如坐标系的转换，四元数微分方程的求解。

其次，由于课本的姿态角是以顺时针为正，而原始数据是以逆时针为正，因此需要对书上的公式进行修改，在这个过程中就出现了许多问题，比如正负号问题。

总之，这次作业弥补了学习上的不足，使我对基本原理理解更为深刻，也初步了解惯导的基本操作。

七、程序clccleara=load('C:\Users\Administrator\Documents\MATLAB/jlfw.dat');Wib_INSc=a(:,2:4)';f_INSc=a(:,5:7)';%第一列：数据包序号第二至四列：分别为东、北、天向陀螺仪角速率信息wib_INSc（单位：rad/s）%第五至七列：分别为东、北、天向比力信息f_INSc（单位：m/s^2）.L(1,:)=zeros(1,60001);Lambda(1,:)=zeros(1,60001);Vx(1,:)=zeros(1,60001);Vy(1,:)=zeros(1,60001);Vz(1,:)=zeros(1,60001);Rx(1,:)=zeros(1,60001);%定义存放卯酉圈曲率半径数据的矩阵Ry(1,:)=zeros(1,60001);%定义存放子午圈曲率半径数据的矩阵psi(1,:)=zeros(1,60001);%定义存放偏航角数据的矩阵theta(1,:)=zeros(1,60001);%定义存放俯仰角数据的矩阵gamma(1,:)=zeros(1,60001);%定义存放滚转角数据的矩阵I=eye(4); %定义四阶矩阵L(1,1)=39.975172/180*pi;%纬度初始值单位：弧度Lambda(1,1)=116.344695283/180*pi;%经度初始值单位：弧度Vx(1,1)=-9.993908270;%初始速度x方向分量Vy(1,1)=0;%初始速度y方向分量Vz(1,1)=0.348994967;%初始速度z方向分量Wibx(1,:)=a(:,2); %提取陀螺正东方向角速率并定义Wiby(1,:)=a(:,3); %提取陀螺正北方向角速率并定义Wibz(1,:)=a(:,4); %提取陀螺天向角速率并定义fibbx(1,:)=a(:,5);%x方向的比力数据fibby(1,:)=a(:,6);%y方向的比力数据fibbz(1,:)=a(:,7);%z方向的比力数据g0=9.7803267714;Wie=7.292115147e-5;%地球自转角速度Re=6378245;%长半径e=1/298.3;%椭圆度t=0.01;%采样时间psi(1,1)=90/180*pi;%偏航角初始值单位：弧度theta(1,1)=2/180*pi;%俯仰角初始值单位：弧度gamma(1,1)=1/180*pi;%滚转角初始值单位：弧度gk1=0.00193185138639;gk2=0.00669437999013;H=30;%高度%求解四元数系数q0,q1,q2,q3的初值q(1,1)=cos(psi(1,1)/2)*cos(theta(1,1)/2)*cos(gamma(1,1)/2)-sin(psi(1,1)/2)*sin(theta( 1,1)/2)*sin(gamma(1,1)/2); %q0q(2,1)=cos(psi(1,1)/2)*sin(theta(1,1)/2)*cos(gamma(1,1)/2)-sin(psi(1,1)/2)*cos(theta( 1,1)/2)*sin(gamma(1,1)/2); %q1q(3,1)=cos(psi(1,1)/2)*cos(theta(1,1)/2)*sin(gamma(1,1)/2)+sin(psi(1,1)/2)*sin(theta( 1,1)/2)*cos(gamma(1,1)/2); %q2q(4,1)=cos(psi(1,1)/2)*sin(theta(1,1)/2)*sin(gamma(1,1)/2)+sin(psi(1,1)/2)*cos(theta( 1,1)/2)*cos(gamma(1,1)/2); %q3for i=1:60000g=g0*(1+gk1*sin(L(i)^2)*(1-2*H/Re)/sqrt(1-gk2*sin(L(i)^2)));%计算重力加速度Rx(i)=Re/(1-e*(sin(L(i)))^2);%根据纬度计算卯酉圈曲率半径Ry(i)=Re/(1+2*e-3*e*(sin(L(i)))^2);%根据纬度计算子午圈曲率半径%求解四元数姿态矩阵q0=q(1,i);q1=q(2,i);q2=q(3,i);q3=q(4,i);Ctb=[q0^2+q1^2-q2^2-q3^2,2*(q1*q2+q0*q3),2*(q1*q3-q0*q2);2*(q1*q2-q0*q3),q2^2-q3^2+q0^2-q1^2,2*(q2*q3+q0*q1);2*(q1*q3+q0*q2),2*(q2*q3-q0*q1),q3^2-q2^2-q1^2+q0^2;];Cbt=Ctb';fibt=Cbt*[fibbx(i);fibby(i);fibbz(i)];%比力数据fibtx(i)=fibt(1,1);fibty(i)=fibt(2,1);fibtz(i)=fibt(3,1);Vx(1,i+1)=(fibtx(i)+(2*Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i))*Vy(i)-(2*Wie*cos(L(i)) +Vx(i)/Rx(i))*Vz(i))*t+Vx(i);%计算速度x方向分量Vy(1,i+1)=(fibty(i)-(2*Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i))*Vx(i)+Vy(i)*Vz(i)/Ry(i) )*t+Vy(i);%计算速度y方向分量Vz(1,i+1)=(fibtz(i)+(2*Wie*cos(L(i)+Vx(i))/Rx(i))*Vx(i)+Vy(i)*Vy(i)/Ry(i)-g)*t+Vz (i);%计算速度z方向分量Witt=[-Vy(i)/Ry(i);Wie*cos(L(i))+Vx(i)/Rx(i);Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i)];%求出平台指令角速度值Wibb=[Wibx(i);Wiby(i);Wibz(i)];Wtbb=Wibb-Ctb*Witt;%将指令角速度转换到平台坐标系,并求解WtbbL(1,i+1)=t*Vy(i)/Ry(i)+L(i);Lambda(1,i+1)=t*Vx(i)/(Rx(i)*cos(L(i)))+ Lambda(i);x=Wtbb(1,1)*t;y=Wtbb(2,1)*t;z=Wtbb(3,1)*t; %求取迭代矩阵中的各ΔthetaA=[0 -x -y -z;x 0 z -y;y -z 0 x;z y -x 0];%求取迭代矩阵[Δtheta]T=x^2+y^2+z^2; % 计算[Δtheta]^2的q(:,i+1)=((1-T/8+T^2/384)*I+(1/2-T/48)*A)*q(:,i);%求q0theta(i+1)=asin(Ctb(2,3));if(Ctb(2,2)>=0)psi(i+1)=atan(-Ctb(2,1)/Ctb(2,2));elseif(Ctb(2,1)>0)psi(i+1)=atan(-Ctb(2,1)/Ctb(2,2))+pi;elsepsi(i+1)=atan(-Ctb(2,1)/Ctb(2,2))-pi;endif(Ctb(3,3)>0)gamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3));elseif(Ctb(1,3)<0)gamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3))+pi;elsegamma(i+1)=atan(-Ctb(1,3)/Ctb(3,3))-pi;endendfigure(1)plot(L*180/pi,Lambda*180/pi);title('经纬度位置曲线');xlabel('经度/°');ylabel('纬度/°');grid ont=0:0.01:600;figure(2)plot(t,Vx);title('东西方向速度');xlabel('时间/s');ylabel('速度/m/s');grid on figure(3)plot(t,Vy);title('南北方向速度');xlabel('时间/s');ylabel('速度/m/s');grid on figure(4)plot(t,theta*180/pi);title('俯仰角');xlabel('时间/s');ylabel('度');grid on figure(5)plot(t,gamma*180/pi);title('滚转角');xlabel('时间/s');ylabel('度');grid on figure(6)plot(t,psi*180/pi);title('偏航角');xlabel('时间/s');ylabel('度');grid on。