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

卡尔曼滤波与组合导航》课程实验报告实验 捷联惯导 /GPS 组合导航系统静态导航实验实验序号 3 姓名 陈星宇系院专业17班级ZY11172学号ZY1117212日期2012-5-15指导教师宫晓琳成绩、实验目的① 掌握捷联惯导 /GPS 组合导航系统的构成和基本工作原理； ②掌握采用卡尔曼滤波方法进行捷联惯导 /GPS 组合的基本原理； ③掌握捷联惯导 /GPS 组合导航系统静态性能；④了解捷联惯导 /GPS 组合导航静态时的系统状态可观测性；、实验原理（ 1）系统方程 X FX GW系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为：2）量测方程和 H 分别为捷联解算与 GPS 的东向速度、北向速度、天向速度、纬度、经度和高度之 差；量测矩阵 H H V H P T，H P 03 6 diag R M H, （R N H ）cos L, 036 ，H V 033 diag 1, 1, 1 039，v v V Ev VNv V U v L v v H 为量测噪声。

量测噪声v E v NTvUL hx y z x y z其中， E 、 N 、 U 为数学平台失准角；v E 、 v N 、 v U 分别为载体的东向、北向和天向速度误差；L 、 、 h 分别为纬度误差、经度误差和高度误差；x 、 y 、 z 、 x 、 y 、 z 分别为陀螺随机常值漂移和加速度计随机常值零偏。

（下标 系统的噪声转移矩阵 G 为：E 、N 、 U 分别代表东、北、天）C bn3 3 09 33 3 C n C b9 315 6系统的状态转移矩阵 w w w wF 组成内容为：wzF06N9F SFM，其中 F N 中非零元素为可由惯导误差模型获得。

F SC bn3 3 03 33 3C bn3 396量测变量 z V E V N V U L H ，， V E 、 V N 、 V U 、 L 、XU方差阵R 根据GPS的位置、速度噪声水平选取。

(3)卡尔曼滤波方程状态一步预测：X?k/k 1 k,k 1X?k 1状态估计：X?k X?k/k 1 K k(Z k H k X?k/k 1)滤波增益：K k P k/k 1H k T(H k P k/k 1H k T R k) 1 2 一步预测均方误差：P k/k 1 k,k三、实验内容及步骤1、实验内容① 捷联惯导/GPS 组合导航系统静态导航实验；2、实验步骤2实验准备(由实验教师操作)① 将IMU 安装在大理石实验台上，确认IMU 的安装基准面靠在大理石实验平台上的方位基准尺上；②将IMU 与导航计算机、导航计算机与稳压电源、导航计算机与监控计算机、GPS 接收机与导航计算机、GPS天线与GPS接收机、GPS 接收机与GPS 电池之间的连接线正确连接；③打开GPS 信号转发器；④打开监控计算机中的监控软件；⑤打开稳压电源开关，确认稳压电源输出为28V ；⑥打开捷联惯导/GPS 组合实验系统电源，实验系统开始启动，注意30s 内严禁动IMU；⑦打开GPS接收机电源，确认通过信号转发器可以接收到 4 颗以上卫星；⑧将监控软件设置为“准备”状态，准备时间 5 分钟；⑨准备过程中由实验教师介绍捷联惯导/GPS 组合实验系统的组成、工作原理；1P k 1 k T,k 1 Q k 1 估计均方误差：P k (I K k H k)P k/k 12) 捷联惯导 /GPS 组合导航系统静态导航实验① 实验系统准备 5 分钟之后， 通过监控软件， 将实验系统设置为 “组合导航” 状态；② 记录 IMU 的原始输出，即角增量和比力信息；③ 记录数据过程中，由实验教师介绍采用卡尔曼滤波方法进行捷联惯导 /GPS 组合导 航的基本原理；④ 记录 IMU 数据 5 分钟后，停止记录数据，利用监控计算机中的捷联惯导/GPS 组 合导航软件进行静态导航解算，并显示静态导航结果； 四、实验结果与分析 1、SINS/GPS 组合导航后得纬度曲线和 GPS 导航纬度曲线对比如下图）度（度34.95 34.934.8534.8 34.75 34.7 34.65 34.6 34.55 34.534.45 0 1 2 3 4 5 6 7 84 x 102、SINS/GPS 组合导航后得经度曲线和 GPS 导航经度曲线对比如下图GPS 纬 度 组合导航后纬度 )度 (度 经110.1 110 109.9 109.8 109.7109.6 109.5 109.4 109.3 109.2 109.1GPS 经 度 组合导航后经度4x 103、SINS/GPS 组合导航后得高度曲线和 GPS 导航高度曲线对比如下图35003000 GPS 高 度 组合导航后高度2500 2000（度高1500高 100050084x 104、 SINS/GPS 组合导航后得东向速度曲线和GPS 导航东向速度曲线对比如下图80 60 GPS 东 向 速 度 组 合 导 航 后 东 向 速 度40 20 0 2040）s /m（度速向东-601 2 3 4 5 6 7 8 4x 105、 SINS/GPS 组合导航后得北向速度曲线和GPS 导航北向速度曲线对比如下图604020-20-40-60GPS 北 向 速 度 组合导航后北向速度84x 10 46、 SINS/GPS 组合导航后得天向速度曲线和GPS 导航天向速度曲线对比如下图567GPS 天 向 速 度 组 合 导 航 后 天 向 速 度6 4 2 0 ）s /m（度速向天244x 107、 SINS/GPS 组合导航后航向角曲线、俯仰角曲线和横滚角曲线如下图200 15010050-50-100-150-200组合导航后航向角 组合导航后俯仰角 组合导航后横滚角012 4x 108、纯惯性导航轨迹、 GPS 导航轨迹和 SINS/GPS 组合导航轨迹对比如下图110.1 110109.9109.8 109.7 度 109.6 经109.5 109.4 109.3 109.2109.134.45 34.5 34.55 34.6 34.65 34.7 34.75 34.8 34.85 34.9 34.95 纬度 纯惯性导航轨迹 GPS 导航 轨 迹 组合导航导航轨迹9、平台失准角的估计均方差曲线如下图0.02 度 0.01 00.02 度 0.01 00 1 23北 向 失 准4 角 方 差 5 6784x 10东向失准角方差0 1 2 3 4 5 6 78 0.5天向失准角方差4x 100 1 2 3 4 5 6 7 8 4x 1010、速度和位置的估计均方差曲线如下图东向速度误差方差0.01/s m0.0050 2 4 6 8 4x 10 天向速度误差方差 0.01/s m 0.0050 02468 4 x10 经度误差方差0.1m 0.050 024684 x 10北 向 速 度 误 差 方 差0.01/s m0.0050 2 4 6 84x 10 纬度误差方差0.1m 0.050 2 4 6 84x 10 高度误差方差0.2m 0.10 2 4 6 84x 1011、陀螺漂移的估计均方差曲线和加速度计偏置的估计均方差曲线如下图结果分析：从仿真结果可以看出，滤波之后载体的位置和速度比GPS 输出的位置和速度精度高，载体姿态在滤波校正后结果较好， INS/GPS 组合导航有效地抑制了纯惯性导航的发散，也有效地去除 了 GPS 量测输出的噪声。

东北天方向的速度误差均能够估计出来， 天向陀螺漂移估计不出来， 在动基 座情况下，东向和北向加计零偏、天向平台失准角以及东向和北向陀螺漂移均变得可观测，收敛性变 好。

可见， INS/GPS 是一种较为理想的组合导航方式。

五、源程序clear; clc; % 载入数据卡尔曼 \IMU.dat'); 卡尔曼 \GPS.dat');%%%%%%%%%%定义常数 e=1/298.3; re=6378245;/时小度度0.1 0.1 /时小度度0.1 东向陀螺漂移方差0.05/时小度度x 104天向陀螺漂移方差 北向陀螺漂移方差0.05x 104东向加计偏置方差50gμ0.0550gμgμ4x 104北向加计偏置方差4x 10 44 x 104北向加计偏置方差4x 10 4A (6σ51)s c u ①NI G O1)① >9 G ol )s cu ① ZIΓ1L H ⅛≡≡J M s ⅛≡t ⅛ 世≡1*沢抿&&&&&&&&&&&±f +(0v (ωDUω⅛*c?① *0+L)a)」HQ±f +(0v (ωDUω⅛丄)05」H X M(O H H (S )P O E )七⅛8L *d ¾Ee 6-08L *d '2o -08L *⅛e s①兰H C -¥Pn ≡ωEOEddUO--OFdAe=H C-Duomsodπ>A >x >H U D A l oo α)> <^≡t ⅛鋼曰，翌®旨旨^*<吟擊型>&&&&&&&&&&&&PU ①PU①⅛+Llelue6Helue6φω-φ⅛ILIeEe6Helue6OAGL)q u o七φω-φ=IeIUe6Helue6o ^e o)q u o 4φs φPU①0ddelue6φω-φZAdHelUeCOAGL)q u o七O H H o o )q u o七PU①fai_xFn(5,6)=-vy/Ry;kesai_kf=-pi/2;');ylabel('经度(度) '); figure(3) plot(1:72001,GPS(:,5));hold onplot(t,position_kf(:,3),'r');hold on legend('GPS高度','组合导航后高度');ylabel('高度( m)'); figure(4) plot(1:72001,GPS(:,6));hold onplot(t,velocity_kf(:,1),'r');hold on legend('GPS东向速度','组合导航后东向速度');ylabel('东向速度( m/s)'); figure(5)plot(1:72001,GPS(:,7));hold on plot(t,velocity_kf(:,2),'r');hold on legend('GPS北向速度','组合导航后北向速度');ylabel('北向速度( m/s)'); figure(6) plot(1:72001,GPS(:,8));hold onplot(t,velocity_kf(:,3),'r');hold on legend('GPS天向速度','组合导航后天向速度');ylabel('天向速度( m/s)') figure(7)plot(t,attitude_kf(:,1));hold on plot(t,attitude_kf(:,2),'r');hold onplot(t,attitude_kf(:,3),'g');hold on legend('组合导航后航向角','组合导航后俯仰角','组合导航后横滚角');ylabel(' 度'); figure(8)plot(position(:,1),position(:,2));hold on plot(GPS(:,3),GPS(:,4),'r');hold on plot(position_kf(:,1),position_kf(:,2),'g');hold on legend('纯惯性导航轨迹','GPS导航轨迹','组合导航导航轨迹');xlabel('纬度'); ylabel('经度');figure(9) subplot(3,1,1);plot(t,p_kf(:,1));title(' 东向失准角方差');ylabel(' 度');subplot(3,1,2);plot(t,p_kf(:,2));title(' 北向失准角方差');ylabel(' 度');subplot(3,1,3);plot(t,p_kf(:,3));title(' 天向失准角方差');ylabel(' 度'); figure(10) subplot(3,2,1); plot(t,p_kf(:,4));title(' 东向速度误差方差');ylabel('m/s');subplot(3,2,2); plot(t,p_kf(:,5));title(' 北向速度误差方差');ylabel('m/s');subplot(3,2,3); plot(t,p_kf(:,6));title(' 天向速度误差方差');ylabel('m/s');subplot(3,2,4); plot(t,p_kf(:,7));title(' 纬度误差方差');ylabel('m'); subplot(3,2,5); plot(t,p_kf(:,8));title(' 经度误差方差');ylabel('m');subplot(3,2,1);plot(t,p_kf(:,10));title(' 东向陀螺漂移方差');ylabel(' 度/小时'); subplot(3,2,2);plot(t,p_kf(:,11));title(' 北向陀螺漂移方差');ylabel('度/小时'); subplot(3,2,3);plot(t,p_kf(:,12));title(' 天向陀螺漂移方差');ylabel(' 度/小时'); subplot(3,2,4);plot(t,p_kf(:,13));title(' 东向加计偏置方差');ylabel(' μg'); subplot(3,2,5);plot(t,p_kf(:,14));title(' 北向加计偏置方差');ylabel(' μg'); subplot(3,2,6);plot(t,p_kf(:,15));title(' 北向加计偏置方差');ylabel(' μg');。