北航惯性导航期末大作业
北航惯性导航综合实验四实验报告
基于运动规划的惯性导航系统动态实验二零一三年六月十日实验4.1 惯性导航系统运动轨迹规划与设计实验一、实验目的为进行动态下简化惯性导航算法的实验研究，进行路径和运动状态规划，以验证不同运动状态下惯导系统的性能。
通过实验掌握步进电机控制方法，并产生不同运动路径和运动状态。
二、实验内容学习利用6045B 控制板对步进电机进行控制的方法，并控制电机使运动滑轨产生定长运动和不同加速度下的定长运动。
三、实验系统组成USB_PCL6045B 控制板（评估板）、运动滑轨和控制计算机组成。
四、实验原理IMU安装误差系数的计算方法USB_PCL6045B 控制板采用了USB 串行总线接口通信方式，不必拆卸计算机箱就可以在台式机或笔记本电脑上进行运动控制芯片PCL6045B 的学习和评估。
USB_PCL6045B 评估板采用USB 串行总线方式实现评估板同计算机的数据交换，由评估板的FIFO 控制回路完成步进电机以及伺服电机的高速脉冲控制，任意 2 轴的圆弧插补，2-4 轴的直线插补等运动控制功能。
USB_PCL6045B 评估板上配置了全部PCL6045B 芯片的外部信号接口和增量编码器信号输入接口。
由USB_PCL6045B 评估测试软件可以进行PCL6045B 芯片的主要功能的评估测试。
图4-1-1USB_PCL6045B 评估板原理框图如图4-1-1 所示，CN11 接口主要用于外部电源连接，可以选择DC5V 单一电源或DC5V/24V 电源。
CN12 接口是USB 信号接口，用于USB_PCL6045B 评估板同计算机的数据交换。
USB_PCL6045B 评估板已经完成对PCL6045B 芯片的底层程序开发和硬件资源与端口的驱动，并封装成156 个API 接口函数。
用户可直接在VC 环境下利用API 接口函数进行编程。
五、实验内容1、操作步骤1）检查电机驱动电源（24V）2）检查USB_PCL6045B 控制板与上位机及电机驱动器间的连接电缆3）启动USB_PCL6045B 控制板评估测试系统检查系统是否正常工作。
北航最优化方法有关大作业参考
1流量工程问题重述定一个有向网 G=(N,E) ，此中 N 是点集， E 是弧集。
令 A 是网 G 的点弧关矩，即 N×E 矩，且第 l 列与弧里 (I,j) ，第 i 行元素 1 ，第 j 行元素 -1 ，其他元素 0。
再令b m=(b m1 ,⋯,b mN )T，f m =(f m1,⋯ ,f mE )T，可将等式束表示成：Af m=b m本算例一典 TE 算例。
算例网有 7 个点和 13 条弧，每条弧的容量是 5 个位。
别的有四个需求量均 4 个位的源一目的，详细的源点、目的点信息如所示。
里了，省区了未用到的弧。
别的，弧上的数字表示弧的号。
此，c=((5,5 ⋯,5) 1 )T，×13依据上述四个束条件，分求得四个状况下的最决议量x=((x 12 ,x13,⋯ ,x75)1×13 )。
1 网拓扑和流量需求7 节点算例求解算例1（b1=[4;-4;0;0;0;0;0]T）转变为线性规划问题：Minimize c T x1Subject to Ax1=b1x1>=0 利用 Matlab 编写对偶纯真形法程序，可求得:最优解为 x1*=[4 0 0 0 0 0 0 0 0 0 0 0 0] T对应的最优值 c T x1=201.2.2 算例 2（b2=[4;0;-4;0;0;0;0] T）Minimize c T x2Subject to Ax2=b2X2>=0 利用 Matlab 编写对偶纯真形法程序，可求得:最优解为 x2*=[0 4 0 0 0 0 0 0 0 0 0 0 0]T对应的最优值 c T x2=201.2.3 算例 3（b3=[0;-4;4;0;0;0;0] T）MinimizeTc x3Subject to Ax3=b3X3>=0 利用 Matlab 编写对偶纯真形法程序，可求得:最优解为 x3*=[4 0 0 0 4 0 0 0 0 0 0 0 0] T对应的最优值 c T x3=40算例4（b4=[4;0;0;0;0;0;-4]T ）Minimize c T x4Subject to Ax4=b4X4>=0利用 Matlab 编写对偶纯真形法程序，可求得:最优解为 x4*=[4 0 0 4 0 0 0 0 0 4 0 0 0] T对应的最优值 c T x4=601.3 计算结果及结果说明算例1（b1=[4;-4;0;0;0;0;0]T）算例 1 中，由 b1 可知，节点 2 为需求节点，节点 1 为供应节点，由节点 1 将信息传输至节点 2 的最短路径为弧 1。
(完整)北航惯性导航作业二.
(完整)北航惯性导航作业二.编辑整理：尊敬的读者朋友们：这里是精品文档编辑中心，本文档内容是由我和我的同事精心编辑整理后发布的，发布之前我们对文中内容进行仔细校对，但是难免会有疏漏的地方，但是任然希望（(完整)北航惯性导航作业二.）的内容能够给您的工作和学习带来便利。
同时也真诚的希望收到您的建议和反馈，这将是我们进步的源泉，前进的动力。
本文可编辑可修改，如果觉得对您有帮助请收藏以便随时查阅，最后祝您生活愉快业绩进步，以下为(完整)北航惯性导航作业二.的全部内容。
惯性导航作业一、数据说明：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：在作业报告中要写出“程序流程图、现阶段学习小结”,写明联系方式。
北航惯性导航综合实验五实验报告
惯性导航技能概括真验之阳早格格创做真验五惯性基拉拢导航及应用技能真验惯性/卫星拉拢导航系统车载真验一、真验手段①掌握捷联惯导/GPS拉拢导航系统的形成战基础处事本理；②掌握采与卡我曼滤波要领举止捷联惯导/GPS拉拢的基根源基本理；③掌握捷联惯导 /GPS拉拢导航系统固态本能；④掌握动背情况下捷联惯导 /GPS拉拢导航系统的本能.两、真验真质①复习卡我曼滤波的基根源基本理（参照《卡我曼滤波与拉拢导航本理》第两、五章）；②复习捷联惯导/GPS拉拢导航系统的基础处事本理（参照以光衢编著的《惯性导航本理》第七章）；三、真验系统组成①捷联惯导/GPS拉拢导航真验系统一套；②监控预计机一台.③好分GPS交支机一套；④真验车一辆；⑤车载大理石仄台；⑥车载电源系统.四、真验真质1）真验准备①将IMU紧固正在车载大理石减振仄台上，确认IMU 的拆置基准里紧靠真验仄台；②将IMU与导航预计机、导航预计机与车载电源、导航预计机与监控预计机、GPS交支机与导航预计机、GPS 天线与GPS交支机、GPS交支机与GPS电池之间的连交线细确连交；③挨开GPS交支机电源，确认不妨交支到4颗以上卫星；④挨开电源，开用真验系统.2）捷联惯导/GPS拉拢导航真验①加进捷联惯导初初对于准状态，记录IMU的本初输出，注意5分钟内宽禁移动真验车战IMU；②真验系统通过5分钟初初对于准之后，加进导航状态；③移动真验车，按安排真验门路止驶；④利用监控预计机中的导航硬件举止导航解算，并隐现导航截止.五、真验截止及分解(一) 表里推导捷联惯导短时段（1分钟）位子缺点，并用1分钟惯导真验数据考证.1、一分钟惯导位子缺点表里推导：短时段内（t<5min ），忽略天球自转0ie ω=，疏通轨迹近似为仄里1/0R =，此时的位子缺点分解可简化为：（1） 加速度计整偏偏∇引起的位子缺点：210.88022t x δ∇==m（2） 得准角0φ引起的缺点：202 0.92182g t x φδ==m（3） 陀螺漂移ε引起的缺点：330.01376g t x εδ==m可得1min 后的位子缺点值123 1.8157m x x x x δδδδ=++=2、一分钟惯导真验数据考证截止：（1）杂惯导解算1min 的位子及位子缺点图：（2）杂惯导解算1min 的速度及速度缺点图：真验截止分解：杂惯导解算短时间内细度很下，1min 的惯导解算的北背最大位移缺点-2.668m ，东背最大位移缺点-8.231m ，可睹真验数据所得位子缺点与表里推导的位子缺点正在共一数量级，截止没有真足相共是果为表里推导时干了洪量简化，而且真验时视GPS 为真正在值也会戴去缺点；其余，可睹1min 内杂惯导解算的东背速度最大缺点-0.2754m/s ，北背速度最大缺点-0.08027m/s.(二) 采用IMU 前5分钟数据举止对于准真验.将初初对于准截止动做初值完毕1小时捷联惯导战拉拢导航解算，对于比1小时捷联惯导战拉拢导航截止.1、5minIMU数据的剖析细对于准截止：2、5minIMU数据的Kalman滤波细对于准截止：3、一小时IMU/GPS数据的拉拢导航截止图及预计圆好P 阵图：4、一小时IMU数据的捷联惯导解算截止与拉拢滤波、GPS 输出对于比图：5、截止分解：由滤波截止图不妨瞅出：（1）由拉拢后的速度、位子的P阵不妨瞅出滤波之后载体的速度战位子比GPS输出的细度下.（2）短时间内SINS的细度较下，初初阶段的导航截止基础战GPS、拉拢导航截止沉合，1小时后的捷联惯导解算截止很好，纬度、经度、下度均收集.（3）INS/GPS拉拢滤波的截止战GPS的输出截止格中近似，果为1小时的导航GPS的细度比SINS导航的细度下很多，Kalman滤波器中GPS旗号的权沉更大.（4）总体瞅去，SINS/GPS拉拢滤波的截止劣于单独用SINS或者GPS导航的截止，起到了协做、超出、冗余的效率，使导航系统更稳当.六、SINS/GPS拉拢导航步调%%%%%%%%%%%%%%%%%%%%%%%INS/GPS拉拢导航跑车1h真验%%%%%%%%%%%%%%%%%%%%%%%%%%%该步调为15维状态量，6维瞅丈量的kalman滤波步调，惯性/卫星拉拢紧耦合的数教模型clearclcclose all%%初初量定义wie = 0.000072921151467;Re = 6378135.072;g = 9.7803267714;e = 1.0 / 298.25;T = 0.01; %IMU频次100hz,此步调中GPS频次100hzdatanumber = 360000; %数据时间3600sa = load('imu_1h.dat');w = a(:,3:5)'*pi/180/3600; %陀螺仪输出的角速率疑息单位由°/h化为rad/sf = a(:,6:8)'; %三轴比较输出，单位ga = load('gps_1h_new.dat');gps_pos = a(:,3:5); %GPS输出的纬度、经度、下度疑息gps_pos(:,1:2) = gps_pos(:,1:2)*pi/180; %纬经单位化为弧度gps_v = a(:,6:8); %GPS输出的东北天速度疑息%捷联解算及卡我曼相闭v=zeros(datanumber,3); %拉拢后的速度疑息atti = zeros(datanumber,3); %拉拢后的姿态疑息pos = zeros(datanumber,3); %拉拢后的位子疑息gyro=zeros(3,1);acc=zeros(3,1);x_kf = zeros(datanumber,15);p_kf = zeros(datanumber,15);lon =116.3703629769560*pi/180;height =43.0674;fai = 219.9744642380873*pi/180;theta = -0.895865732956914*pi/180;gama = 0.640089448357591*pi/180;Vx=gps_v(1,1);Vy=gps_v(1,2);Vz=gps_v(1,3);X_o=zeros(15,1); %X的初值选为0X=zeros(15,1);%Q=diag([(50e-6*g)^2,(50e-6*g)^2,(50e-6*g)^2,(0.1*pi/180/3600)^2,(0.1*pi/180/3600)^2,(0.1*pi/180/36 00)^2,0,0,0,0,0,0,0,0,0]); %随机Q=diag([(0.008*pi/180/3600)^2,(0.008*pi/180/3600)^2,(0.008*pi/180/3600)^2,(50e-6*g)^2,(50e-6*g)^2,(50e-6*g)^2,0,0,0,0,0,0,0,0,0]);R=diag([(0.01)^2,(0.01)^2,(0.01)^2,(0.1)^2,(0.1)^2,(0.15)^2]); P=zeros(15);P_k=diag([(0.00005*pi/180)^2,(0.00005*pi/180)^2,(0.00005*pi /180)^2,0.00005^2,0.00005^2,0.00005^2,2^2,2^2,2^2,(0.001*pi /180/3600)^2,(0.001*pi/180/3600)^2,(0.001*pi/180/3600)^2,(50 e-6*g)^2,(50e-6*g)^2,(50e-6*g)^2]); %K=zeros(15,6);Z=zeros(6,1);I=eye(15);Cnb = [ cos(gama)*cos(fai)-sin(gama)*sin(theta)*sin(fai), cos(gama)*sin(fai)+sin(gama)*sin(theta)*cos(fai), -sin(gama)*cos(theta);-cos(theta)*sin(fai), cos(theta)*cos(fai), sin(theta);sin(gama)*cos(fai)+cos(gama)*sin(theta)*sin(fai),sin(gama)*sin(fai)-cos(gama)*sin(theta)*cos(fai), cos(gama) * cos(theta)];q = [ cos(fai/2)*cos(theta/2)*cos(gama/2) -sin(fai/2)*sin(theta/2)*sin(gama/2);cos(fai/2)*sin(theta/2)*cos(gama/2) - sin(fai/2)*cos(theta/2)*sin(gama/2);cos(fai/2)*cos(theta/2)*sin(gama/2) + sin(fai/2)*sin(theta/2)*cos(gama/2);cos(fai/2)*sin(theta/2)*sin(gama/2) + sin(fai/2)*cos(theta/2)*cos(gama/2)];Cnb_s=Cnb;q_s=q;for i=1:1:datanumberRmh = Re * (1.0 - 2.0 * e + 3.0 * e * sin(lat) * sin(lat)) + height;Rnh = Re * (1.0 + e * sin(lat) * sin(lat)) + height;Wien = [ 0; wie * cos(lat); wie * sin(lat)];Wenn = [ -Vy / Rmh; Vx / Rnh; Vx * tan(lat) / Rnh];Winn = Wien + Wenn;Winb = Cnb * Winn;for j=1:3gyro(j,1) = w(j,i);acc(j,1) = f(j,i)*g; %加速度疑息，单位化为m/s^2endangle = (gyro - Winb) * T;fn = Cnb'* acc;difVx = fn(1) + (2.0 * wie * sin(lat) + Vx * tan(lat) / Rnh) * Vy;difVy = fn(2) - (2.0 * wie * sin(lat) + Vx * tan(lat) / Rnh) * Vx;difVz = fn(3) + (2.0 * wie * cos(lat) + Vx / Rnh) * Vx + Vy * Vy / Rmh -g;Vx = difVx * T + Vx;Vy = difVy * T + Vy;Vz = difVz * T + Vz;lat = lat + Vy * T / Rmh;lon = lon + Vx * T / Rnh / cos(lat);height = height + Vz * T;M = [0, -angle(1), -angle(2), -angle(3);angle(1), 0, angle(3), -angle(2);angle(2), -angle(3), 0, angle(1);angle(3), angle(2), -angle(1), 0];q = (cos(norm(angle) / 2) * eye(4) + sin(norm(angle) / 2) / norm(angle) * M) * q;q = q / norm(q);Cnb = [ q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4), 2*(q(2)*q(3)+q(1)*q(4)), 2*(q(2)*q(4)-q(1)*q(3));2*(q(2)*q(3)-q(1)*q(4)), q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4), 2*(q(3)*q(4)+q(1)*q(2));2*(q(2)*q(4)+q(1)*q(3)), 2*(q(3)*q(4)-q(1)*q(2)),q(1)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4)];Rmh = Re * (1.0 - 2.0 * e + 3.0 * e * sin(lat) * sin(lat)) +height;Rnh = Re * (1.0 + e * sin(lat) * sin(lat)) + height; %以上为杂惯导解算%%F1=[0 wie*sin(lat)+v(i,1)*tan(lat)/(Rnh) -(wie*cos(lat)+v(i,1)/(Rnh)) 0 -1/(Rmh) 0 0 0 0;-(wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)) 0 -v(i,2)/(Rmh)1/(Rnh) 0 0 -wie*sin(lat) 0 0;wie*cos(lat)+v(i,1)/(Rnh) v(i,2)/(Rmh) 0 tan(lat)/(Rnh) 0 0 wie*cos(lat)+v(i,2)*sec(lat)*sec(lat)/(Rnh) 0 0;0 -fn(3) fn(2) v(i,2)*tan(lat)/(Rmh)-v(i,3)/(Rmh)2*wie*sin(lat)+v(i,1)*tan(lat)/(Rnh) -(2*wie*cos(lat)+v(i,1)/(Rnh))(2*wie*cos(lat)*v(i,2)+v(i,1)*v(i,2)*sec(lat)*sec(lat)/(Rnh)+2*wie*sin(lat)*v(i,3)) 0 0;fn(3) 0 -fn(1) -2*(wie*sin(lat)+v(i,1)*tan(lat)/(Rnh)) -v(i,3)/(Rmh) -v(i,2)/(Rmh) -(2*wie*cos(lat)+v(i,1)*sec(lat)*sec(lat)/(Rnh))*v(i,1) 0 0;-fn(2) fn(1) 0 2*(wie*cos(lat)+v(i,1)/(Rnh)) 2*v(i,2)/(Rmh) 0 -2*wie*sin(lat)*v(i,1) 0 0;0 0 0 0 1/(Rmh) 0 0 0 0;0 0 0 sec(lat)/(Rnh) 0 0 v(i,1)*sec(lat)*tan(lat)/(Rnh) 0 0;0 0 0 0 0 1 0 0 0];G=[Cnb',zeros(3);zeros(3),Cnb';zeros(9,6)];H=[zeros(3),eye(3),zeros(3),zeros(3,6);zeros(3),zeros(3),diag([Rmh,Rnh*cos(lat),1]),zeros(3,6)];%量测阵F2=[-Cnb',zeros(3);zeros(3),Cnb';zeros(3),zeros(3)];F=[F1,F2;zeros(6,15)]; %以上为kalman滤波模型参数F = F * T; %得集化temp1 = eye(15);disF = eye(15);for j = 1:10temp1 = F * temp1 / j;disF = disF + temp1;endtemp1 = Q * T;disQ = temp1;for j = 2:11temp2 = F * temp1;temp1 = (temp2 + temp2')/j;disQ = disQ + temp1;endZ(1) = Vx - gps_v(i,1); %量丈量为杂惯导与GPS的速度好、位子好Z(2) = Vy - gps_v(i,2);Z(3) = Vz - gps_v(i,3);Z(4) = (lat - gps_pos(i,1)) * Rmh; %纬经度化为位移，单位mZ(5) = (lon - gps_pos(i,2)) * Rnh * cos(lat);Z(6) = height - gps_pos(i,3);X = disF * X_o; %kalman滤波五个公式P = disF * P_k * disF'+ disQ;K = P * H'/( H * P * H'+ R);X_k = X + K * (Z - H * X);P_k = (I - K * H) * P;x_kf(i,1) = X_k(1)/pi*180; %仄台缺点角x_kf(i,2) = X_k(2)/pi*180;x_kf(i,3) = X_k(3)/pi*180;x_kf(i,4) = X_k(4); %速度缺点x_kf(i,5) = X_k(5);x_kf(i,6) = X_k(6);x_kf(i,7) = X_k(7); %位子缺点x_kf(i,8) = X_k(8);x_kf(i,9) = X_k(9);x_kf(i,10) = X_k(10)/pi*180*3600; %陀螺随机常值漂移,单位°/hx_kf(i,11) = X_k(11)/pi*180*3600;x_kf(i,12) = X_k(12)/pi*180*3600;x_kf(i,13) = X_k(13)*10^6/g; %加计随机常值偏偏置，单位ugx_kf(i,14) = X_k(14)*10^6/g;x_kf(i,15) = X_k(15)*10^6/g;p_kf(i,1) = sqrt(abs(P_k(1,1)))/pi*180;p_kf(i,2) = sqrt(abs(P_k(2,2)))/pi*180;p_kf(i,3) = sqrt(abs(P_k(3,3)))/pi*180;p_kf(i,4) = sqrt(abs(P_k(4,4)));p_kf(i,5) = sqrt(abs(P_k(5,5)));p_kf(i,6) = sqrt(abs(P_k(6,6)));p_kf(i,7) = sqrt(abs(P_k(7,7)));p_kf(i,8) = sqrt(abs(P_k(8,8)));p_kf(i,9) = sqrt(abs(P_k(9,9)));p_kf(i,10) = sqrt(abs(P_k(10,10)))/pi*180*3600;p_kf(i,11) = sqrt(abs(P_k(11,11)))/pi*180*3600;p_kf(i,12) = sqrt(abs(P_k(12,12)))/pi*180*3600;p_kf(i,13) = sqrt(abs(P_k(13,13)))*10^6/g;p_kf(i,14) = sqrt(abs(P_k(14,14)))*10^6/g;p_kf(i,15) = sqrt(abs(P_k(15,15)))*10^6/g;Vx = Vx - X_k(4); %速度矫正Vy = Vy - X_k(5);Vz = Vz - X_k(6);v(i,:) = [Vx, Vy, Vz];lat = lat - X_k(7); %位子矫正lon = lon - X_k(8);height = height - X_k(9);pos(i,:) = [lat, lon, height];Atheta = X_k(1); %kalman滤波预计得出的得准角thetaAgama = X_k(2); %kalman滤波预计得出的得准角gamaAfai = X_k(3); %kalman滤波预计得出的得准角faiCtn = [ 1, Afai, -Agama;-Afai, 1, Atheta;Agama, -Atheta, 1];Cnb = Cnb*Ctn; %革新姿态阵fai = atan(-Cnb(2,1) / Cnb(2,2));theta = asin(Cnb(2,3));gama = atan(-Cnb(1,3) / Cnb(3,3));if (Cnb(2,2) < 0)fai = fai + pi;elseif (fai < 0)fai = fai + 2*pi;endif (Cnb(3,3) < 0)if(gama > 0)gama = gama - pi;elsegama = gama + pi;endendatti(i,:) = [fai/pi*180, theta/pi*180, gama/pi*180];q(2) = sqrt(abs(1 + Cnb(1,1) - Cnb(2,2) - Cnb(3,3))) / 2; q(3) = sqrt(abs(1 - Cnb(1,1) + Cnb(2,2) - Cnb(3,3))) / 2; q(4) = sqrt(abs(1 - Cnb(1,1) - Cnb(2,2) + Cnb(3,3))) / 2; q(1) = sqrt(abs(1 - q(2) * q(2) - q(3) * q(3) - q(4) * q(4))); if (Cnb(2,3) < Cnb(3,2))q(2) = - q(2);endif (Cnb(3,1) < Cnb(1,3))q(3) = - q(3);endif (Cnb(1,2) < Cnb(2,1))q(4) = - q(4);endX_k(1:9) = 0;X_o=X_k;iend%%%画图%%%%%%%%%%t=1:datanumber;figure(1)subplot(311);plot(t,pos(:,1)*180/pi,'r',t,gps_pos(:,1)*180/pi,'b') title('纬度');xlabel('0.01s');ylabel('°');subplot(312);plot(t,pos(:,2)*180/pi,'r',t,gps_pos(:,2)*180/pi,'b') title('经度');xlabel('0.01s');ylabel('°');subplot(313);plot(t,pos(:,3),'r',t,gps_pos(:,3),'b')title('下度');xlabel('0.01s');ylabel('m');legend('拉拢滤波','GPS')figure(2)subplot(311);plot(t,v(:,1),'r',t,gps_v(:,1),'b')title('东背速度');xlabel('0.01s');ylabel('m/s'); subplot(312);plot(t,v(:,2),'r',t,gps_v(:,2),'b')title('北背速度');xlabel('0.01s');ylabel('m/s'); subplot(313);plot(t,v(:,3),'r',t,gps_v(:,3),'b')title('天背速度');xlabel('0.01s');ylabel('m/s'); legend('拉拢滤波','GPS')figure(3)subplot(311);plot(t,atti(:,1))title('航背角');xlabel('0.01s');ylabel('°'); subplot(312);plot(t,atti(:,2))title('俯俯角');xlabel('0.01s');ylabel('°'); subplot(313);plot(t,atti(:,3))title('横滚角');xlabel('0.01s');ylabel('°');figure(4)subplot(311);plot(t,p_kf(:,1))title('P航背角缺点');xlabel('0.01s');ylabel('度');subplot(312);plot(t,p_kf(:,2))title('P俯俯角缺点');xlabel('0.01s');ylabel('度'); subplot(313);plot(t,p_kf(:,3))title('P横滚角缺点');xlabel('0.01s');ylabel('度'); figure(5)subplot(311);plot(t,p_kf(:,4))title('P东背速度缺点');xlabel('0.01s');ylabel('m/s'); subplot(312);plot(t,p_kf(:,5))title('P北背速度缺点');xlabel('0.01s');ylabel('m/s'); subplot(313);plot(t,p_kf(:,6))title('P天背速度缺点');xlabel('0.01s');ylabel('m/s'); figure(6)subplot(311);plot(t,p_kf(:,7))title('P纬度缺点');xlabel('0.01s');ylabel('度'); subplot(312);plot(t,p_kf(:,8))title('P经度缺点');xlabel('0.01s');ylabel('度'); subplot(313);plot(t,p_kf(:,9))title('P下度缺点');xlabel('0.01s');ylabel('m'); figure(7)subplot(311);plot(t,p_kf(:,10))title('P东背陀螺');xlabel('0.01s');ylabel('度/小时'); subplot(312);plot(t,p_kf(:,11))title('P北背陀螺');xlabel('0.01s');ylabel('度/小时'); subplot(313);plot(t,p_kf(:,12))title('P天背陀螺');xlabel('0.01s');ylabel('度/小时'); figure(8)subplot(311);plot(t,p_kf(:,13))title('P东背加计');xlabel('0.01s');ylabel('ug'); subplot(312);plot(t,p_kf(:,14))title('P北背加计');xlabel('0.01s');ylabel('ug'); subplot(313);plot(t,p_kf(:,15))title('P天背加计');xlabel('0.01s');ylabel('ug');。
GPS期末大作业
自主导航及在无人机的运用发展综述摘要：无人机系统是未来进行信息对抗、夺取信息优势、实施火力打击的重要手段。
”自主性”是无人机系统区别于有人机最重要的技术特征, 实现无人机系统的自主控制, 提高其智能程度, 是无人机系统的重要发展趋势。
无人机自主导航即是指无人机不依赖地面支持, 而利用机上自备的测量设备实时地确定自己的位置和速度。
无人机自主导航是无人机发展的必然趋势，无人机自主导航是保证无人机成功完成各种任务的关键技术之一。
随着机载传感器性能的不断提高，利用多传感器信息融合实现自主精确导航将是一个重要的发展趋势。
本文阐述了国内外自主导航系统的研制情况、特点，并基于此介绍了自主导航在无人机中的运用及发展。
可提供相关技术人员参考。
关键词：自主导航；无人机0 引言无人驾驶飞机是一种有动力、可控制、能携带多种任务设备、执行多种任务，并能重复使用的无人驾驶航空器。
简称无人机(Unmanned AenM Vehicle缩写UAV)。
自1913年世界上出现第一个自动驾驶仪以来，无人机受到越来越多国家的重视，发展迅猛。
目前从事研究和生产无人机的有美国、俄罗斯、以色列、法国、英国和南非等近3O个国家，无人机基本型数量已增加到300多种。
鉴于其独有的低成本、低损耗、零伤亡、可重复使用和高机动等诸多优势，其使用范围已拓宽到军事、民用和科学研究三大领域。
自主导航是指不依赖外界支持而在飞行器上实时确定自身的位置和速度的能力。
从严格意义上来说, 只有当飞行器是自保持的, 以实时运行, 不与外界交换信息, 且不依靠地面支持, 在这种情况下, 系统才是自主的。
无人机自主导航是指无人机依靠机载电子系统、飞行控制系统等设备实现位姿估计、目标检测、路径规划并按照要求的精度, 在一定时间内正确地引导至目的地。
无人机自主导航一般利用全球定位系统(global positioning system) 和惯性测量单元(inertial measurement unit) 的组合以及图像导航技术来完成, 图像导航技术具有精度高、适应性强、自动化程度高等优点。
哈工大惯性技术(导航原理)大作业
Assignment ofInertial Technology《惯性技术》作业My Chinese NameMy Student ID 15S004001Autumn 2015Assignment 1: 2-DOF response simulationA 2-DOF gyro is subjected to a sinusoidal torque with amplitude of 4 g.cm and frequency of 10 Hz along its outer ring axis. The angular moment of its rotor is 10000 g.cm/s , and the angular inertias in its equatorial plane are both 4 g.cm/s 2. Please simulate the response of the gyro within 1 second, and present whatever you can discover or confirm from the result.In this simulation, we are going to discuss the response of a 2-DOF gyro to sinusoidal torque input. According to the transfer function of the 2-DOF gyro, the outputs can be expressed as:12222()()()()yx y x y x y J Hs M s M s J J s H s J J s H α=-++12222()()()()x yx x y x y J Hs M s M s J J s H s J J s H β=+++ The original system transfer function is a 2-input, 2-output coupling system. But the given input only exists one input, we can treat the system as 2 separate FIFO systemAs a consequence, we can establish the block diagram of the system in simulink in Fig 1.1. Substitute the parameters into the system and input, then we have the input signals as follow: 0,4sin(20)y ox M M t π==Then the inverse Laplace transform of the output equals the response of the gyro in timedomain as follows:0222200020222200()sin sin ()()()cos cos ()()ox a oxa x a x a ox ox a ox a a a a a M M t t t J J M M M t t t H H H ωαωωωωωωωωωβωωωωωωωω=-+--=+---Fig 1.1 The block diagram of the system in simulinkAnd the simulation results in time domain within 1 second are shown in the follwing pictures. Fig1.2 is the the output of outer ring ()t α. Fig1.3 is the output of inner ring ()t β. Fig1.4 is the trajectory of 2-DOF gyro ’s response to sinusoidal input. As we can see from the Fig1.3,there are obvious sawtooth wave in the output of the inner ring. It ’s a unexpected phenomenon in my original theoretical analysis.Fig1.2 The output of inner ring ()t βFig 1.3 The output of outer ring ()t αI believe the sawtooth wave is caused by the nutation. For the frequency of the nutation is obtained as010*******/3974e H rad s Hz J ω===≈, which is far higher than the frequencyof the applied sinusoidal torque , namely0a ωω .Fig 1.4 Trajectory of 2-DOF gyro ’s response to sinusoidal inputThe trajectory of 2-DOF gyro ’s response to sinusoidal input are shown in Fig1.4. As we can see, it ’s coupling of X and Y channel scope output. The overall shape is an ellipse, which is not perfect for there are so many sawteeth on the top of it.Note that the major axis of ellipse is in the direction of the forced procession, amplitude of which is 0ox M H ω, whereas the minor axis is in the direction of the torsion spring effects,with amplitude ox aM H ω.The nutation components are much smaller than that of the forced vibration, which can be eliminated to get the clear static response.22002220()sin sin ()cos (1cos )()ox oxaa x a ox ox oxa a a a a aM M t t t J H M M M t t t H H H αωωωωωωβωωωωωωω≈≈-≈-=--To prove it, we eliminate the effects of the nutation namely the quadratic term in the denominator and get Fig 1.5, which is a perfect ellipse. We can conclude that when input to the 2-DOF gyro is sinusoidal torque, the gyro will do an ellipse conical pendulum as a static response, including procession and the torsion spring effects, together with a high-frequency vibration as the dynamic response.Fig 1.5 Trajectory of the gyro’s response without nutationAssignment 2: Single-axis INS simulation2.1 description of the problemA magnetic levitation train is being tested along a track running north-south. It first accelerates and then cruises at a constant speed. Onboard is a single-axis platform INS, working in the way described by the courseware of Unit 5: Basic problems of INS. The motion informationand Earth parameters are shown in table 2-1, and the possible error sources are shown in Table2-2.Fig2.1 The sketch map of the single-axis INS problemYou are asked to simulate the operation of the INS within 10,000 seconds, and investigate,first one by one and then altogether, the impact of these error sources on the performance of theINS.The block diagram in the courseware might be of some help. However, there lurks aninconspicuous error, which you have to correct before you can obtain reasonable results.There are one core relevant formula, to get the specific form of its solution, we should substitute the unknown parameters.(1)()c N a y A K yga =∆++-Firstly, the input signal is accelerometer of the platform, and the velocity of the platform is the integration of the acceleration.0/p y y ydt yR ω=+=-⎰The acceleration along Yp may contains two parts:cos gsin g yp f y yααα=-≈- When accelerometer errors are concerned, the output of accelerometer will be:(1)N a yp N a K f A =++When gyro errors concerned:'(1)p g p K ωωε=++Onlyαis unknown:000[(1)]tg p t tp t K dt dtααωεϕϕω=+++-∆∆=⎰⎰And the reference block diagram and simulink block diagram are as follows in Fig2.2, Fig2.3. There is a small fault in the reference block, which is that the sign of the marked add operation should be positive instead of negative.The results of the simulation are shown in Fig 2.4 to Fig 2.13.Fig2.2 The reference block diagram in the courseware(unrectified)Fig2.3 The simulink block diagram for Assignment 22.2 results and analysis of the problemFig.2.4 real acceleration,velocity and displacement output without error sourcesKFig2.5 position bias when only accelerometer scale factor error exists0.0001aFig2.6 position bias output when only gyro scale factor error exists 0.0001g K =Fig2.7 position bias when only acceleromete bias error exists 20.0002/N A m s ∆=Fig2.8 position bias output when only initial velocity error exists 200.01/ym s ∆=Fig2.9 position bias output when only initial position error exists 010y m ∆=Fig2.10 position bias output bias when only gyro bias error exists 0.000000024240.00681/3/h rad s ε=︒=Fig2.11 position bias output when only initial platform misalignment angle exists0.000012120''345radα==Fig2.12 output considering all the error sourcesFig2.13 position bias output considering all error sourcesAs we can see in the above simulation results, if there is no error we can navigate the train ’s motion correctly, which comes from north to the south as shown in Fig2.4, beginning with an constant acceleration within 60 seconds then cruises at a constant speed, approximately 65 m/s. However, the situation will change a lot when different errors put into the simulation. The initial position error 010y m ∆= effects least as Fig.2.8, for this error doesn ’t enter into the closed loop and it won ’t influence the iterative process. The position bias is constant and can be negligible.In the second case, when the accelerometer scale factor error exists, 0.0001a K =, as shown in Fig2.5, the result are stable and almost accurate, the position bias is a sinusoidal output. So it is with the accelerometer bias error situation, 20.0002/N A m s ∆=, in Fig2.7, the initialvelocity error, 200.01/ym s ∆= in Fig2.9, and the initial platform misalignment angle, 05''α=, in Fig2.11. However, the influence degrees of the different factors are not in the samemagnitude. The accelerometer scale factor influences the least with magnitude of 5, then the initial velocity larger magnitude of 8, and the accelerometer bias magnitude of 25. The influence of the initial platform misalignment angle is much more significant with a magnitude of 150. All the navigation bias in the second kind case is sinusoidal, which means they ’re limited and negligible as time passes by.In the third case, such as the gyro scale factor error situation, 0.0001g K =, in Fig2.6, and the gyro bias error,0.01/h ε=︒, results in Fig2.10, effects the most significant, the trajectory of the navigation disvergence accumulated as time goes. The position bias is a combination of sinusoidal signal and ramp signal. They also show that the longitudinal and distance errors resulted from gyro drifts are not convergent in time. It means the errors in the gyroscope do most harm to our navigation. And due to the significant influence of the gyro bias errors and the gyroscope scale factor error, results considering all the error sources disverge, and the navigationposition of the motion will be away from the real motion after a enough long time, as shown in Fig2.10. The gyro bias error is the most significant effect factor of all errors. By the time of 10000s, it has reaches 1600m, and it’s nearly the quantity of the position bias considering all error sources. Through contrasting all the results, We can conclude that the gyro bias error is the main component of the whole position bias.Impression of the Whole simulation experimentThrough contrasting all the results, We can conclude that the gyro bias error is the main component of the whole position bias, and the the gyro bias or the drift error do most harm to our navigation. So it is a must for us to weaken or eliminate it anyway. In spite of all the disadvantages discussed above, the INS still shows us a relatively accurate results of single-axis navigation.Assignment 3: SINS simulation3.1 Task descriptionA missile equipped with SINS is initially at the position of 46o NL and 123 o EL, stationary on a launch pad. Three gyros, GX, GY, GZ, and three accelerometers, AX, AY, AZ, are installed along the axes Xb, Yb, Zb of its body frame respectively.Case 1: stationary testThe body frame of the missile initially coincides with the geographical frame, as shown in the figure, with its pitching axis Xb pointing to the east, rolling axis Yb to the north, and azimuth axis Zb upward. Then the body of the missile is made to rotate in 3 steps:(1) -22o around Xb(2) 78o around Yb(3) –16o around ZbFig 3.1 Introduction to assignment 3After that, the body of the missile stops rotating. You are required to compute the final outputs of the three accelerometers on the missile, using quaternion and ignoring the device errors. It is known that the magnitude of gravity acceleration is g = 9.8m/s2.Case 2: flight tes tInitially, the missile is stationary on the launch pad, 400m above the sea level. Its rolling axis is vertical up,and its pitching axis is to the east. Then the missile is fired up. The outputs of the gyros and accelerometers are both pulse numbers. Each gyro pulse is an angular increment of 0.01arc-sec, and each accelerometer pulse is 1e-7g, with g =9.8m/s2. The gyro output frequency is 200Hz, and the accelerometer’s is 10Hz. The outputs of the gyros and accelerometers within 1315s are stored in a MA TLAB data file named imu.mat, containing matrices gm of 263000× 3 and am of 13150× 3 respectively. The format of the data is as shown in the tables, with 10 rows of each matrix selected. Each row represents the outputs of the type of sensors at each sampling time. The Earth can be seen as an ideal sphere, with radius 6371.00km and spinning rate 7.292× 10-5 rad/s, The errors of the sensors are ignored, so is the effect of height on the magnitude of gravity. The outputs of the gyros are to be integrated every 0.005s. The rotation of the geographical frame is to be updated every 0.1s, so are the velocities and positions of the missile.You are required to:(1) compute the final attitude quaternion, longitude, latitude, height, and east, north, vertical velocities of the missile.(2) compute the total horizontal distance traveled by the missile.(3) draw the latitude-versus-longitude trajectory of the missile, with horizontal longitude axis.(4) draw the curve of the height of the missile, with horizontal time axis.Fig 3.2 simplified navigation algorithm for SINS 3.2Procedure code3.2.1 Sub function code：quaternion multiply code:function [q1]=quml(q1,q2);lm=q1(1);p1=q1(2);p2=q1(3);p3=q1(4);q1=[lm -p1 -p2 -p3;p1 lm -p3 p2;p2 p3 lm -p1;p3 -p2 p1 lm]*q2;endquaternion inversion code:function [qni] =qinv(q)q(1)=q(1);q(2)=-q(2);q(3)=-q(3);q(4)=-q(4);qni=q;end3.2.2case1 DCM algorithm：function ans11cz=[cos(-22/180*pi) sin(-22/180*pi) 0 ;-sin(-22/180*pi) cos(-22/180*pi) 0;0 0 1]; %The third rotation DCMcx=[1 0 0 ;0 cos(78/180*pi) sin(78/180*pi);0 -sin(78/180*pi) cos(78/180*pi)]; %The first rotation DCMcy=[cos(-16/180*pi) 0 -sin(-16/180*pi);0 1 0;sin(-16/180*pi) 0 cos(-16/180*pi)]; %The second rotation DCM A=cz*cy*cx*[0;0;-9.8]end3.2.3case1 quaternion algorithm：function ans12g=[0;0;0;-9.8];q1=[cos(-11/180*pi);sin(-11/180*pi);0;0]; %The first rotation quaternionq2=[cos(39/180*pi);0;sin(39/180*pi);0]; %The second rotation quaternionq3=[cos(-8/180*pi);0;0;-sin(-8/180*pi)]; %The third rotation quaternionr=quml(q1,q2); %call the quaternion multiplication subfunction q=quml(r,q3);P1=[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)];P2=[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)];P=P1*P2;gn=P*g;gn=gn(2:4)end3.2.4 case2 SINS quaternion algorithm code：function ans2clc;clear;%parameters initializing：T=0.005;K=13150;R=6371000; %radius of earthwE=7.292*10^(-5); %spinning rate of earthQ=zeros(4, 263001) ;%quaternion matrix initializinglongitude=zeros(1,13151);latitude=zeros(1,13151);H=zeros(1,13151); %altitude matrixQ(:,1)=[cos(45/180*pi);-sin(45/180*pi);0;0]; % initial quaternion longitude(1)=123; %initial longitudelatitude(1)=46; % initial latitudeH(1)=400; %initial altitudelength=0;g=9.8;vE = zeros(1,13151); %eastern velocityvN = zeros(1,13151); %northern velocityvH = zeros(1,13151); %upward velocityvE(1)=0;vN(1)=0;vH(1)=0;load imu.mat %data loading%main calculation sectionfor N=1:Kq1=zeros(4,11);q1(:,1)=Q(:,N);for n=1:20 % Attitude iteration wx=0.01/(3600*180)*pi*gm((N-1)*10+n,1); % Angle incrementwy=0.01/(3600*180)*pi*gm((N-1)*10+n,2);wz=0.01/(3600*180)*pi*gm((N-1)*10+n,3);w=[wx,wy,wz]';normw=norm(w); % Norm calculationW=[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);Q(:,N+1)=q1(:,n+1);endWE=-vN(N)/R; % rotational angular velocity component of a geographic coordinate systemWN=vE(N)/R+wE*cos(latitude(N)/180*pi);WH=vE(N)/R*tan(latitude(N)/180*pi)+wE*sin(latitude(N)/180*pi);attitude=[WE,WN,WH]'*T; %correction of the quaternion by updating the rotation of geographic coordinatenormattitude=norm(attitude);e=attitude/normattitude;QG=[cos(normattitude/2);sin(normattitude/2)*e];Q(:,N+1)=quml(qinv(QG),Q(:,N+1));fx=1e-7*9.8*am(N,1); %specific force measured by accelerometerfy=1e-7*9.8*am(N,2);fz=1e-7*9.8*am(N,3);Fb=[fx fy fz]';F=quml(Q(:,N+1),quml([0;Fb],qinv(Q(:,N+1)))); %The specific force is decomposed into geographic coordinate system.FE(N)=F(2);FN(N)=F(3);FU(N)=F(4);%calculate the velocity of the vehicle:VED(N)=FE(N)+vE(N)*vN(N)/R*tan(latitude(N)/180*pi)-(vE(N)/R+2*wE*cos(latitude(N)/ 180*pi))*vH(N)+2*vN(N)*wE*sin(latitude(N)/180*pi);VND(N)=FN(N)-2*vE(N)*wE*sin(latitude(N)/180*pi)-vE(N)*vE(N)/R*tan(latitude(N)/180 *pi)-vN(N)*vH(N)/R;VUD(N)=FU(N)+2*vE(N)*wE*cos(latitude(N)/180*pi)+(vE(N)^2+vN(N)^2)/R-g;%integration and get the relative velocity of vehicle:vE(N+1)=VED(N)*T+vE(N);vN(N+1)=VND(N)*T+vN(N);vH(N+1)=VUD(N)*T+vH(N);% integration and get the position of vehicle:longitude(N+1)=vE(N)/(R*cos(latitude(N)/180*pi))*T/pi*180+longitude(N);latitude(N+1)=vN(N)/R*T/pi*180+latitude(N);H(N+1)=vH(N)*T+H(N);length=sqrt((vE(N))^2+(vN(N))^2)+length;endfigure(1) %picture the longitude-latitude curve of the motion within 1315 secondstitle(' longitude-latitude ');hold ongrid onplot(longitude,latitude);figure(2) % picture the altitude curve of the motion within 1315 secondstitle('altitude');hold ongrid onplot(0:13150,H);3.3Simulation outputs and results analysisIn case 1, the DCM algorithm and quaternion results are the same as follow:A =7.53165.9788-1.8892And the results suggest the solution of DCM and quaternion is equivalence in this problem.In case 2, the latitude-versus-longitude trajectory of the missile(with horizontal longitude axis) and the altitude curve of the missile are shown as follows in Fig 3.3 and Fig 3.4.Fig3.3 the latitude-versus-longitude trajectory of the missile(with horizontal longitude axis)Fig3.4the altitude curve of the missileImpression of the Assignment 3In this assignment, we simulate the missile navigation situation in a real problem, as the problem we have done before. I did this based on my own original code, but to my surprise, it’s harder than I have expected. For the detailed thoughts for my program I have forgotten. I have to recheck the code line by line, But I am still troubled in correcting the initial parameters for many times. It has taught me a lesson, which is never to be egotistical for your ability to memory and thework you have done or understood.。
北航惯性导航综合实验三实验报告
北航惯性导航综合实验三实验报告惯性导航技术综合实验实验三惯性导航综合实验实验3.1 初始对准实验一、实验目的结合已经采集并标定好的惯性传感器数据进行粗对准，了解实现对准的过程；通过比较不同传感器数据的对准结果，进一步认识惯性传感器性能在导航系统中的重要地位。
为在实际工程设计中针对不同应用需求下采取相应的导航系统方案提供依据。
二、实验内容利用加速度计输出计算得到系统的初始姿态，利用陀螺输出信号计算航向角。
对比利用不同的惯性传感器数据计算所得的不同结果。
三、实验系统组成MEMS IMU（或其他类型IMU）、稳压电源、数据采集系统与分析系统。
四、实验原理惯导系统在开始进行导航解算之前需要进行初始对准，水平对准的本质是将重力加速度作为对准基准，其对准精度主要取决于两个水平加速度计的精度，加速度计的零位输出将会造成水平对准误差；方位对准最常用的方位是罗经对准，其本质是以地球自转角速度作为对准基准，影响对准精度的主要因素是等效东向陀螺漂移。
(1) 其中，分别为当前时刻的俯仰角和横滚角计算值。
水平对准误差和方位对准误差如下所示：，(2) 五、实验步骤及结果1、实验步骤：采集静止状态下水平加速度计输出，按下式计算其平均值。
(3) 其中，为前n个加计输出均值；为前n-1个加计输出均值；为当前时刻加计输出值。
利用加计平均值来计算系统初始俯仰角和横滚角(4) 其中，分别为当前时刻的俯仰角和横滚角计算值。
2、实验结果与分析：2.1、用MIMS IMU的加速度计信息计算（1）俯仰角和横滚角图：（2）失准角：2.2、实验结果分析以上计算是基于MIMS IMU静止时data2进行的初始对准，与data2给定的初始姿态角相差不大。
六、源程序clear clc g = 9.__14; a=load(＇E:＼郭凤玲＼chushiduizhun＼data2.txt＇); ax=a(:,4)＇; ay=a(:,5)＇; az=a(:,6)＇; %初始值ax0(1)=ax(1)/1000*g; %%%%转化单位，由mg转化为m/s^2 ay0(1)=ay(1)/1000*g; az0(1)=az(1)/1000*g; theta(1)=asin(ay(1)/az(1)); gama(1)=-asin(ax(1)/az(1)); for i=2:__ ax0(i)=ax0(i-1)+(ax(i)-ax0(i-1))/i; ay0(i)=ay0(i-1)+(ay(i)-ay0(i-1))/i; az0(i)=az0(i-1)+(az(i)-az0(i-1))/i; theta(i)=asin(ay0(i)/az0(i));gama(i)=-asin(ax0(i)/az0(i)); end detfaix=mean(ay0)/g; detfaiy=mean(-ax0)/g; t=1:__; plot(t,theta,＇r＇,t,gama,＇b＇) title(＇俯仰角和横滚角＇);ylabel(＇弧度（rad）＇); legend(＇俯仰角＇,＇横滚角＇) 实验3.2 惯性导航静态实验一、实验目的1、掌握捷联惯导系统基本工作原理2、掌握捷联惯导系统捷联解算方法3、了解捷联惯导系统误差传递规律和方程二、实验原理捷联惯性导航系统（SINS）的导航解算流程如图1所示。
北航惯性导航综合实验五实验报告
惯性导航技术综合实验实验五惯性基组合导航及应用技术实验惯性/卫星组合导航系统车载实验一、实验目的①掌握捷联惯导/GPS组合导航系统的构成和基本工作原理；②掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理；③掌握捷联惯导/GPS组合导航系统静态性能；④掌握动态情况下捷联惯导/GPS组合导航系统的性能。
二、实验内容①复习卡尔曼滤波的基本原理（参考《卡尔曼滤波与组合导航原理》第二、五章）；②复习捷联惯导/GPS组合导航系统的基本工作原理（参考以光衢编著的《惯性导航原理》第七章）；三、实验系统组成①捷联惯导/GPS组合导航实验系统一套；②监控计算机一台。
③差分GPS接收机一套；④实验车一辆；⑤车载大理石平台；⑥车载电源系统。
四、实验内容1）实验准备①将IMU紧固在车载大理石减振平台上，确认IMU的安装基准面紧靠实验平台；②将IMU与导航计算机、导航计算机与车载电源、导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连接线正确连接；③ 打开GPS 接收机电源，确认可以接收到4颗以上卫星； ④ 打开电源，启动实验系统。
2） 捷联惯导/GPS 组合导航实验① 进入捷联惯导初始对准状态，记录IMU 的原始输出，注意5分钟内严禁移动实验车和IMU ；② 实验系统经过5分钟初始对准之后，进入导航状态； ③ 移动实验车，按设计实验路线行驶；④ 利用监控计算机中的导航软件进行导航解算，并显示导航结果。
五、 实验结果及分析(一) 理论推导捷联惯导短时段（1分钟）位置误差，并用1分钟惯导实验数据验证。
1、一分钟惯导位置误差理论推导：短时段内（t<5min ），忽略地球自转0ie ω=，运动轨迹近似为平面1/0R =，此时的位置误差分析可简化为：（1） 加速度计零偏∇引起的位置误差：210.88022t x δ∇==m （2） 失准角0φ引起的误差：202 0.92182g t x φδ==m （3） 陀螺漂移ε引起的误差：330.01376g t x εδ==m 可得1min 后的位置误差值123 1.8157m x x x x δδδδ=++= 2、一分钟惯导实验数据验证结果：（1）纯惯导解算1min 的位置及位置误差图：lat0.01s 度lon0.01s度北向位移误差0.01sm 东向位移误差0.01sm（2）纯惯导解算1min 的速度及速度误差图：-100-50050Vx0.01s m /s020406080Vy0.01sm /s100020003000400050006000-0.4-0.3-0.2-0.10Vx 误差0.01s m /s100020003000400050006000-0.1-0.0500.050.1Vy 误差0.01sm /s实验结果分析：纯惯导解算短时间内精度很高，1min 的惯导解算的北向最大位移误差-2.668m ，东向最大位移误差-8.231m ，可见实验数据所得位置误差与理论推导的位置误差在同一数量级，结果不完全相同是因为理论推导时做了大量简化，而且实验时视GPS 为真实值也会带来误差；另外，可见1min 内纯惯导解算的东向速度最大误差-0.2754m/s ，北向速度最大误差-0.08027m/s 。
惯导第二次大作业
《惯性导航原理》第二次大作业原理分析在利用方向余弦方法对惯性导航系统进行测算时，刚体空间位置用固连于刚体的动坐标系对固定参考系各轴的九个方向余弦来确定，九个方向余弦角存在六个约束条件，计算比较繁琐，模型也比较复杂。
如果在计算过程中引入四元数, 则可以通过坐标系的一次转动，实现方向余弦方法中的三次坐标旋转。
原理图如下:L、hVy、Vz从原理图可以清楚的看出，通过捷联姿态矩阵C b 可以将任意姿态的平台坐 标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方 法即可以得到任意时刻载体的位置和速度信息。
关键在于捷联姿态矩阵的求解。
在这里应用四元数知识进行解算。
1.捷联姿态矩阵的求解因为平台的初始姿态角都是已知的，则可以先求解四元数中各元的初值。
平 台坐标系相对于地理坐标系的三次旋转可以由四元数的乘积得到。
这三次转动为（逆时针为正）：© ©Q i = cos — + ??sin2AAQ 2 = cos — + ??????sin 2 2丫丫Q 3 = cos + ????sij对三者进行四元素乘法运算：Q = Q 3 0 Q 2 0 Q i结果与四兀数《瓜+ /i x+ h y +甩z 中的各兀素相对应, 的平台初始姿态求解出四元数的各元初值。
©A丫cos cos 二 cos ：-2 2 2 © . A Ycos sin cos ：- 2 2 2 © A . Y cos cos 一 sin + 2 2 2.©A Ysin cos ： cos ： + 2 2 2.© . A . Y sin sinsin 2 2 2 .© A . Y sin cos ： sin 2 2 2.© . AY sin sin cos - 2 2 2 © A 丫 cos 二 sin sin 2 2 2带入四元数姿态矩阵即可得到捷联姿态矩阵:h + h + 入2+ h 2( h h + h h ) C b=( 2( 1 2 h 0 ?3)為-斤+h 2 - h2( 1 ?3+ h 心 2( 2 ?3 - 入)通过此矩阵可以将地理坐标系的参数转移到平台坐标系。
北航惯性导航综合实验五实验报告
惯性导航技术综合实验实验五惯性基组合导航及应用技术实验惯性/卫星组合导航系统车载实验一、实验目的①掌握捷联惯导/GPS组合导航系统的构成和基本工作原理：②掌握采用卡尔曼波波方法进行捷联惯导/GPS组合的基本原理：③掌握捷联惯导/GPS组合导航系统静态性能：④掌握动态情况下捷联惯导/GPS组合导航系统的性能。
二、实验内容①复习卡尔曼滤波的基本原理（参考《卡尔曼滤波与组合导航原理》第二、五章）；②复习捷联惯导/GPS组合导航系统的基本工作原理（参考以光衢编著的《惯性导航原理》第七章）；三、实验系统组成①捷联惯导/GPS组合导航实验系统一套：②监控计算机一台。
③差分GPS接收机一套：④实验车一辆：⑤车载大理石平台：⑥车载电源系统。
四、实验内容D实验准备①将IMU紧固在车载大理石减振平台上，确认IMU的安装基准面紧靠实验平台；②将IMU与导航计算机、导航计算机与车载电源、导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连接线正确连接；③打开GPS接收机电源，确认可以接收到4颗以上卫星；④打开电源，启动实验系统。
2）捷联惯导/GPS组合导航实验①进入捷联惯导初始对准状态，记录IMU的原始输出，注意5分钟内严禁移动实验车和IMU；②实验系统经过5分钟初始对准之后，进入导航状态；③移动实验车，按设计实验路线行驶；④利用监控计算机中的导航软件进行导航解算，并显示导航结果。
五、实验结果及分析（一）理论推导捷联惯导短时段（1分钟）位置误差，并用1分钟惯导实验数据验证。
1、一分钟惯导位置误差理论推导：短时段内（t<5min）,忽略地球自转绍。
=0,运动轨迹近似为平面1/R = O,此时的位置误差分析可简化为：Vr2（1）加速度计零偏V引起的位置误差：Jx,= —= 0.8802 m2（2）失准角我引起的误差：3X2=^-= 0.9218m（3）陀螺漂移£引起的误差：= 0.0137 m6可得1min后的位置误差值5x =+ J.r2 + 5x. = 1.8157m2、一分钟惯导实验数据验证结果：（1）纯惯导解算1min的位置及位置误差图：（2）纯惯导解算1min的速度及速度误差图：vy 8060| 40200 1000 2000 3000 4000 5000 60000.01s实验结果分析：纯惯导解算短时间内精度很高，1min的惯导解算的北向最大位移误差-2.668m,东向最大位移误差-8.231m,可见实验数据所得位置误差与理论推导的位置误差在同一数量级，结果不完全相同是因为理论推导时做了大量简化，而且实验时视GPS为真实值也会带来误差；另外，可见1min内纯惯导解算的东向速度最大误差-0.2754m/s，北向速度最大误差-0.08027m/So(-)选取IMU前5分钟数据进行对准实验。
哈工大-导航原理作业
Assignments of Inertial Navigation 《惯性导航》作业Autumn 2017Assignment 1: Coordinate transformation for 3D animationfigure 1.1 Interface for rotating animation control of missileAttached is a group of MATLAB programs for 3D animation control of a missile. Initially, the body frame of the missile coincides with the local geographical frame, with its pitching axis X pointing to the east, rolling axis Y to the north, and heading axis Z to the sky, as shown in figure 1.1 which is the controlling in-terface produced by running the program main.m.The three coordinates of each vertex of the 3D model occupies a row in the matrix VTX, The color property and surface information of the model are defined by matrices VTXcolor and faceM respectively. These matrices are loaded from the data file missiledata.mat.Input an angle (in degree) in the "Rotation Angle" box, and click one of the rotation buttons ("Heading Ro-tation", "Pitching Rotation", or "Rolling Rotation"), then it is expected that the missile will rotate from its current attitude for the input angle around the chosen axis of its current body.The animation is to be achieved by the program redraw.m which redraws the missile every once it rotates for one degree, using patch command. During each step of the animation, the current rotation angle of the missile relative to its pre-animation attitude has been generated and stored in one of the variables head, pitch and roll, with only one of them being nonzero. Before each redrawing, you need to re-calculate the coordinates of the missile in VTX resolved in the geographical frame, instead of keeping them the same as their values at the very beginning (VTX0). That is, in the program redraw.m, you need to replace the command line VTX=VTX0with your own codes for re-calculating VTX. Elsewhere, additional codes also might be required to make it work.Please rewrite the program redraw.m, or others if necessary, so that successive rotating animations can be achieved, and explain the rationale behind your rewriting.一、 任务分析本作业需要实现的功能为实现火箭的连续旋转。
北航惯性导航作业二.
惯性导航作业一、数据说明：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：作业以纸质报告形式提交，附源程序。
惯性技术课程大作业
Assignment of Inertial Technology 《惯性技术》作业The report is to contain:1. Description of the tasks – contents of the next page.2. The code of your programs, and its explanation.3. The results of your computation or simulation.4. Your analysis of the result, and your reflections on the programming or simulation5. Acknowledgements of the help or reference in finishing your assignment (optional).A missile is initially at the position with latitude of45.75 degree and longitude of 126.63 degree in thenorth hemisphere. Three gyros, Gx, Gy, Gz, andthree accelerometers, Ax, Ay, Az, are installed along the axes Xb, Yb, Zb of its body frame respectively.Case 1: stationary ground testThe body frame of the missile initially coincides with the geographical frame, as shown in the figure, with it axis Xb pointing to the east, Yb to the north, and Zb to the sky. Then the body of the missile undergoes three successive rotations relative to the geographical frame:(1) 20 degrees around its axis Zb;(2) 45 degrees around its axis Xb;(3) -60 degrees around its axis Yb.After that, the body of the missile stops rotating. You are required to compute the final outputs of the three accelerometers on the missile, using both direction cosine matrix and quaternion respectively, and ignoring the device errors. It is known that the magnitude of gravity acceleration is g = 9.8m/s2.Case 2: in flightInitially, the body frame of the missile also coincides with the local geographical frame, as in Case 1. It has initial height of 200m, north velocity of 1800m/s, and zero east and upward velocities.The outputs of the gyros and accelerometers are both in the form of pulse numbers. Each pulse from a gyro output represents an angular increment of 1e-5 rad, and each pulse from an accelerometer output represents 1e-6g, with g = 9.8m/s2. The output sampling frequency of the gyros is 10Hz, and that of the accelerometers is 1Hz. The output data of the gyros and accelerometers within 200s are stored in MATLAB data files named gout.mat and aout.mat, containing matrices gm of 2000 by 3 and am of 200 by 3 respectively. The format of the data is shown in the tables, with the first 10 rows of each matrix included. Each row represents the outputs of the gyros or those of the accelerometers at a sampling time.The Earth can be seen as an ideal sphere, with radius 6371.00km and spinning rate 7.292×10-5 rad/s, The errors of the gyros and accelerometers can be neglected, and the effect of height on the magnitude of the gravity acceleration can be ignored. The outputs of the gyros are to be integrated every 0.1s. The rotation of the geographical frame is to be updated every 1 second, so are the velocity and position of the missile. You are required: (1) to compute the final longitude, latitude, height, east velocity and north velocity of the missile.(2) to compute the final attitude quaternion of the missile relative to the local geographical frame.(3) to draw the curve of the latitude of the missile versus its longitude during the 200s, with horizontal longitude axis and vertical latitude axis.(4) to draw the curve of the height of the missile during the 200s, with thehorizontal time axis and vertical height axis.Contents Case 1: stationary ground test1）Direction cosine matrixz1=20*2*pi/360;z2=45*2*pi/360;z3=-60*2*pi/360;% radians around its axis Zb,Xb,YbCz=[cos(z1) sin(z1) 0; -sin(z1) cos(z1) 0; 0 0 1]; Cx=[1 0 0; 0 cos(z2) sin(z2); 0 -sin(z2) cos(z2)]; Cy=[cos(z3) 0 -sin(z3); 0 1 0; sin(z3) 0 cos(z3)]; %three direction cosine matrixsg=[0; 0; -9.8];% g in the geographical frameg1=Cy*Cx*Cz*g%g1 is the output of g2）quaternionsz1=20*2*pi/360;z2=45*2*pi/360;z3=-60*2*pi/360;% radians around its axis Zb,Xb,Ybq1=[cos(b1/2); 0; 0; sin(b1/2)];q2=[cos(b2/2); sin(b2/2); 0; 0];q3=[cos(b3/2); 0; sin(b3/2); 0];% quaternions around its axis Zb,Xb,Ybg=[0; 0; 0; -9.8];% quaternions of g in the original geographical frameq=qmul(qmul(q1,q2),q3);% quaternions of rotationsg2=q-1*g*q% quaternions of g in new frame1.1R esults of the computation1)Using DCM algorithmg1=[-6.0017; -6.9296; -3.4648]2)Using quaternion algorithmg2=[-6.0013;; -6.9296; -3.4648]1.2A nalysis of the resultThe computation results using both direction cosine matrix and quaternion respectively are approximately same.Case 2: in flightCode %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %initialformat long;I4=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];R=6371000; %radius of the earthlongitude(1)=126.63*pi/180;%The initial latitudelatitude(1)=45.75*pi/180; % The initial longitudehigh=200; % The initial heightomiga=0.00007292; % The earth's rotation speedg=9.8; % The earth's gravitational accelerationVE(1)=0; %The initial eastward velocityVN(1)=1800; % The initial northward speedVU(1)=0; % The initial upward speedh1=1e-5; %the angular increment in a pulseh2=1e-6*9.8; % the accelerate increment in a pulsedetat=0.1; %The inner cycle timek=10; % Attitude iteration speeddetaT=k*detat; %The outer cycleK=200; %Total position number of iterationsT=K*detaT; %The total cycle timeQ=[1;0;0;0]; %The initial quaternions %%%%%%%%%%%%%%%%%%%%%%%%%Initialize some storage dataLongitude=zeros(1,201); % storage Longitude informationLongitude(1)=126.63*pi/180;Latitude=zeros(1,201); %Storage latitude information Latitude(1)=45.75*pi/180;High=zeros(1,201); % Storage height information High(1)=200;D=zeros(2001,4); % Storage quaternions information before uptatingdetagama=zeros(3,1);ga = gm;ma = am;ga = load('gout.mat');ma = load('aout.mat'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%startfor N=1:1:K%inner circle information of gryo in 1sfor n=1:1:kloadax=ga(((N-1)*10+n),1)*h1; %read the increase of gyro in the x axis directionloaday=ga(((N-1)*10+n),2)*h1; % read the increase of gyro in the y axis directionloadaz=ga(((N-1)*10+n),3)*h1; % read the increase of gyro in the z axis directionloadta=[loadax;loaday;loadaz];detabeta=[0 -loadax -loaday -loadaz;loadax 0 loadaz -loaday;loaday-loadaz 0 loadax;loadaz loaday -loadax 0];det=sqrt(loadax*loadax+loaday*loaday+loadaz*loadaz);Q=(cos(det/2)*I4+((1/det)*sin(det/2))*detabeta)*Q;%storage information in Q as quaternionsD((N-1)*10+n,:)=Q'; %stor age Q in Dend%%%%%%%%%%%use gyro information updatingomigaE=-VN(N)/R;omigaN=VE(N)/R+omiga*cos(latitude);omigaU=(VE(N)/R)*tan(latitude)+omiga*sin(latitude);detagama=[omigaE;omigaN;omigaU]*detaT;detagama1=sqrt(detagama(1)*detagama(1)+detagama(2)*detagama(2)+d etagama(3)*detagama(3));n=detagama/detagama1;m=sin(detagama1/2)*n;GM=[cos(detagama1/2);m(1);m(2);m(3)];Gm=[cos(detagama1/2);-m(1);-m(2);-m(3)];Q=[Gm(1) -Gm(2) -Gm(3) -Gm(4);Gm(2) Gm(1) -Gm(4) Gm(3);Gm(3) Gm(4) Gm(1) -Gm(2);Gm(4) -Gm(3) Gm(2) Gm(1)]*Q;u=(Q(1)*Q(1)+Q(2)*Q(2)+Q(3)*Q(3)+Q(4)*Q(4));Q1=(1/u)*[Q(1);-Q(2);-Q(3);-Q(4)];fb=[0;ma(N,1)*h2;ma(N,2)*h2;ma(N,3)*h2];f1=[Q(1) -Q1(2) -Q1(3) -Q1(4);Q1(2) Q1(1) Q1(4) -Q1(3);Q1(3) -Q1(4) Q1(1) Q1(2);Q1(4) Q1(3) -Q1(2) Q1(1)]*fb;f=[Q1(1) -Q(2) -Q(3) -Q(4);Q(2) Q(1) -Q(4) Q(3);Q(3) Q(4) Q1(1)-Q(2);Q(4) -Q(3) Q(2) Q(1)]*f1;fE=f(2);fN=f(3);fU=f(4);detVE=fE+VE(N)*VN(N)*tan(latitude)/R-(VE(N)/R+2*omiga*cos(latitu de))*VU(N)+2*VN(N)*omiga*sin(latitude);detVN=fN-2*VE(N)*omiga*sin(latitude)-VE(N)*VE(N)*tan(latitude)/R-VN(N)*VU(N)/R;detVU=fU+2*VE(N)*omiga*cos(latitude)+(VE(N)*VE(N)+VN(N)*VN( N))/R-g;VE(N+1)=detVE*detaT+VE(N);VN(N+1) =detVN*detaT+VN(N);VU(N+1)=detVU*detaT+VU(N);longitude=VE(N)/(R*cos(latitude))*detaT+longitude;latitude=VN(N)/R*detaT+latitude;high=VU(N)*detaT+high;Longitude(N+1)=longitude;Latitude(N+1)=latitude;High(N+1)=high; %%storage height endSimulation resultsHigh:Latitude:Longitude:Analysis of the simulationWe can transform the coordinate of the missile to geographic coordinate by using the quaternion algorithm ,and some information of missile ,for example the latitude value ,longtitude value and the height of real time can be computed,like pictures above.。
北航惯性导航综合实验一实验报告
SCBOOl OF msiw SCIENd I OPTO-ELECIROIIICS EHGINEEKING实验一陀螺仪关键参数测试与分析实验加速度计关键参数测试与分析实验二零一三年五月十二日实验一陀螺仪关键参数测试与分析实验一、实验目的通过在速率转台上的测试实验，增强动手能力和对惯性测试设备的感性认识；通过对陀螺仪测试数据的分析，对陀螺漂移等参数的物理意义有清晰的认识，同时为在实际工程中应用陀螺仪和对陀螺仪进行误差建模与补偿奠定基础。
二、实验内容利用单轴速率转台，进行陀螺仪标度因数测试、零偏测试、零偏重复性测试、零漂测试实验和陀螺仪标度因数与零偏建模、误差补偿实验。
三、实验系统组成单轴速率转台、MEMS它螺仪（或光纤陀螺仪）、稳压电源、数据采集系统与分析系统。
四、实验原理1. 陀螺仪原理陀螺仪是角速率传感器，用来测量载体相对惯性空间的角速度，通常输出与角速率对应的电压信号。
也有的陀螺输出频率信号（如激光陀螺）和数字信号（把模拟电压数字化）。
以电压表示的陀螺输出信号可表示为：U G■二U G o k G「k c f G（a） k G ；G （1-1）式中f G（a）是与比力有关的陀螺输出误差项，反映了陀螺输出受比力的影响，本实验不考虑此项误差。
因此，式（1-1 ）简化为U G二U G 0 k^ k G G 由（1-2）式得陀螺输出值所对应的角速度测量值：_U G-U G（0）岂测量Gk G (1-2) (1-3)对于数字输出的陀螺仪，传感器内部已经利用标度因数对陀螺仪模拟输出 进行了量化，直接输出角速度值，即：■'测量二 o ''真值.；G--0是是陀螺仪的零偏，物理意义是输入角速度为零时，陀螺仪输出值所对应的角速度。
且U G (0) * 0••测量精度受陀螺仪标度因数k G 、随机漂移；G 、陀螺输出信号U G 的检测精 度和U G (0)的影响。
通常k G 和U G (0)表现为有规律性，可通过建模与补偿方法 消除，；G 表现为随机特性，可通过信号滤波方法抵制。
导航原理大作业
导航原理作业（惯性导航部分）作业内容需包括：1. 题目内容（即本页）2. 程序设计说明及代码3. 计算或仿真结果及分析4. 仿真中遇到的问题或体会一枚导弹采用捷联惯性导航系统，三个速率陀螺仪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/s 2。
第二种情形：导弹正在飞行中。
初始时刻弹体坐标系仍和地理坐标系重合；且导弹初始高度200m ，初始北向速度 1800 m/s ，初始东向速度和垂直速度都为零。
陀螺仪和加速度计的输出都为脉冲数形式，陀螺输出的每个脉冲代表 0.00001弧度的角增量。
加速度计输出的每个脉冲代表1μg ，1g = 9.8m/s 2。
陀螺仪和加速度计输出的采样频率都为10Hz ，在200秒内三个陀螺仪和三个加速度计的输出存在了数据文件gaout.mat 中，内含一矩阵变量ga ，有2000行，6列。
每一行中的数据代表每个采样时刻三个陀螺Gx, Gy, Gz 和三个加速度计Ax, Ay, Az将地球视为理想的球体，半径 6371.00公里，且不考虑仪表误差，也不考虑弹体高度对重力加速度的影响。
选取弹体的姿态计算周期为0.1秒，速度和位置的计算周期为1秒。
北航惯性导航作业一
惯性导航作业一、作业内容：1、数据说明：惯导系统为指北方位的平台系统。
初始经度为：116.344762072818度纬度为：39.981430918136度高度为：40.8236米。
初始姿态角为[0 0 0]（俯仰，横滚，航向，单位：度，航向角以逆时针为正）。
初始速度为0米/秒，飞行高度不变（即：无须计算高度通道）。
排列顺序为一~三行分别为东、北、天向信息,共600秒数据，陀螺仪和加速度计采样周期为0.01秒。
二、作业要求：1、以经度为横轴，纬度为纵轴（单位均转换为：度）作出系统位置坐标曲线图并附在报告中；2、以列表形式给出系统纬度、经度、东向速度、北向速度的终点值；3、作业以纸质报告形式提交，在报告中写“程序流程图”和小结（小结不要写套话，按条简捷的写），报告附源程序，封皮写明联系方式。
三、程序流程四、结果五、总结本次作业是处理实际数据然后得到导航结果，以此对之前所学的进行了一下总结。
由于时间问题，对加速度和速度的积分采用的是欧拉法，相比四阶龙格库塔法，这种方法计算简单但精度较低。
遗憾的是并没有比较两者的差距。
计算过程中发现Z方向速度不为零，即高度并不像假设的是恒定不变的。
六、源程序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);%定义存放子午圈曲率半径数据的矩阵L(1,1)=39.981430918136/180*pi;%纬度初始值单位：弧度Lambda(1,1)=116.344762072818/180*pi;%经度初始值单位：弧度Vx(1,1)=0;%初始速度x方向分量Vy(1,1)=0;%初始速度y方向分量Vz(1,1)=0;%初始速度z方向分量fx=f_INSc(1,1:end);%x方向的比力数据fy=f_INSc(2,1:end);%y方向的比力数据fz=f_INSc(3,1:end);%z方向的比力数据g0=9.78049;Wie=7.2921E-5;%地球自转角速度Re=6378245;%长半径e=1/298.3;%椭圆度t=0.01;%采样时间for i=1:60000g=g0*(1+0.0052884*sin(L(1,i))^2-0.0000059*sin(2*L(1,i))^2);%重力加速度Rx(1,i)=Re/(1-e*(sin(L(1,i)))^2);%根据纬度计算卯酉圈曲率半径Ry(1,i)=Re/(1+2*e-3*e*(sin(L(1,i)))^2);%根据纬度计算子午圈曲率半径Vx(1,i+1)=(fx(1,i)+(2*Wie*sin(L(1,i))+Vx(1,i)*tan(L(1,i))/Rx(1,i))*Vy(1,i)-(2 *Wie*cos(L(1,i))+Vx(1,i)/Rx(1,i))*Vz(1,i))*t+Vx(1,i);%计算速度x方向分量Vy(1,i+1)=(fy(1,i)-(2*Wie*sin(L(1,i))+Vx(1,i)*tan(L(1,i))/Rx(1,i))*Vx(1,i)+V y(1,i)*Vz(1,i)/Ry(1,i))*t+Vy(1,i);%计算速度y方向分量Vz(1,i+1)=(fz(1,i)+(2*Wie*cos(L(1,i)+Vx(1,i))/Rx(1,i))*Vx(1,i)+Vy(1,i)*Vy(1 ,i)/Ry(1,i)-g)*t+Vz(1,i);%计算速度z方向分量L(1,i+1)=t*Vy(1,i)/Ry(1,i)+L(1,i);Lambda(1,i+1)=t*Vx(1,i)/(Rx(1,i)*cos(L(1,i)))+ Lambda(1,i);endLend=L(1,60001)*180/piLambdaend= Lambda(1,60001)*180/piVxend=Vx(1,60001)Vyend=Vy(1,60001)plot(Lambda*180/pi,L*180/pi);xlabel('经度/°'),ylabel('纬度/°');grid on。
作业要求
《惯性导航原理》第一次大作业1 数据说明惯导系统为指北方位的平台系统，运动过程中系统高度不变。
(1) 系统初始信息初始位置经度（º）纬度（º）高度（m）116.34369207640.16256540237.74319初始速度东向速度（m/s）北向速度（m/s）天向速度（m/s）0.00.00.0初始姿态航向角（º）俯仰角（º）横滚角（º）152.08690.5894-0.8758(2) 数据格式文件fw.mat中保存的为比力信息f（单位：m/s^2）和陀螺仪角速率信息w（单位：rad/s）,排列顺序为一~三行分别为东、北、天向信息,共1200秒数据，陀螺仪和加速度计采样周期为0.01秒。
使用MATLAB编程时可使用如下形式的语句读取数据：“load 文件路径...\fw”,可得到变量名为f的比力信息和变量名为w的陀螺仪角速率信息。
文件fw.dat中数据与fw.mat文件中数据相同，只是数据格式不同，fw.dat数据：第一列：数据包序号，第二至四列：分别为东、北、天向陀螺仪角速率信息w（单位：rad/s），第五至七列：分别为东、北、天向比力信息f（单位：m/s^2）.用MATLAB编程时使用如下形式的语句读取数据：a=load('文件路径/fw.dat');w =a(:,2:4)';f =a(:,5:7)';可得到变量名为f的比力信息和变量名为w的陀螺仪角速率信息(与fw.mat格式文件得到的信息相同)。
提示：导航过程中只考虑水平通道。
2 作业要求作业以纸质报告形式提交。
具体要求为在报告第一页上写明本人姓名、学号、联系方式，报告中包含以下内容一、“原理分析”给出对本次作业相关原理及公式的具体分析。
二、“程序流程图”在原理分析基础上画出程序流程图，并对程序流程图结构进行文字说明。
三、“导航结果”以经度为横轴，纬度为纵轴（单位均转换为：度）作出系统位置坐标曲线图；以时间为横轴（单位：秒），东向和北向速度为纵轴作出系统速度随时间变化曲线图；以列表形式给出系统纬度、经度、东向速度、北向速度的终点值。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
惯性导航期末大作业
14171106 苗家语
1.平台式惯导系统，求导航坐标系n 系与计算机坐标系c 系的区别和转换矩阵，设c 系相对n 系的误差角为()x y z δθδθδθ
答：n 系与c 系误差角为：δϕθ-=x ，ϕδλθcos y =，
ϕδλθsin z =，三个欧拉角矩阵相乘，近似得到转换矩阵：⎪⎪⎪⎭⎫ ⎝⎛---=111z z x y x y c n C δθδθδθδθδθδθ
2.以四元数相乘的法则计算两个矢量P 和Q 的四元数相乘的结果。
答：矢量P =i a i a i a 332
211++ 矢量Q=i b i b i b 3
32211++ P·Q=- (b a b a b a 332211++) + ⎪⎪⎪⎭
⎫ ⎝⎛b b b a a a i i i 321321321= -P·Q + P ×Q 四元数A=→+P a 0 B=→
+Q b 0
则AB=→→→→→→⨯++++Q P Q P P b Q a b a 0000
3.某刚体沿自身k 轴旋转30°得到i'-j'-k'刚体，请分别在i-j-k 坐标系和i'-j'-k'坐标系给出v1=i+j+k 和v2=i'+j'+k'的关系。
答：在i-j-k 坐标系中
v2相当于v1绕k 轴旋转30°
q=cos15°+k sin15°
q v q v 112-= 在i'-j'-k'坐标中
v1相当于v2绕k 轴旋转-30°
q ’=cos15°- k sin15°=q 1-
所以q v q v 21
1-=
4.以3-1-2顺序定义欧拉角旋转，请采用四元数方法，计算载体的欧拉角（即已知载体初始姿态与地理坐标系重合），且已获得当前瞬时四元数q(t)，求欧拉角。
答：不太会做
5.证明||||q *
1
q q =-
证明：四元数范数的定义为23
22212*qq ||q ||p p p +++==λ
得||
||q 1*
q q =
逆四元数定义为q
1q
1=- ||||q 1q *
1
q q ==- 证明完毕
6.求证两个矢量αβ
、， =1()21()2αβαβαβ
αβαββααβαββα⊗-⋅+⨯⋅=-⊗+⊗⨯=⊗-⊗
证明：第一个等式在第二题已经证明，将PQ 换成αβ即可。
βαβαβα ⨯+∙=⊗-得证
可以写出：αβαβαβ
⨯+∙=⊗- 两者相加可得： ）（αββαβα
⊗+⊗=∙21- 两者相减可得 ）（αββαβα
⊗⊗=⨯-21
7.求从（0°E ， 0°N ）地理坐标到（45°W ， 45°N ）地理坐标的转动四元数以及两个坐标系之间的转换方向余弦矩阵。
答：
由题可知：︒-=45x θ︒=45y θ0z =θ
转动镜像四元数可以写为： 2222q 1i -=
2222q 2j +=
1
3=q 合成转动四元数为：k j i q q q 212121-21321-+=
方向余弦矩阵为⎪⎪⎪⎭⎫ ⎝⎛⎪⎪⎪⎭⎫ ⎝⎛-⎪⎪⎪⎭⎫ ⎝⎛-=10001000145-cos 45-sin 045-sin 45-cos 000145cos 045sin 01045sin 045cos ）（）（）（）（A =⎪⎪⎪⎪⎪⎪⎪⎭⎫ ⎝
⎛21212222-22021-
21-22
8.地球自转角速度在以下三处地理坐标系（0°E ， 30°N ），（30°E ，0°N ），（45°W ，30°S ）的投影分量。
已知公式：
⎪⎩⎪⎨⎧=⎪⎩⎪⎨⎧=ϕωϕ
ωωωωωsin cos 0ie ie t iez t iey t iex t ie
其中ϕ为当地的纬度，ie ω为地球自转角速度。
（0°E ， 30°N ） ⎪⎪⎪⎩
⎪⎪⎪⎨⎧=ie ie t ie ωωω21230
（30°E ，0°N ） ⎪⎩
⎪⎨⎧=00ie t ie ωω
（45°W ，30°S ） ⎪⎪⎪⎩⎪⎪⎪⎨⎧=ie ie t ie ωωω21-230。