北航惯性导航综合实验五实验报告Word格式.docx
《北航惯性导航综合实验五实验报告Word格式.docx》由会员分享,可在线阅读,更多相关《北航惯性导航综合实验五实验报告Word格式.docx(14页珍藏版)》请在冰豆网上搜索。
短时段内(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);
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(:
东向速度'
m/s'
su