北航惯性导航期末大作业
北航惯性导航综合实验四实验报告
基于运动规划的惯性导航系统动态实验二零一三年六月十日实验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 。
- 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。