导航原理作业（惯性导航部分）
一、题目内容。
一枚导弹采用捷联惯性导航系统，三个速率陀螺仪gx, gy, gz和三个加速度计 ax, ay, az的敏感轴分别沿着着弹体坐标系的xb, yb, zb轴。初始时刻该导弹处在北纬45.75度，东经126.
63度。
第一种情形：正对导弹进行地面静态测试（导弹质心相对地面静止）。
初始时刻弹体坐标系和地理坐标系重合，如图所示，弹体的xb轴指东，yb轴指北，zb轴指天。此后弹体坐标系xb-yb-zb相对地理坐标系的转动如下：
首先，弹体绕zb（方位轴）转过 -10 度；
接着，弹体绕xb（俯仰轴）转过 15 度；
然后，弹体绕yb（滚动轴）转过 20 度；
最后弹体相对地面停止旋转。
请分别用方向余弦矩阵和四元数两种方法计算：弹体经过三次旋转并停止之后，弹体上三个加速度计ax, ay, az的输出。取重力加速度的大小g = 9.8m/s2。
第二种情形：导弹正在飞行中。
初始时刻弹体坐标系仍和地理坐标系重合；且导弹初始高度200m，初始北向速度 1800 m/s，初始东向速度和垂直速度都为零。
陀螺仪和加速度计的输出都为脉冲数形式，陀螺输出的每个脉冲代表 0.00001弧度的角增量。加速度计输出的每个脉冲代表1μg，1g = 9.
8m/s2。陀螺仪和加速度计输出的采样频率都为10hz，在200秒内三个陀螺仪和三个加速度计的输出存在了数据文件中，内含一矩阵变量ga，有2000行，6列。每一行中的数据代表每个采样时刻三个陀螺gx, gy, gz和三个加速度计ax, ay, az的输出的脉冲数。
格式如下表（前10行）
将地球视为理想的球体，半径 6371.00公里，且不考虑仪表误差，也不考虑弹体高度对重力加速度的影响。选取弹体的姿态计算周期为0.1秒，速度和位置的计算周期为1秒。
1） 请计算200秒后弹体到达的经纬度和高度，东向和北向速度；
2） 请计算200秒后弹体相对当地地理坐标系的姿态四元数；
3） 请绘制出200秒内导弹的经、纬度变化曲线（以经度为横轴，纬度为纵轴）；
4） 请绘制出200秒内导弹的高度变化曲线（以时间为横轴，高度为纵轴）。
二、第一种情形解答。
2.1方向余弦阵法。
1） 先绕轴转过。
2） 再绕轴转过。
3） 再绕轴转过。
所以变换后的坐标。
由于初始时刻有。
所以计算得。
弹体上三个加速度计的输出分别是，
计算程序见附录一。
2.2四元数法。
1）先绕轴转过。
2）再绕轴转过。
3）再绕轴转过。
则合成四元数。
合成四元数的逆。
计算得。由及。得。其中。
所以计算得。
弹体上三个加速度计的输出分别是，
由两种计算方法的计算结果可以看出，方向余弦阵法和四元数法的计算结果是一致的。
计算程序见附录二。
三、第二种情形解答。
3.1 程序流程图。
3.2 matlab程序见附录三。
3.3 程序运算结果及**图。
在200秒后，弹体到达的经度为128.149度，到达的纬度为48.897度，导弹所到达的高度为84.
504米，200秒时的东向速度为1123.96米每秒，北向速度为1568.016米每秒。
200秒后弹体相对当地地理坐标系的姿态四元数为：
导弹飞行过程中的经纬度变化曲线如图1所示：
图1 导弹飞行过程中经度纬度变化曲线。
导弹飞行过程中高度变化曲线如图2：
图2 导弹飞行过程中高度变化曲线。
3.4 程序**中遇到的问题及体会。
在按照程序流程图编程时，由于把某些公式的正负号写错，导致在程序**时出现了图3的结果。
图3 **遇到的问题。
在程序**中遇到的另一个问题是在计算四元数阵时，把四元数的维数弄错，导致程序运行出错。
在编写程序时，四元数的初始化非常重要，如果四元数的初始化错误，会导致所有的运算结果错误。某些子函数的调用实参与虚参要一致，子函数的返回值要符合需要的格式。
同时，这次编程，让我学会了很多以前没用到的matlab函数，在编程过程中，每当遇到问题，我就会上网查一些资料，学习并解决问题。这在很大程度上提高了我的自学能力。
附录一。c1=[cos(-10/180*pi) sin(-10/180*pi) 0
-sin(-10/180*pi) cos(-10/180*pi) 0
0 0 1]; 第一次转动方向余弦阵。
c2=[1 0 0
0 cos(15/180*pi) sin(15/180*pi)
0 -sin(15/180*pi) cos(15/180*pi)];
第二次转动方向余弦阵。
c3=[cos(20/180*pi) 0 -sin(20/180*pi)
sin(20/180*pi) 0 cos(20/180*pi)];
第三次转动方向余弦阵。
a=c3*c2*c1*[0;0;9.8] %计算转动后弹体上加速度的输出。
norm(a) %检验输出是否正确。
附录二。q1=[cos(-10/360*pi);0;0;sin(-10/360*pi)];第一次转动四元数。
q2=[cos(15/360*pi);sin(15/360*pi);0;0];%第二次转动四元数。
q3=[cos(20/360*pi);0;sin(20/360*pi);0];%第三次转动四元数。
r=quml(q1,q2);%计算四元数乘积。
q=quml(r,q3);
a1=[q(1) q(2) q(3) q(4)
-q(2) q(1) q(4) -q(3)
-q(3) -q(4) q(1) q(2)
-q(4) q(3) -q(2) q(1)]
a2=[q(1) -q(2) -q(3) -q(4)
q(2) q(1) q(4) -q(3)
q(3) -q(4) q(1) q(2)
q(4) q(3) -q(2) q(1)]
a=a1*a2;%
g=a*[0;0;0;9.8] %计算转动后的弹体加速度计的输出。
附录三。m文件名。
clc;clear;
---已知参数。
k=10;t=1;
k=200;
r=6371000;
we=15/180*pi/3600;
初始导航参数和地球参数。
q=zeros(4,201); 定义变换四元数矩阵。
lamda = zeros(1,201);%定义长度为200的经度数组。
fai=zeros(1,201); 定义长度为200的纬度数组。
h=zeros(1,201定义长度为200的高度数组。
q(:,1)=[1;0;0;0]; 初始化四元数。
lamda(1)=126.63; %初始化经度。
fai(1)=45.75;%初始化纬度。
h(1)=200; %初始化高度。
g=9.8;
定义速度矩阵，供画图用。
ve_matrix = zeros(1,201); 东向速度。
vn_matrix = zeros(1,201); 北向速度。
vu_matrix = zeros(1,201); 天向速度。
ve_matrix(1)=0;
vn_matrix(1)=1800;
vu_matrix(1)=0;
定义加速度矩阵。
fe_matrix = zeros(1,201); 东向加速度。
fn_matrix = zeros(1,201); 北向加速度。
fu_matrix = zeros(1,201); 天向加速度。
for n=1:k %位置、速度迭代。
q1=zeros(4,11);
q1(:,1)=q(:,n);
for n=1:k %姿态迭代。
wx=0.00001*ga((n-1)*10+n,1);
wy=0.00001*ga((n-1)*10+n,2);
wz=0.00001*ga((n-1)*10+n,3);
w=[wx,wy,wz]';
normw=norm(w);
w=[0,-w(1),-w(2),-w(3);
w(1),0,w(3),-w(2);
w(2),-w(3),0,w(1);
w(3),w(2),-w(1),0];
i=eye(4);
%三阶近似。
s=1/2-normw^2/48;
c=1-normw^2/8;
q1(:,n+1)=(c*i+s*w)*q1(:,n);
endq(:,n+1)=q1(:,n+1);
we=-vn_matrix(n)/r; %地理坐标系的转动角速度分量。
wn=ve_matrix(n)/r+we*cos(fai(n)/180*pi);
wh=ve_matrix(n)/r*tan(fai(n)/180*pi)+we*sin(fai(n)/180*pi);
gama=[we,wn,wh]'*t; %用地理坐标系的转动四元数。
normgama=norm(gama); 修正载体姿态四元数。
n=gama/normgama;
qg=[cos(normgama/2);sin(normgama/2)*n];
q(:,n+1)=quml(qiuni(qg),q(:,n+1));
fx=1e-6*9.8*mean(ga((n-1)*10+1:n*10,4));加速度计测得的比力。
组合导航大作业
第11组赵魏郭泽宇李晓龙。一 题目。雷达追踪目标的滤波算法 观测变量 距离信息和角度信息 雷达是追踪和计算目标位置的重要设备，可以提供目标的相对距离和相对角度信息，如下图所示 假设在当地水平坐标系上 x y z 分别指向北向 天向和东向 描述目标相对于雷达的位置和速度信息。雷达测量的视距 方位角 高...
北航惯性导航大作业
惯性导航基础课程大作业报告 一 光纤陀螺误差建模与分析。班级 111514 姓名 学号。2014年5月26日。一。系统误差原理图。二。系统误差的分析。一 漂移引起的系统误差。1.x，y，z对东向速度误差 vx的影响。clc clear all t 1 0.01 25 g 9.8 l pi 180 3...
北航惯性导航期末大作业
惯性导航期末大作业。14171106苗家语。1.平台式惯导系统，求导航坐标系n系与计算机坐标系c系的区别和转换矩阵，设c系相对n系的误差角为。答 n系与c系误差角为 三个欧拉角矩阵相乘，近似得到转换矩阵 2.以四元数相乘的法则计算两个矢量p和q的四元数相乘的结果。答 矢量p 矢量q p qp q p...