五、源程序
clear;
clc;
%载入数据
IMU=load('C:\Users\Administrator\Desktop\卡尔曼\IMU.dat');
GPS=load('C:\Users\Administrator\Desktop\卡尔曼\GPS.dat');
%%%%%%%%%%定义常数
e=1/298.3;
else
kesai=kesai_1-pi;
end
end
if Cnb(3,3)==0
if Cnb(1,3)>0
gama=pi/2;
else
gama=-pi/2;
end
elseif Cnb(3,3)>0
gama=gama_1;
else
if Cnb(1,3)>0
gama=gama_1-pi;
else
gama=gama_1+pi;
end
end
%%%%%%%%%%%%存储惯导解算求的的速度、位置和姿态角
velocity(i,:) = [vx,vy,vz];
position(i,:) = [lat/pi*180,long/pi*180,h];
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);
5、SINS/GPS组合导航后得北向速度曲线和GPS导航北向速度曲线对比如下图
6、SINS/GPS组合导航后得天向速度曲线和GPS导航天向速度曲线对比如下图
7、SINS/GPS组合导航后航向角曲线、俯仰角曲线和横滚角曲线如下图
8、纯惯性导航轨迹、GPS导航轨迹和SINS/GPS组合导航轨迹对比如下图
系统的噪声转移矩阵 为：
系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为：
系统的状态转移矩阵 组成内容为：
，其中 中非零元素为可由惯导误差模型获得。 。
（2）量测方程
量测变量 ，， 、 、 、 、 和 分别为捷联解算与GPS的东向速度、北向速度、天向速度、纬度、经度和高度之差；量测矩阵 ， ， ， 为量测噪声。量测噪声方差阵 根据GPS的位置、速度噪声水平选取。
四、实验结果与分析
1、SINS/GPS组合导航后得纬度曲线和GPS导航纬度曲线对比如下图
2、SINS/GPS组合导航后得经度曲线和GPS导航经度曲线对比如下图
3、SINS/GPS组合导航后得高度曲线和GPS导航高度曲线对比如下图
4、SINS/GPS组合导航后得东向速度曲线和GPS导航东向速度曲线对比如下图
9、平台失准角的估计均方差曲线如下图
10、速度和位置的估计均方差曲线如下图
11、陀螺漂移的估计均方差曲线和加速度计偏置的估计均方差曲线如下图
结果分析：从仿真结果可以看出，滤波之后载体的位置和速度比GPS输出的位置和速度精度高，载体姿态在滤波校正后结果较好，INS/GPS组合导航有效地抑制了纯惯性导航的发散，也有效地去除了GPS量测输出的噪声。东北天方向的速度误差均能够估计出来，天向陀螺漂移估计不出来，在动基座情况下，东向和北向加计零偏、天向平台失准角以及东向和北向陀螺漂移均变得可观测，收敛性变好。可见，INS/GPS是一种较为理想的组合导航方式。
（3）卡尔曼滤波方程
状态一步预测：
状态估计：
滤波增益：
一步预测均方误差：
估计均方误差：
三、实验内容及步骤
1、实验内容
捷联惯导/GPS组合导航系统静态导航实验；
2、实验步骤
1）实验准备（由实验教师操作）
将IMU安装在大理石实验台上，确认IMU的安装基准面靠在大理石实验平台上的方位基准尺上；
将IMU与导航计算机、导航计算机与稳压电源、导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连接线正确连接；
ax=fibn(1)+(2*wie*sin(lat)+vx*tan(lat)/Rx)*vy-(2*wie*cos(lat)+vx/Rx)*vz;
ay=fibn(2)-(2*wie*sin(lat)+vx*tan(lat)/Rx)*vx+vy*vz/Ry;
az=fibn(3)+(2*wie*cos(lat)+vx/Rx)*vx+vy^2/Ry-g;
(50e-6*g0)^2,(50e-6*g0)^2,(50e-6*g0)^2]);
for i=1:LOOP
Rx=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);
③掌握捷联惯导/GPS组合导航系统静态性能；
④了解捷联惯导/GPS组合导航静态时的系统状态可观测性；
二、实验原理
（1）系统方程
其中， 、 、 为数学平台失准角； 、 、 分别为载体的东向、北向和天向速度误差； 、 、 分别为纬度误差、经度误差和高度误差； 、 、 、 、 、 分别为陀螺随机常值漂移和加速度计随机常值零偏。（下标E、N、U分别代表东、北、天）
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;
%%%%定义存储惯导解算的位置、速度、姿态和滤波后的位置、速度、姿态
%%%%%%用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 ; %俯仰角
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];
%%%%%%%%%%%%%%根据更新过的四元素姿态矩阵求姿态角
cita=asin(Cnb(2,3)); %俯仰角
kesai_1=atan(Cnb(2,1)/Cnb(2,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)];
%%%%滤波初始状态量和滤波初始所需矩阵
《卡尔曼滤波与组合导航》课程实验报告
实验
捷联惯导/GPS组合导航系统静态导航实验
实验序号
3
姓名
陈星宇
系院专业
17
班级
ZY11172
学号
ZY1117212
日期
2012-5-15
指导教①掌握捷联惯导/GPS组合导航系统的构成和基本工作原理；
②掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理；
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);
%%%%%%四元素姿态矩阵
Cnb=[q(1)^2+q(2)^2-q(3)^2-q(4)^2 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)^2-q(2)^2+q(3)^2-q(4)^2 2*(q(3)*q(4)+q(1)*q(2));
vx=ax*IMU_T+vx;
vy=ay*IMU_T+vy;
vz=az*IMU_T+vz;
lat=lat+vy/Ry*IMU_T;
long=long+vx*sec(lat)/Rx*IMU_T;
h=h+vz*IMU_T;
%%%%%%%更新四元素姿态矩阵
wiet=[0;wie*cos(lat);wie*sin(lat)];
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);
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,