北航惯性导航综合实验五实验报告.doc

惯性导航技术综合实验

实验五 惯性基组合导航及应用技术实验

惯性/卫星组合导航系统车载实验
一、 实验目的
①掌握捷联惯导/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），忽略地球自转，运动轨迹近似为平面，此时的位置误差分析可简化为：
（1） 加速度计零偏引起的位置误差：m
（2） 失准角引起的误差：m
（3） 陀螺漂移引起的误差：m
可得1min后的位置误差值
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滤波程序，惯性/卫星组合松耦合的数学模型
clear
clc
close all
%%初始量定义
wie = 0.000072921151467;
Re = 6378135.072;
g = 9.7803267714;
e = 1.0 / 298.25;
T = 0.01; %IMU频率100hz,此程序中GPS频率100hz
datanumber = 360000; %数据时间3600s

a = load('imu_1h.dat');
w = a(:,3:5)'*pi/180/3600; %陀螺仪输出的角速率信息单位由°/h化为rad/s
f = a(:,6:8)'; %三轴比力输出，单位g
a = 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.0211142228246*pi/180; %组合导航的初始位置、姿态、速度
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的初值选为0
X=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.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,(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:datanumber

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;

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:3
gyro(j,1) = w(j,i);
acc(j,1) = f(j,i)*g; %加速度信息，单位化为m/s^2
end

angle = (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:10
temp1 = F * temp1 / j;
disF = disF + temp1;
end
temp1 = Q * T;
disQ = temp1;
for j = 2:11
temp2 = F * temp1;
temp1 = (temp2 + temp2')/j;
disQ = disQ + temp1;
end
Z(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; %陀螺随机常值漂移,单位°/h
x_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; %加计随机常值偏置，单位ug
x_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;
end
if (Cnb(3,3) < 0)
if(gama > 0)
gama = gama - pi;
else
gama = gama + pi;
end
end
atti(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);
end
if (Cnb(3,1) < Cnb(1,3))
q(3) = - q(3);
end
if (Cnb(1,2) < Cnb(2,1))
q(4) = - q(4);
end
X_k(1:9) = 0;
X_o=X_k;
i
end
%%%绘图%%%%%%%%%%
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');