卡尔曼滤波与组合导航课程实验报告

《卡尔曼滤波与组合导航》课程实验报告实验捷联惯导/GPS组合导航系统静态导航实验实验序号3姓名陈星宇系院专业17班级ZY11172学号ZY1117212日期2012-5-15指导教师宫晓琳成绩一、实验目的①掌握捷联惯导/GPS组合导航系统的构成和基本工作原理；②掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理；③掌握捷联惯导/GPS组合导航系统静态性能；④了解捷联惯导/GPS组合导航静态时的系统状态可观测性；二、实验原理（1）系统方程XFXGWTENUENUxyzxyzXvvvLh其中，E、N、U为数学平台失准角；Ev、Nv、Uv分别为载体的东向、北向和天向速度误差；L、、h分别为纬度误差、经度误差和高度误差；x、y、z、x、y、z分别为陀螺随机常值漂移和加速度计随机常值零偏。（下标E、N、U分别代表东、北、天）系统的噪声转移矩阵G为：333393931560000nbnbCGC系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为：Txyzxyz系统的状态转移矩阵F组成内容为：690NSMFFFF，其中NF中非零元素为可由惯导误差模型获得。nb33n33b333396S0000CFC。（2）量测方程量测变量TENUzVVVLH，，EV、NV、UV、L、和H分别为捷联解算与GPS的东向速度、北向速度、天向速度、纬度、经度和高度之差；量测矩阵TVPHHH，P360HN36diag,()cos,MRHRHL0，VH33039diag1,1,10，ENUTLHVVVvvvvvvv为量测噪声。量测噪声方差阵R根据GPS的位置、速度噪声水平选取。（3）卡尔曼滤波方程状态一步预测：/1,11ˆˆkkkkkXX状态估计：/1/1ˆˆˆ()kkkkkkkkXXKZHX滤波增益：1/1/1()TTkkkkkkkkkKPHHPHR一步预测均方误差：/1,11,11TkkkkkkkkPPQ估计均方误差：/1()kkkkkPIKHP三、实验内容及步骤1、实验内容①捷联惯导/GPS组合导航系统静态导航实验；2、实验步骤1）实验准备（由实验教师操作）①将IMU安装在大理石实验台上，确认IMU的安装基准面靠在大理石实验平台上的方位基准尺上；②将IMU与导航计算机、导航计算机与稳压电源、导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连接线正确连接；③打开GPS信号转发器；④打开监控计算机中的监控软件；⑤打开稳压电源开关，确认稳压电源输出为28V；⑥打开捷联惯导/GPS组合实验系统电源，实验系统开始启动，注意30s内严禁动IMU；⑦打开GPS接收机电源，确认通过信号转发器可以接收到4颗以上卫星；⑧将监控软件设置为“准备”状态，准备时间5分钟；⑨准备过程中由实验教师介绍捷联惯导/GPS组合实验系统的组成、工作原理；2）捷联惯导/GPS组合导航系统静态导航实验①实验系统准备5分钟之后，通过监控软件，将实验系统设置为“组合导航”状态；②记录IMU的原始输出，即角增量和比力信息；③记录数据过程中，由实验教师介绍采用卡尔曼滤波方法进行捷联惯导/GPS组合导航的基本原理；④记录IMU数据5分钟后，停止记录数据，利用监控计算机中的捷联惯导/GPS组合导航软件进行静态导航解算，并显示静态导航结果；四、实验结果与分析1、SINS/GPS组合导航后得纬度曲线和GPS导航纬度曲线对比如下图012345678x10434.4534.534.5534.634.6534.734.7534.834.8534.934.95纬度（度）GPS纬度组合导航后纬度2、SINS/GPS组合导航后得经度曲线和GPS导航经度曲线对比如下图012345678x104109.1109.2109.3109.4109.5109.6109.7109.8109.9110110.1经度（度）GPS经度组合导航后经度3、SINS/GPS组合导航后得高度曲线和GPS导航高度曲线对比如下图012345678x1040500100015002000250030003500高度（m）GPS高度组合导航后高度4、SINS/GPS组合导航后得东向速度曲线和GPS导航东向速度曲线对比如下图012345678x104-60-40-20020406080东向速度（m/s）GPS东向速度组合导航后东向速度5、SINS/GPS组合导航后得北向速度曲线和GPS导航北向速度曲线对比如下图012345678x104-60-40-200204060北向速度（m/s）GPS北向速度组合导航后北向速度6、SINS/GPS组合导航后得天向速度曲线和GPS导航天向速度曲线对比如下图012345678x104-4-202468天向速度（m/s）GPS天向速度组合导航后天向速度7、SINS/GPS组合导航后航向角曲线、俯仰角曲线和横滚角曲线如下图012345678x104-200-150-100-50050100150200度组合导航后航向角组合导航后俯仰角组合导航后横滚角8、纯惯性导航轨迹、GPS导航轨迹和SINS/GPS组合导航轨迹对比如下图34.4534.534.5534.634.6534.734.7534.834.8534.934.95109.1109.2109.3109.4109.5109.6109.7109.8109.9110110.1纬度经度纯惯性导航轨迹GPS导航轨迹组合导航导航轨迹9、平台失准角的估计均方差曲线如下图012345678x10400.010.02东向失准角方差度012345678x10400.010.02北向失准角方差度012345678x10400.5天向失准角方差度10、速度和位置的估计均方差曲线如下图02468x10400.0050.01东向速度误差方差m/s02468x10400.0050.01北向速度误差方差m/s02468x10400.0050.01天向速度误差方差m/s02468x10400.050.1纬度误差方差m02468x10400.050.1经度误差方差m02468x10400.10.2高度误差方差m11、陀螺漂移的估计均方差曲线和加速度计偏置的估计均方差曲线如下图02468x10400.050.1东向陀螺漂移方差度/小时02468x10400.050.1北向陀螺漂移方差度/小时02468x10400.050.1天向陀螺漂移方差度/小时02468x104050东向加计偏置方差μg02468x104050北向加计偏置方差μg02468x104050北向加计偏置方差μg结果分析：从仿真结果可以看出，滤波之后载体的位置和速度比GPS输出的位置和速度精度高，载体姿态在滤波校正后结果较好，INS/GPS组合导航有效地抑制了纯惯性导航的发散，也有效地去除了GPS量测输出的噪声。东北天方向的速度误差均能够估计出来，天向陀螺漂移估计不出来，在动基座情况下，东向和北向加计零偏、天向平台失准角以及东向和北向陀螺漂移均变得可观测，收敛性变好。可见，INS/GPS是一种较为理想的组合导航方式。五、源程序clear;clc;%载入数据IMU=load('C:\Users\Administrator\Desktop\卡尔曼\IMU.dat');GPS=load('C:\Users\Administrator\Desktop\卡尔曼\GPS.dat');%%%%%%%%%%定义常数e=1/298.3;re=6378245;wie=7.292115147e-5;IMU_T=1/100;GPS_T=1/20;g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;LOOP=360000;%%%%定义存储惯导解算的位置、速度、姿态和滤波后的位置、速度、姿态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=0.0090;vz=0.0350;lat=34.6522*pi/180;long=109.2496*pi/180;h=362.2690;%%%%%%定义四元数初值cita=0.25097*pi/180;%俯仰角gama=1.78357*pi/180;%横滚角kesai=305.34023*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/2)*cos(gama/2)];%%%%滤波初始状态量和滤波初始所需矩阵k=1;X_f=zeros(15,1);Q=diag([(0.01*pi/(180*3600))^2,(0.01*pi/(180*3600))^2,(0.01*pi/(180*3600))^2,(50e-6*g0)^2,(50e-6*g0)^2,(50e-6*g0)^2]);R=diag([0.01^2,0.01^2,0.01^2,0.1^2,0.1^2,0.15^2]);H=zeros(6,15);p_kf=zeros(72000,15);x_kf=zeros(72000,15);P=diag([(60*pi/180/3600)^2,(60*pi/180/3600)^2,(30*pi/180/60)^2,0.05^2,0.05^2,0.05^2,(2/re)^2,(2/re)^2,2^2,(0.1*pi/180/3600)^2,(0.1*pi/180/3600)^2,(0.1*pi/180/3600)^2,(50e-6*g0)^2,(50e-6*g0)^2,(50e-6*g0)^2]);fori=1:LOOPRx=re/(1-e*sin(lat)^2)+h;Ry=re/(1+2*e-3*e*sin(lat)^2)+h;g=g0*(1+gk1*sin(lat)^2)*(1-2*h/re)/sqrt(1-gk2*sin(lat)^2);%%%%%%四元素姿态矩阵Cnb=[q(1)^2+q(2)^2-q(3)^2-q(4)^22*(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)^2-q(2)^2+q(3)^2-q(4)^22*(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)^2-q(2)^2-q(3)^2+q(4)^2];%%%%%%%%%%%%%%%%%%捷联惯导结算fibb=IMU(i,6:8)'*g;fibn=(Cnb')*fibb;%%%%求解加速度、速