北航惯性导航综合实验五实验报告
惯性导航实验
惯性导航实验一、 实验目的1、了解惯性导航设备;2、掌握惯性导航设备的物理连接；3、掌握惯性导航信息的处理方法；4、掌握惯性导航方法并学会用编程实现惯性导航算法。
二、 实验器材YH-5000AHRS ；工业控制计算机；数据采集软件； 稳压电源；串口连接线； 三、 实验原理 （1） 姿态解算基于四元数法解算姿态矩阵。
p j p i p l Q +++=21 （1）⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡--++----+++---+=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡b b b p b b b b p p p z y x C z y x p p p l lp p p lp p p lp p p p p p l lp p p lp p p lp p p p p p l z y x 222123213223113223212223212313212322212)(2)(2)(2)(2)(2)(2 （2） b pbQw Q 21= （3） 上述微分方程表示成矩阵形式：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡------=⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡321321000021p p p l w w w w w w w w w w w w p p p l b pbxb pbyb pbzb pbxb pbz bpbyb pby b pbz b pbx b pbz b pbyb pbx（4）初始四元数的确定计算如下：⎥⎥⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎢⎢⎣⎡--++=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡2cos 2cos 2sin 2sin 2sin 2cos 2cos 2sin 2sin 2sin 2cos 2cos 2sin 2cos 2sin 2cos 2sin 2cos 2sin2sin2sin2cos2cos2cos )0(3)0(2)0()0(0000000000000000000001γθϕγθϕγθϕγθϕγθϕγθϕγθϕγθϕG G G G G G G G p p p l （5） 用四阶龙格库塔法解（4）的微分方程；⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=333231232221131211T T T T T T T T T C p b由p b C 中提取γϕλ，，G231sin T -=主λ22211tan T T G -=主ϕ )(tan 33131T T --=主γ 从而可得到：主λλ=⎪⎩⎪⎨⎧<>+>><+=0,020,002122212222T T T T T GG G G πϕϕπϕϕ主主主⎩⎨⎧<>=0,)(-0331333T T sign T πγγγ （2） 速率位置解算将加速度测量的沿坐标系轴向的比力bib a 转换成沿着导航坐标系轴向的比力p ib a ,则速度方程为:p p epp ep p ib p ep g V w a V +⨯+Ω-=)2( 展开得到：⎪⎩⎪⎨⎧-+Ω-+Ω+=+Ω+Ω-=+Ω-Ω+=gV w V w a V V w V a V V w V a V p epy p epx p x p epx p epy p y p ibz p epzp epz p epx p x p epx p z p iby p epy p epzp epy p y p epy p z p ibx p epx )2()2()2(2)2(2 由于Ω,pep w 都很小，故而速度方程简化为：⎪⎩⎪⎨⎧-===ga V a V a V p ibz pepz piby p epy p ibxp epx用一阶欧拉法解，则：⎪⎩⎪⎨⎧+-=++=++=+)(*)()()(*)()(*)(t V T g a T t V t V T a T t V t V T a T t V p epz p ibz p epzpepy p iby p epy pepx p ibx p epx 其中T 为采样时间。
北航惯性导航综合实验四实验报告
基于运动规划的惯性导航系统动态实验二零一三年六月十日实验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 控制板评估测试系统检查系统是否正常工作。
北航惯性导航综合实验五实验报告
惯性导航技术综合实验实验五惯性基组合导航及应用技术实验惯性/卫星组合导航系统车载实验一、实验目的①掌握捷联惯导/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 的惯导解算的北向最大位移误差，东向最大位移误差，可见实验数据所得位置误差与理论推导的位置误差在同一数量级，结果不完全相同是因为理论推导时做了大量简化，而且实验时视GPS 为真实值也会带来误差；另外，可见1min 内纯惯导解算的东向速度最大误差s ，北向速度最大误差s 。
惯导实习报告
一、前言惯性导航系统（Inertial Navigation System，简称INS）是一种基于物体自身运动状态进行导航定位的系统。
在军事、民用等领域具有广泛的应用。
为了深入了解惯导系统的原理和应用，我们于近期进行了惯导实习。
以下是对本次实习的总结和报告。
二、实习目的1. 了解惯性导航系统的基本原理和组成；2. 掌握惯导系统的安装、调试和操作方法；3. 通过实际操作，提高动手能力和解决实际问题的能力；4. 为今后从事相关领域的工作奠定基础。
三、实习内容1. 惯性导航系统原理（1）惯性导航系统概述惯性导航系统是利用物体惯性原理进行导航定位的一种系统。
它通过测量物体运动过程中的加速度、速度和位置等参数，实时计算出物体的运动轨迹和位置。
（2）惯性导航系统组成惯性导航系统主要由惯性测量单元（IMU）、数据处理单元和显示单元组成。
2. 惯导系统安装与调试（1）安装将惯导系统按照说明书要求安装到试验平台上，确保安装牢固。
（2）调试连接电源和通信线，启动系统，进行自检。
检查各部件工作状态，确保系统正常运行。
3. 惯导系统操作（1）启动系统按下启动按钮，系统开始工作。
（2）输入初始数据输入起始位置、速度和航向等初始数据。
（3）实时监测观察系统实时显示的加速度、速度和位置等信息，分析系统工作状态。
（4）数据记录记录实验过程中各参数的变化情况，为后续分析提供依据。
四、实习总结1. 通过本次实习，我们掌握了惯性导航系统的基本原理和组成，了解了惯导系统的安装、调试和操作方法。
2. 在实际操作过程中，我们遇到了一些问题，如系统不稳定、数据误差等。
通过查阅资料和请教指导老师，我们找到了解决问题的方法，提高了自己的动手能力和解决问题的能力。
3. 本次实习使我们认识到，惯性导航系统在实际应用中具有重要意义，为今后从事相关领域的工作打下了基础。
五、心得体会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 的位子及位子缺点图：（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');。
惯性导航课程实验报告问问
一.实验目的1．认识三轴惯性平台的各个组成器件2.讨论验证三轴平台的工作原理，并对其稳定回路及工作过程做出分析二．实验原理一个双自由度陀螺有两个测量轴，可为平台提供两个轴的稳定基准，而三轴平台要求陀螺为平台提供三个轴的稳定基准，所以三轴平台需要两个双自由度陀螺。
设两个陀螺的外环轴均平行于平台的方位轴安装，则内环轴自然平行于平台平面。
在正常工作状态下，两个陀螺的自转轴也平行于平台台面，且相互之间保持垂直关系，即两个陀螺的内环轴之间也保持垂直关系。
两个陀螺的内环轴作为平台绕两个水平轴稳定的基准，而两个陀螺的外环轴之一，作为平台绕方位轴稳定的基准。
三.实验内容1.方位稳定轴的空间积分状态在双自由度陀螺构建的三轴惯性平台中，平台的方位稳定回路陀螺2外环轴上的信号器，放大器，平台方位轴上的稳定电机等组成。
当干扰力矩作用在平台的方位轴上时，平台绕方位轴转动偏离原有的方位，而平台上的陀螺却具有稳定性。
这样，平台相对陀螺外环出现了偏转角，陀螺2外环轴上的信号器必有信号输出，经放大器放大后送至平台方位轴上的稳定电机，方位稳定电机输出稳定力矩作用到平台方位轴上，从而平衡作用在平台方位轴上的干扰力矩，使平台绕方位轴保持稳定。
同样，给陀螺2内环轴上的力矩器输入与指令角速度大小成比例的电流，可实现方位稳定轴的空间积分要求2.水平稳定回路的工作如下图所示由三个单轴平台直接叠加的三轴平台在航向变化时，平台上的陀螺与稳定电机之间的相对位置关系．图(a)表示航向为零，即方位环环对俯仰环没有转角时陀螺与稳定电机之间的相对位置关系，此时的陀螺Ⅱ感受沿横滚轴（纵向）方向作用到平台上的干扰力矩，信号器输出的信号经横滚放大器A．放大后给横滚轴稳定电机，产生纵向稳定力矩，使平台沿纵向（x.轴）保持稳定，陀螺I感受沿俯仰轴（横向）方向作用到平台上的干扰力矩。
经信号器．放大器和俯仰轴稳定电机，产生沿横向的稳定力矩．使平台沿横向保持稳定。
同样，若给两个陀螺的力矩器输入与指令角速度成比例的电流，平台也可正常工作在空间积分状态。
北航研究生惯性导航技术综合实验报告
角 速 率 （ /°s）
按照公式： Bs
1 KG
1 n n 1 i1
Fi F
2
1/ 2
，计算零漂，结果为：
1 秒平滑零漂 10 秒平滑零漂 100 秒平滑零漂
0.0214°/s 0.0075°/s 0.0028°/s
2
计算源程序：
d1=sum(reshape(data,5,[]))/5;
角 速 率 （ /°s）
角 速 率 （ /°s）
-0.9 1s平 滑
-1
-1.1 0
-0.95 -1
100
200
300
400
500
600
时 间 /s
10s平 滑
-1.05 0
-1 -1.02
100
200
300
400
500
600
时 间 /s
100s平 滑
-1.04 100 150 200 250 300 350 400 450 500 550 600 时 间 /s
4. 零偏重复性测试 a. 令转台某角速度下进行正转，转速平稳后，采集陀螺输出数据，并保存； b. 令转台某角速度下进行反转，转速平稳后，采集陀螺输出数据，并保存； c. 按计算陀螺零偏； d. 关掉陀螺电源，并重新启动，重复步骤 a、b； e. 重复步骤 d 进行 3-5 次，共得到陀螺零偏 5-7 个； f. 对 5-7 个陀螺零偏求均方差，得零偏重复性指标。
三、实验仪器
双轴位置转台、MEMS 加速度计、稳压电源、数据采集系统和分析系统。
四、实验步骤
1. 测试前工作 a．把加速度计安装到转台上，使其敏感轴垂直于工作台面； b．连接加速度计的各信号线和电源线； c．测试加速度计电缆是否正常连接； d．检查加速度计 5V 电源是否正常； e．启动数据采集与测试系统，并检查是否正常，正常后关闭。
北航惯性导航综合实验三实验报告
北航惯性导航综合实验三实验报告惯性导航技术综合实验实验三惯性导航综合实验实验3.1 初始对准实验一、实验目的结合已经采集并标定好的惯性传感器数据进行粗对准，了解实现对准的过程；通过比较不同传感器数据的对准结果，进一步认识惯性传感器性能在导航系统中的重要地位。
为在实际工程设计中针对不同应用需求下采取相应的导航系统方案提供依据。
二、实验内容利用加速度计输出计算得到系统的初始姿态，利用陀螺输出信号计算航向角。
对比利用不同的惯性传感器数据计算所得的不同结果。
三、实验系统组成MEMS IMU（或其他类型IMU）、稳压电源、数据采集系统与分析系统。
四、实验原理惯导系统在开始进行导航解算之前需要进行初始对准，水平对准的本质是将重力加速度作为对准基准，其对准精度主要取决于两个水平加速度计的精度，加速度计的零位输出将会造成水平对准误差；方位对准最常用的方位是罗经对准，其本质是以地球自转角速度作为对准基准，影响对准精度的主要因素是等效东向陀螺漂移。
(1) 其中，分别为当前时刻的俯仰角和横滚角计算值。
1/ 15水平对准误差和方位对准误差如下所示：，(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:120XX年7 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;2/ 15theta(i)=asin(ay0(i)/az0(i)); gama(i)=-asin(ax0(i)/az0(i)); end detfaix=mean(ay0)/g; detfaiy=mean(-ax0)/g; t=1:120XX年7; 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.实验准备：准备好所需的惯性测量单元（IMU）以及与IMU配套的计算设备或软件。
2.安装与校准：将IMU安装在需要进行定位解算的平台上，例如车辆、飞机或机器人。
然后对IMU进行校准，确保其能够准确测量姿态、角速度和加速度。
3.数据采集：启动数据采集设备或软件，开始记录IMU输出的原始数据。
同时，获取参考位置信息，可以通过全球定位系统（GPS）或其他外部定位设备来提供参考位置。
4.解算算法：使用惯性导航算法进行位置解算。
常见的算法包括卡尔曼滤波算法、扩展卡尔曼滤波算法或粒子滤波算法。
这些算法会利用IMU测量的姿态、角速度和加速度信息，结合参考位置信息，推算出当前的位置。
5.结果评估：将解算得到的位置与参考位置进行比较，并评估定位误差。
可以计算误差指标，如均方根误差（RMSE）或平均绝对误差（MAE），来衡量解算的准确性。
需要注意的是，惯性导航系统在长时间使用过程中可能存在累积误差的问题。
为了提高定位精度，可以结合其他定位技术，如GPS、地标识别或视觉定位，进行融合定位。
这样可以综合利用多种传感器的信息，提高定位的精度和可靠性。
惯性试验实验报告
一、实验目的1. 了解惯性的概念和特点。
2. 通过实验验证惯性的存在和表现。
3. 掌握惯性实验的基本方法和步骤。
4. 培养学生的动手操作能力和观察能力。
二、实验仪器1. 小车一辆2. 水平桌面一块3. 测量尺一把4. 秒表一个5. 纸张若干6. 粘土适量三、实验步骤1. 准备实验器材：将小车、水平桌面、测量尺、秒表、纸张和粘土准备好。
2. 将小车放置在水平桌面上，确保小车平稳。
3. 使用测量尺测量小车前进的距离，记录下来。
4. 使用秒表记录小车前进的时间，记录下来。
5. 在小车的尾部放置一块粘土，模拟增加小车的质量。
6. 重复步骤3和步骤4，记录小车前进的距离和时间。
7. 比较增加质量前后小车前进的距离和时间，分析惯性的影响。
8. 将实验结果整理成表格，并进行数据处理。
9. 根据实验结果，分析惯性的特点和表现。
四、实验结论1. 实验结果表明，小车在增加质量后，前进的距离和时间发生了变化。
2. 增加质量后，小车前进的距离变短，时间变长。
3. 这说明惯性的存在，质量越大，惯性越大。
4. 惯性是物体保持原有运动状态的性质，与物体的质量有关。
五、反思体会1. 本次实验让我对惯性的概念有了更深入的了解。
2. 通过实验，我学会了如何进行惯性实验，掌握了实验的基本方法和步骤。
3. 在实验过程中，我注意到了实验数据的准确性，培养了严谨的科学态度。
4. 通过观察和分析实验结果，我认识到惯性的特点和表现。
5. 在今后的学习和生活中，我会运用惯性知识，解决实际问题。
六、实验拓展1. 探究不同质量的小车在相同条件下，前进的距离和时间的变化。
2. 研究惯性与速度、加速度之间的关系。
3. 利用惯性原理，设计简单的惯性玩具。
4. 将惯性知识应用于实际生产生活中，提高生产效率。
5. 深入研究惯性的相关理论，为我国物理学的发展贡献力量。
总结：本次惯性试验实验让我对惯性的概念和特点有了更深入的了解，提高了我的动手操作能力和观察能力。
北航惯性导航综合实验二实验报告
惯性测量单元安装误差系数标定实验二零一三年六月十日惯性测量单元安装误差系数标定试验一、实验目的1、掌握惯性测量单元（inertial measurement unit ，IMU ）的标度系数、安装误差、零偏的标定方法；2、利用现有实验条件实现实验过程的设计。
二、实验内容利用单轴速率转台，进行IMU 的安装误差系数标定，并通过公式计算该安装误差系数。
三、实验系统组成单轴速率位置转台、MEMS 惯性测量单元、稳压电源、数据采集系统。
四、实验原理IMU 安装误差系数的计算方法通常，惯导系统至少需要三个陀螺和三个加速度计，用以感知载体的三轴角速度和加速度变化。
将这些陀螺和加计按照敏感轴两两正交的方式集成在一起，安装在一个结构框架上，便构成了一个能感知完整惯性测量信息的小型系统，称之为惯性测量单元。
对惯性测量单元进行标定时，除了要对其中的陀螺、加速度计进行常规标定外，还要考虑由于安装时不能严格保证敏感轴两两正交所带来的交叉耦合误差，即，要对IMU 的安装误差进行标定，测量出不正交角。
因此，在考虑IMU 的安装误差、标度因数误差、零偏误差的情况下，建立东北天坐标系下IMU 的角速度通道误差方程。
x x xx xy xz x y y yxyy yz y z z zx zy zz z K E E E K E E E K ωεωωεωωεω⎡⎤⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=+⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦⎣⎦ (1)式中i ω为惯性系统i 轴向陀螺输出角速度，i ω为i 轴向的输入角速度；i ε为i 轴向陀螺零偏；ii K 为i 轴向陀螺标度因数；ij E 为角速度通道的安装误差系数，i 和j 为坐标轴X ，Y ，Z 的统称。
设输入矩阵为x1xn y1yn I z1zn ...11ωωωωωω⎡⎤⎢⎥⎢⎥Ω=⎢⎥⎢⎥⎣⎦，输出矩阵为x1xn o y1yn z1zn ...ωωωωωω⎡⎤⎢⎥Ω=⎢⎥⎢⎥⎣⎦，则标度因数、安装误差系数与陀螺漂移组成的矩阵可按最小二乘法估计为： 类似，可计算加速度计的标度因数、安装误差系数与加计零偏。
北航惯性导航综合实验五实验报告
惯性导航技术综合实验实验五惯性基组合导航及应用技术实验惯性/卫星组合导航系统车载实验一、实验目的①掌握捷联惯导/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 。
【2018-2019】导航技术综合实验报告word版本 (16页)
本文部分内容来自网络整理，本司不为其真实性负责，如有异议或侵权请及时联系，本司将立即删除！== 本文为word格式，下载后可方便编辑和修改！ ==导航技术综合实验报告导航技术综合实验报告201X年4月23日星期一一、实验目的理论：基本掌握解码星历表（数制转换）、求伪距观测量、求卫星位置和卫星钟差、最小二乘法求接收机位置的概念、方法和运算的过程。
实践：通过对解码星历表（数制转换）、求伪距观测量、求卫星位置和卫星钟差、最小二乘法求接收机位置的程序仿真实际掌握方法。
二、实验的方法和步骤实验方法：结合课本的理论知识使用matlab仿真软件完成实验。
实验步骤：1、仔细阅读课本基本了解理论知识，掌握基本问题的解决方法与方式。
2、认真研究老师所给实验题目，明确实验要求和目的，确立完成实验的基本思路。
3、对照实验题目回顾书本理论知识，结合理论知识确定解决问题的方式方法。
结算出理论值。
4、绘制原理框图，方案流程图。
5、确立仿真环境。
6、绘制流程图‘7、编程调试，得到结果与理论推导结果做对比。
三、实验结果实验9 解码星历表（数制转换）一．函数模块解释函数功能：根据导航数据位信息，将从子帧1，子帧2和子帧3中得到星历数据转换成十进制格式，并求第一子帧的时间周TOW。
一个子帧包含300数据位，导航信息共5个子帧。
函数名称：function [eph, TOW] = ephemeris(bits, D30Star)1.实验要求：实验代码见实验9中的ephemeris.m文件，理解实现星历解算的代码，并把函数代码和流程图中的各个环节对应起来。
1、获得输入的导航数据信息function [eph, TOW] = ephemeris(bits, D30Star)利用function形式，调用子函数，输入参数。
2、检查导航数据的bits长度是否满足导航数据5个子帧的长度要求if length(bits) < 1500error('The parameter BITS must contain 1500 bits!');end3、检查参数bits和D30Star是否是字符串if ~ischar(bits)error('The parameter BITS must be a character array!');endif ~ischar(D30Star)error('The parameter D30Star must be a char!');end4、循环操作，一一对5个子帧进行解码。
北京航空航天大学科技成果——自主实时机载地形辅助惯性导航方法和系统
北京航空航天大学科技成果——自主实时机载地形辅助惯性导航方法和系统成果简介地形辅助导航（Terrain Aided Navigation，TAN）由于具有自主性、隐蔽性、抗干扰能力强和全天候工作的优点，其在组合导航系统中受到广泛的重视和研究，并已在巡航导弹、飞机和舰船上成功应用。
TAN 的定位思想是利用实测的运动轨迹下方的地形高程数据和存储的数字地形高程模型（Digital Elevation Model，DEM）为信息，采用地形匹配算法来达到定位的目的。
地形辅助导航系统的定位精度和实时性不仅与地形匹配算法有关还受地形特征的影响。
在地形特征较明显的山地和丘陵地区定位准确度较高，在平原或海平面这些地形特征不明显的区域匹配就不适合执行匹配定位。
由于受地形匹配算法和地形特征的限制，目前地形辅助导航系统的应用，大都需要事先根据飞行任务规划飞行路线、选取匹配区域，使得载体机动飞行和实时定位的能力受限。
如何实现载体在飞行过程中自主选择匹配区域并根据飞行状态和所规划区域的地形特征自动执行合适的地形匹配算法，将极大的提高载体机动飞行和实时地形辅助定位的能力从而摆脱受事先规划的飞行轨迹的束缚，这也将有利于地形辅助导航的推广和应用。
本项目开发了一种实时机载地形辅助导航方法和系统，以解决目前地形辅助导航系统需要事先根据飞行任务规划飞行路线、选取匹配区域，使得载体机动飞行和实时定位的能力受限的问题。
对于传统的地形辅助惯性导航系统，本项目引入匹配区域规划与适应性分析模块，可实现载体在飞行过程中自主规划选取匹配区域，并根据不同地形区域和飞行状态选择合适的地形匹配算法进行匹配定位，从而提高载体机动飞行和实时定位的能力。
为确保匹配定位算法在跟踪模式下定位结果的可靠性，采用基于位置序列估值与滤波值比较的方法确保定位结果的可靠性。
本发明可以提高载体机动飞行和实时定位的能力，从而解决目前地形辅助导航系统需要事先根据飞行任务规划飞行路线和匹配区域导致载体机动飞行和实时定位能力受限的问题。
北航_仪器光电综合实验报告_水平对准及陀螺测角实验
［键入公司名称］［键入文档标题］2012/4/20水平对准实验实验时间：2012年4月20日星期五实验地点：新主楼B628（一）实验目的利用加速度计进行水平对准，掌握加速度计输出信号的内涵，水平对准的基本原理及方法。
（二）实验原理加速度计测角原理加速度计是惯性导航与惯性制导系统的重要敏感元件。
其输出的是比力信号，比力信号中包括运动加速度和引力加速度两部分，其数值为运动加速度与引力加速度之差。
其中运动加速度反映了物体运动状态（速度）变化，而引力加速度是物体在万有引力作用下产生的加速度（如近地物体受到重力加速度）。
导航定位时是利用加速度计输出信号中的运动加速度进行积分计算速度与位置，引力加速度做为有害加速度补偿掉。
在用加速度进行水平对准时，则把运动加速度做为有害加速度，而利用引力加速度与重力加速度的比值还计算水平姿态角。
通常加速度计输出对应一定的电压，表示为：U A（f） U A（0） af f A（g）A（1）式中U A（ f ）是载体加速度f对应的加速度计输出电压，U A （0）是f为零时的加速度计输出电压，K A是加速度计标度因数, f A（g）是与加速度有关的误差项，A是加速度计的随机误差。
f A（g）表示与比力有关的加速度计输出误差函数，包括与比力的一次方或多次方的关系，反映了加速度计输出与比力的关系。
本书不研究此项误差。
因此，（1）式简化为:U A（f）U A（0）K A f A（2）由式（2）得比力：U A U A（0）f A A A（3）K A对于数子输出的加速度计，传感器内部已经利用标度因数对加速度计模拟输出进行了量化，直接输出比力值。
f测量=f O f真值+ A （4）f o是加速度计的常值偏置，物理意义是比力真值为零时，加速度主的输出。
其值与U A（0）对应，且U A（O）=K A f o（5）加速度计输出的比力中包含了运动加速度和引力加速度两部分:与水平面有角度 时，有那么水平姿态角可表示为:在为小角度时，式（8）可近似为:利用加速度计敏感地球重力加速度分量，计算水平姿态角，实现水平对准。
惯性实验集合实验报告
1. 理解惯性的概念和性质；2. 掌握惯性实验的方法和步骤；3. 通过实验验证惯性的规律，提高实验操作技能。
二、实验原理惯性是物体保持其原有运动状态（静止或匀速直线运动）的性质。
当物体受到外力作用时，若外力不足以改变物体的运动状态，则物体将保持原有状态。
本实验通过测量物体在水平面上滑行的距离，验证惯性的规律。
三、实验仪器1. 惯性秤；2. 米尺；3. 水平轨道；4. 金属块；5. 小车；6. 秒表。
四、实验步骤1. 将水平轨道放置在平稳的桌面上，确保轨道与桌面垂直；2. 将金属块放置在轨道的一端，将小车放置在金属块旁边；3. 将惯性秤放置在轨道上，调整至平衡状态；4. 用米尺测量金属块与小车之间的距离；5. 将小车放在金属块上，松开金属块，使小车沿轨道滑行；6. 用秒表记录小车滑行的距离和时间；7. 重复步骤5和6，共进行5次实验；8. 记录每次实验的数据。
实验次数 | 金属块与小车距离（cm） | 小车滑行距离（cm） | 小车滑行时间（s）------- | -------- | -------- | --------1 | 20 | 10 | 1.22 | 20 | 9 | 1.13 | 20 | 8 | 1.04 | 20 | 7 | 0.95 | 20 |6 | 0.8六、实验结果与分析根据实验数据，我们可以得出以下结论：1. 在金属块与小车距离不变的情况下，小车滑行的距离随时间的增加而减小；2. 在金属块与小车距离不变的情况下，小车滑行的时间随时间的增加而减小；3. 在金属块与小车距离不变的情况下，小车滑行的距离与时间的比值基本保持不变。
这些结论与惯性的规律相符。
惯性使得物体在受到外力作用时，保持原有运动状态，因此小车在滑行过程中，速度逐渐减小，最终停止。
七、实验误差分析1. 实验过程中，由于人为操作误差，可能导致测量结果存在偏差；2. 惯性秤的精度可能影响实验结果的准确性；3. 水平轨道的倾斜度可能对实验结果产生影响。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
惯性导航技术综合实验实验五惯性基组合导航及应用技术实验惯性/卫星组合导航系统车载实验一、实验目的①掌握捷联惯导/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 。
(二) 选取IMU 前5分钟数据进行对准实验。
将初始对准结果作为初值完成1小时捷联惯导和组合导航解算，对比1小时捷联惯导和组合导航结果。
1、5minIMU 数据的解析粗对准结果：-1.1960 -0.9998 -0.0084 0.9979 -1.1990 -0.0163 0.0062 0.1198 0.9991Cnb ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦attitude =[219.7700 -0.9318 0.4828]2、5minIMU 数据的Kalman 滤波精对准结果：00.511.522.53x 104航向角0.01秒度x 104俯仰角0.01秒度0.511.522.53x 104横滚角0.01秒度fai = 219.9744642380873*pi/180;theta = -0.895865732956914*pi/180;gama = 0.640089448357591*pi/180;x 104-4北向速度误差0.01秒米/秒x 104-3北向速度误差方差0.01秒米/秒x 104-4东向速度误差0.01秒米/秒x 104-3东向速度误差方差0.01秒米/秒x 104-3北向失准角0.01秒度x 104-3北向失准角方差0.01秒度x 104-3东向失准角0.01秒度x 104-3东向失准角方差0.01秒度x 104-5天向失准角0.01秒度x 104-3天向失准角方差0.01秒度3、一小时IMU/GPS 数据的组合导航结果图及估计方差P 阵图：x 105纬度0.01s °x 105经度0.01s °x 105高度0.01smx 105东向速度0.01s m /sx 105北向速度0.01s m /sx 105天向速度0.01sm /sx 105航向角0.01s °x 105俯仰角0.01s °x 105横滚角0.01s°x 105-4P 航向角误差0.01s度x 105-4P 俯仰角误差0.01s度x 105-3P 横滚角误差0.01s度x 105-3P 东向速度误差0.05sm /sx 105-3P 北向速度误差0.05sm /sx 105-3P 天向速度误差0.01sm /sx 105-8P 纬度误差0.01s 度x 105-8P 经度误差0.01s 度x 105P 高度误差0.01sm4、一小时IMU 数据的捷联惯导解算结果与组合滤波、GPS 输出对比图：x 105lat0.01s 度00.51 1.52 2.53 3.54x 105lon0.01s 度54height0.01sm5、结果分析：由滤波结果图可以看出：（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.1467;Re = 6378135.072;g = 9.7803267714;e = 1.0 / 298.25;T = 0.01; %IMU频率100hz,此程序中GPS频率100hz datanumber = 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);lat = 40.46*pi/180; %组合导航的初始位置、姿态、速度lon =116.60*pi/180;height =43.0674;fai = 219.73*pi/180;theta = -0.6914*pi/180;gama = 0.7591*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/3600)^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.0000 5^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/18 0/3600)^2,(50e-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) 0tan(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; %纬经度化为位移，单位m Z(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滤波估计得出的失准角theta Agama = X_k(2); %kalman滤波估计得出的失准角gama Afai = X_k(3); %kalman滤波估计得出的失准角fai Ctn = [ 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');。