(完整)北航惯性导航作业二.

(完整)北航惯性导航作业二.编辑整理：尊敬的读者朋友们：这里是精品文档编辑中心，本文档内容是由我和我的同事精心编辑整理后发布的，发布之前我们对文中内容进行仔细校对，但是难免会有疏漏的地方，但是任然希望（(完整)北航惯性导航作业二.）的内容能够给您的工作和学习带来便利。

同时也真诚的希望收到您的建议和反馈，这将是我们进步的源泉，前进的动力。

本文可编辑可修改，如果觉得对您有帮助请收藏以便随时查阅，最后祝您生活愉快业绩进步，以下为(完整)北航惯性导航作业二.的全部内容。

惯性导航作业一、数据说明：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、初始姿态矩阵的确定:根据初始姿态角求四元数：再根据四元数求方向余弦矩阵的初始矩阵：2、指北方位系统的运动解算:“平台"指令角速度为:加速度计获得的比力信息为载体坐标系中各个轴向的比力，而我们需要的比力为地理坐标系中各个轴向的比力，它们之间应用矩阵做变换：根据比力信息可以求出各个方向上的加速度：0123coscos cos sin sin sin 222222cos sin cos sin cos sin222222cos cos sin sin sin cos222222cos sin sin sin cos cos222222ab ab ab ab ab ab ab ab 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 ⎡⎤+--+-⎢⎥=--+-+⎢⎥⎢⎥+---+⎣⎦()()co s sin tan ()ty yt tt xit ie xt tx ie xt V R V L R V L L R ωωω⎡⎤-⎢⎥⎢⎥⎢⎥⎢⎥=+⎢⎥⎢⎥⎢⎥+⎢⎥⎣⎦bib f titf tb C ()()1t t bit b ibT t b b bttf C f C C C -=⨯==因此可以求得速度为：载体所在位置的地理纬度L 、经度可由下列方程求得：3、四元数姿态矩阵的更新：式中，为陀螺所测得的角速度.用毕卡逼近法更新的值,T 为采样时间4、姿态角的求解：姿态角与姿态矩阵的关系:()()()()(2s in ta n ())(2c o s )(2s in ta n ())(2c o s )t t t t t t x xx ib xie y ie zx t x t t ty t tt t xy ib yie x zx t y tt ty t tt t xzib zie x y x t y tV V V f L L V L V R R V V V f L L V V R R V V V f L V V gR R ωωωω•••=++-+=-++=+++-0t t t t xx x t tt t y y y V V dt V V V dt V ••=+=+⎰⎰λ0000)sec(λλ+=+=⎰⎰dt L R V L dt RV L txt xttytytb b b t t b i b t i t C ωωω=-bib ωbt C bi b T θω∆=⨯[]0000x y z x z y y z x z y x θθθθθθθθθθθθθ-∆-∆∆⎡⎤⎢⎥∆∆-∆⎢⎥∆=⎢⎥∆-∆∆⎢⎥∆∆-∆⎢⎥⎣⎦22220x y z θθθθ∆=∆+∆+∆()()()()[]()2420001118384248q n I q n θθθθ⎧⎫⎛⎫⎛⎫∆∆∆⎪⎪+=-++-∆ ⎪ ⎪⎨⎬ ⎪ ⎪⎪⎪⎝⎭⎝⎭⎩⎭(完整)北航惯性导航作业二.式中，,分别为俯仰角，滚转角和偏航角，以逆时针为正方向，而课本上是以顺时针为正，因此需要对课本上的公式进行修改,将代入原公式可得现公式。

如果记则由以上两式即可解算出姿态角：四、程序流程图c o s c o s s i n s i n s i n c o s s i n s i n s i n c o s s i n c o s c o s s i n c o s c o s s i n s i n c o s c o s s i n s i n s i n s i n c o s s i n c o s c o s c o s a b a b a b a b b t a b a b a b a b a b a b C γψγθψγψγθψγθθψθψθγψγθψγψγθψγθ-+-⎡⎤⎢⎥=-⎢⎥⎢⎥+--⎣⎦θγab ψab ψ-111213212223313233b t T T T C T T T T T T ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦()1231133312122s i n t a n t a n 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。