《卡尔曼滤波与组合导航课程实验报告》由会员分享，可在线阅读，更多相关《卡尔曼滤波与组合导航课程实验报告（20页珍藏版）》请在金锄头文库上搜索。
1、卡尔曼滤波与组合导航课程实验报告（总18页）-本页仅作为文档封面，使用时请直接删除即可-内页可以根据需求调整合适字体及大小-卡尔曼滤波与组合导航课程实验报告实验捷联惯导/GPS组合导航系统静态导航实验实验序113姓名陈星宇系院专17班级ZY11172学号号ZY1117212日期业2012-5-15指导教宫晓琳成绩师一、实验目的 掌握捷联惯导/GPS组合导航系统的构成和基本工作原理； 掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理; 掌握捷联惯导/GPS组合导航系统静态性能； 了解捷联惯导/GPS组合导航静态时的系统状态可观测性；二、实验原理（1）系统方程X二FX + GWX =6 -
2、 8 v 8 v 8 v_r E tN UE N U其中，、为数学平台失准角EN U888 V V Vx y z x y z；8 v、8v、8v分别为载体的东向、北向和天EN向速度误差；8L、8九、8h分别为纬度误差、经度误差和高度误差； V、V、V分别为陀螺随机常值漂移和加速度计随机常值零偏。xyz别代表东、北、天）系统的噪声转移矩阵G为：8 、8 、8 、xyz（下标E、N、U分Cnb03x309x303x3Cnb09x315x6系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为：y系统的状态转移矩阵F组成内容为：FF NS0F6x9M，其中F中非零元素为可由惯导误差模型获得NC nb
3、03x303x303x3C nb03x39x6（2）量测方程量测变量 z8V8V8V8L88H小，8V、8 V、8 V、8 L、81ENUENU和8 H分别为捷联解算与GPS的东向速度、北向速度、天向速度、纬度、经度和高度之差；量测矩阵H= HH 卜,H=0diag(R+ H,(R+ H)cos L,1)0 ,VPP3x6MN3x6H = 0diag(1,1,1) 0 , v = v v vvL, v v 为量测噪声。量测噪声方V3x33x9SVE， 8vn， 8vu， 8 L 8X 8 H差阵r根据GPS的位置、速度噪声水平选取。(3)卡尔曼滤波方程状态一步预测：丘 =0 丘k / k-1
4、k ,k-1 k-1状态估计：X = X+ K (Z - H X )kk / k-1k k k k / k-1滤波增益：K = P Ht (H P Ht + R )-1kk / k-1 k k k / k-1 k k一步预测均方误差：P =0 P 0T + Qk / k-1 k ,k-1 k-1 k ,k-1k-1估计均方误差：P = (I-KH )Pkkkk / k -1三、实验内容及步骤1、实验内容 捷联惯导/GPS组合导航系统静态导航实验；2、实验步骤1)实验准备(由实验教师操作) 将IMU安装在大理石实验台上，确认IMU的安装基准面靠在大理石实验平台上 的方位基准尺上； 将IMU与导航
5、计算机、导航计算机与稳压电源、导航计算机与监控计算机、 GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连 接线正确连接； 打开GPS信号转发器； 打开监控计算机中的监控软件； 打开稳压电源开关，确认稳压电源输出为28V，- 打开捷联惯导/GPS组合实验系统电源，实验系统开始启动，注意30s内严禁动 IMU ； 打开GPS接收机电源，确认通过信号转发器可以接收到4颗以上卫星； 将监控软件设置为“准备”状态，准备时间5分钟； 准备过程中由实验教师介绍捷联惯导/GPS组合实验系统的组成、工作原理；2）捷联惯导/GPS组合导航系统静态导航实验 实验系统准备5分钟之后
6、，通过监控软件，将实验系统设置为“组合导航”状态； 记录IMU的原始输出，即角增量和比力信息； 记录数据过程中，由实验教师介绍采用卡尔曼滤波方法进行捷联惯导/GPS组合 导航的基本原理； 记录IMU数据5分钟后，停止记录数据，利用监控计算机中的捷联惯导/GPS组 合导航软件进行静态导航解算，并显示静态导航结果；四、实验结果与分析1、SINS/GPS组合导航后得纬度曲线和GPS导航纬度曲线对比如下图34.9534.934.8534.8GPS纬度 组合导航后纬度度度纬34.7534.734.6534.634.5534.53445 08x 10 42、SINS/GPS组合导航后得经度曲线和GPS导航
7、经度曲线对比如下图度度经110.1110109.9109.8109.7109.6109.5109.4109.3109.2GPS经度组合导航后经度109J 0 12345678x 1043、SINS/GPS组合导航后得高度曲线和GPS导航高度曲线对比如下图350030002500GPS高度 组合导航后高度0020咼01234567自x 10气4、SINS/GPS组合导航后得东向速度曲线和GPS导航东向速度曲线对比如下图GPS东向速度组合导航后东向速度2,o-20-40)SWLS速向-60 0 1x 10 SINS/GPS组合导航后得天向速度曲线和GPS导航天向速度曲线对比如下图 01234567
8、4x 10气5、SINS/GPS组合导航后得北向速度曲线和GPS导航北向速度曲线对比如下图60 -GPS北向速度“组合导航后北向速度6、GPS天向速度 组合导航后大向速度-2-4 012345678x 10气7、SINS/GPS组合导航后航向角曲线、俯仰角曲线和横滚角曲线如下图东向速度误差方差0.01m 0-0050北向速度误差方差0.010.00500246802468亠x 10 4x 10 4 天向速度误差方差0.01m 0-0050纬度误差方差0.10.05002468024680.10.2m 0.05m 0.100x 10 4 经度误差方差x 10 4 高度误差方差02468x 10
9、402468x 10 411、陀螺漂移的估计均方差曲线和加速度计偏置的估计均方差曲线如下图东向 陀螺漂移方差02468x 10 4 天向陀螺漂移方一差0.10.05北向 陀 螺漂移 方差0.10.0502468东向加计偏置方-差x00.10.05002468x 10 4 北向加计偏置方弹50002468北向加计偏置方-差 50g002468x 10 450002468x 10 40滤波之后载体的位置和速度比GPS输出的位置和结果分析：从仿真结果可以看出，速度精度高，载体姿态在滤波校正后结果较好，INS/GPS组合导航有效地抑制了纯惯性 导航的发散，也有效地去除了 GPS量测输出的噪声。东北天方
10、向的速度误差均能够估计 出来，天向陀螺漂移估计不出来，在动基座情况下，东向和北向加计零偏、天向平台失 准角以及东向和北向陀螺漂移均变得可观测，收敛性变好。可见，INS/GPS是一种较为 理想的组合导航方式。五、源程序clear;clc;%载入数据IMU=load(C:UsersAdministratorDesktop 卡尔曼);GPS=load(C:UsersAdministratorDesktop 卡尔曼);%定义常数e=l/;re=6378245;wie=;IMU_T=1/100;GPS_T=1/20;g0=;gk1=;gk2=;LOOP=360000;%定义存储惯导解算的位置、速度、姿态
11、和滤波后的位置、速度、姿态 velocity=zeros(LOOP,3);position=zeros(LOOP,3); attitude=zeros(LOOP,3);velocity_kf=zeros(72000,3);position_kf=zeros(72000,3); attitude_kf=zeros(72000,3);%用GPS导航的初始位置和初始速度作为导航结算的初始位置和初始速度 vx=0;vy=;vz=;lat=*pi/180 ;long=*pi/180 ;h=;%定义四元数初值 cita=*pi/180 ; % 俯仰角 gama=*pi/180 ; % 横滚角 kesai=
12、*pi/180 ; % 航向角 q=cos(kesai/2)*cos(cita/2)*cos(gama/2)-sin(kesai/2)*sin(cita/2)*sin(gama/2);cos(kesai/2)*sin(cita/2)*cos(gama/2)-sin(kesai/2)*cos(cita/2)*sin(gama/2); cos(kesai/2)*cos(cita/2)*sin(gama/2)+sin(kesai/2)*sin(cita/2)*cos(gama/2);cos(kesai/2)*sin(cita/2)*sin(gama/2)+sin(kesai/2)*cos(cita/
13、2)*cos(gama/2);%滤波初始状态量和滤波初始所需矩阵k=1;X_f=zeros(15,l);Q=diag(*pi/(180*3600)人2,*pi/(180*3600)人2,*pi/(180*3600)人2, (50e-6*g0)人2,(50e-6*g0)人2,(50e-6*g0)人2);R=diag(A2,A2,A2,A2,A2,A2);H=zeros(6,15);p_kf=zeros(72000,15);x_kf=zeros(72000,15);P=diag(60*pi/180/3600)A2,(60*pi/180/3600)A2,(30*pi/180/60)A2,A2,A2,A2, (2/re)A2,(2/re)A2,2A2,*pi/180/3600)A2, *pi/180/3600)A2,*pi/180/3600)A2, (50e-6*g0)A2,(50e-6*g0)A2,(50e-6*g0)A2);for i=1:L00PRx=re/(1-e*sin(lat)A2)+h;Ry=re/(1+2*e-3*e*sin(lat)A2)+h;g=gO*(1+gk1*sin(lat)
