北航惯性导航综合实验五实验报告之欧阳地创编Word文件下载.docx
《北航惯性导航综合实验五实验报告之欧阳地创编Word文件下载.docx》由会员分享，可在线阅读，更多相关《北航惯性导航综合实验五实验报告之欧阳地创编Word文件下载.docx（17页珍藏版）》请在冰点文库上搜索。
（一）理论推导捷联惯导短时段（1分钟）位置误差，并用1分钟惯导实验数据验证。
1、一分钟惯导位置误差理论推导：
短时段内（t<
5min），忽略地球自转
，运动轨迹近似为平面
，此时的位置误差分析可简化为：
（1）加速度计零偏
引起的位置误差：
m
（2）失准角
引起的误差：
（3）陀螺漂移
可得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
closeall
%%初始量定义
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
gps_1h_new.dat'
gps_pos=a（:
5）;
%GPS输出的纬度、经度、高度信息
gps_pos（:
1:
2）=gps_pos（:
2）*pi/180;
%纬经单位化为弧度
gps_v=a（:
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;
fori=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;
forj=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）];
%以上为纯惯导解算
%%
F1=[0wie*sin（lat）+v（i,1）*tan（lat）/（Rnh）-（wie*cos（lat）+v（i,1）/（Rnh））0-1/（Rmh）0000;
-（wie*sin（lat）+v（i,1）*tan（lat）/（Rnh））0-v（i,2）/（Rmh）1/（Rnh）00-wie*sin（lat）00;
wie*cos（lat）+v（i,1）/（Rnh）v（i,2）/（Rmh）0tan（lat）/（Rnh）00wie*cos（lat）+v（i,2）*sec（lat）*sec（lat）/（Rnh）00;
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））00;
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）00;
-fn
（2）fn
（1）02*（wie*cos（lat）+v（i,1）/（Rnh））2*v（i,2）/（Rmh）0-2*wie*sin（lat）*v（i,1）00;
00001/（Rmh）0000;
000sec（lat）/（Rnh）00v（i,1）*sec（lat）*tan（lat）/（Rnh）00;
000001000
];
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）];
F=[F1,F2;
zeros（6,15）];
%以上为kalman滤波模型参数
F=F*T;
%离散化
temp1=eye（15）;
disF=eye（15）;
forj=1:
10
temp1=F*temp1/j;
disF=disF+temp1;
temp1=Q*T;
disQ=temp1;
forj=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;
if（Cnb（3,3）<
if（gama>
gama=gama-pi;
else
gama=gama+pi;
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）;
if（Cnb（3,1）<
Cnb（1,3））
q（3）=-q（3）;
if（Cnb（1,2）<
Cnb（2,1））
q（4）=-q（4）;
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（:
b'
）
title（'
纬度'
xlabel（'
0.01s'
ylabel（'
°
'
subplot（312）;
2）*180/pi,'
经度'
subplot（313）;
3）,'
高度'
m'
legend（'
组合滤波'
'
GPS'
figure
（2）
plot（t,v（:
1）,'
t,gps_v（:
titl
升级会员 