北航惯性导航综合实验五实验报告Word下载.docx
- 文档编号:21476522
- 上传时间:2023-01-30
- 格式:DOCX
- 页数:19
- 大小:69.79KB
北航惯性导航综合实验五实验报告Word下载.docx
《北航惯性导航综合实验五实验报告Word下载.docx》由会员分享,可在线阅读,更多相关《北航惯性导航综合实验五实验报告Word下载.docx(19页珍藏版)》请在冰豆网上搜索。
(3)陀螺漂移£
引起的误差:
=0.0137m
6
可得1min后的位置误差值5x=+J.r2+5x.=1.8157m
2、一分钟惯导实验数据验证结果:
(1)纯惯导解算1min的位置及位置误差图:
(2)纯惯导解算1min的速度及速度误差图:
vy80
60
|40
20
00100020003000400050006000
0.01s
实验结果分析:
纯惯导解算短时间内精度很高,1min的惯导解算的北向最大位移误差-2.668m,东向最大位移误差-8.231m,可见实验数据所得位置误差与理论推导的位置误差在同一数量级,结果不完全相同是因为理论推导时做了大量简化,而且实验时视GPS为真实值也会带来误差;
另外,可见1min内纯惯导解算的东向速度最大误差-0.2754m/s,北向速度最大误差-0.08027m/So
(-)选取IMU前5分钟数据进行对准实验。
将初始对准结果作为初值完成1小时捷联惯导和组合导航解算,对比1小时捷联惯导和组合导航结果。
1、5niinlMU数据的解析粗对准结果:
-1.1960
-0.9998
-0.0084
Cnh=
0.9979
-1.1990
-0.0163
0.0062
0.1198
0.9991
attitude=[219.7700-0.93180.4828]
fai=219.9744642380873*pi/180;
theta=-0.895865732956914*pi/180;
gama=0.640089448357591*pi/l80;
1
遂0.5
X
*
xio北问速度误差方差
*0
xi(/北向速度误差4
23
0.01秒x104
xio来向速度误差方差
•20123
3
侧2.5
o
12
0.01秒X面
C
)123
00秒X104
X103北向失准角
X10’北向失准角方差
8S
3、一小时
,
xioF向失准角方差
X103条向失准角
。
・。
1秒x104
x10’天向失准角方差
0.01秒*104
东向速度
任〜
-201[1[[1c[
00.511.522.533.54
001SX105
北向i®
度
.20[[[LL_
00.511.522.5
0.01S
天向速度
300
0.01Sx105
俯仰角
°
_.—\%A(<
«
*'
S^/-'
>
zA/YJp>
yV
■5ccIccIc
xio^Pffi®
角误差
41111
X1031
P乐向速度误差
1I
1■,1■■
0.5-
■
irrirr
u■
00.5
11.522.533.54
0.05Sv奇
X10
Xio'
P北向速度误差
1T
0.5•
nr
u
0.05SX105
x10'
1
P天向速度误差
1iV1€€
.
0.01sX105
w
.。
估*105
4、一小时IMU数据的捷联惯导解算结果与组合滤波、GPS输出对比图:
0.01Sheight
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实
A)A)A)/UA)/UA)A)A)A)/UA)/UA)A)A)A)A)A)/UA)/U
%该程序为15维状态量,6维观测量的kalman滤波程序,惯性/卫星组合松耦合的数学模型
clear
clc
closeall
%%初始量定义
\vie=0.1467;
Re=6378135.072;
g=9.7803267714;
%IMU频率100hz,此程序中GPS频率100hz
%数据时间3600s
e=1.0/298.25;
T=0.01;
datanumber=360000;
%组合后的速度信息
%组合后的姿态信息
%组合后的位置信息
%组合导航的初始位置、姿态、速度
%捷联解算及卡尔曼相关
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);
[at=40.46*pi/180;
Ion=116.60*pi/180;
height=43.0674;
fai=219.73*pi/180;
theta=-0.6914*pi/180;
gama=0.7591*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)A2,(50e-6*g)A2,(50e-6*g)A2,(0.1*pi/180/3600)A2,(0.1*pi/180/3600)A2
(0.1*pi/180/3600)A2,0,0,0,0,0,0,0,0,0]);
%随机
Q=diag([(0.008*pi/180/3600)A2,(0.008*pi/180/3600)A2,(0.008*pi/180/3600)A2,(50e-6*g)
A2,(50e-6*g)A2,(50e-6*g)A2,0,0,0,0,0,0,0,0,0]);
R=diag([(0.01)A2,(0.01)A2,(0.01)A2,(0.1)A2,(0.1)A2,(0.15)A2]);
P=zeros(15);
P_k=diag([(0.00005*pi/180)A2,(0.00005*pi/180)A2,(0.00005*pi/180)A2,0.00005A2,0.0000
5A2,0.00005A2,2A2,2A2,2A2,(0.001*pi/180/3600)A2,(0.001*pi/180/3600)A2,(0.001*pi/18
0/3600)A2,(50e-6*g)A2,(50e-6*g)A2,(50e-6*g)A2]);
%
K=zeros(15,6);
Z=zeros(6,1);
l=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;
v/ie*cos(lat);
vzie*sin(lat)];
V/enn=[-Vy/Rmh;
Vx/Rnh;
Vx*tan(lat)/Rnh];
Winn=Wien+Wenn;
V/inb=Cnb*Winn;
forj=1:
gyro(j,1)=v/(j,i);
acc(j,1)=f(j,i)*g;
%加速度信息,单位化为m/s"
end
angle=(gyro-Winb)*T;
fn=Cnb1*acc;
difVx=fn
(1)+(2.0*v/ie*sin(lat)+Vx*tan(lat)/Rnh)*Vy;
difVy=fn
(2)-(2.0*v/ie*sin(lat)+Vx*tan(lat)/Rnh)*Vx;
difVz=fn(3)+(2.0*v/ie*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;
Ion=Ion+Vx*T/Rnh/cos(lat);
height=height+Vz*T;
M=[0,-angle(l),-angleQ),・angle(3);
angle.),0,angle(3),-angle
(2);
angle
(2),-angle(3),0,angle
(1);
angle⑶,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⑷),
2*(q
(2)P⑷・q⑴P⑶);
2*(q
(2)*q(3)-q
(1)*q(4)),q
(1)*q⑴-q
(2)*q
(2)+q(3)*q(3)-q⑷*q⑷,
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)-(v/ie*cos(lat)+v(i,1)/(Rnh))0-1/(Rmh)0000;
-(\vie*sin(lat)+v(i,1)*tan(lat)/(Rnh))0-v(i,2)/(Rmh)1/(Rnh)00
-wie*sin(lat)00;
v/ie*cos(lat)+v(i,1)/(Rnh)v(i,2)/(Rmh)0
ran(srr)、(Rnh)00wie*cos(srr)+v(L2)*sec=at:
rsec(_at:
)/(Rnh)00;
0,m(3)fnsv(L2)*ran=arv(Rmh),v(L3M(Rmh)2zMQSm=at:
)+v(£
:
)*t:
aMat:
)、(Rnh)・(2*wiQcosaa-)+v(Ll)、(Rnh))
fns0—fn3,2*<
MQSm(srt:
)+v(Lq)*ran(srr)、(Rnh)),v(L3)、(Rmh).v(L2)、(Rmh)—(2*wi%cos(srrf)+v(Llr$c(srr)*sec(5rt:
)、(Rnh))<
r(Ll)09
fn
(2)
2*(wie*cos?
r)+v(LlM(RM)),2J<
.e*sm(_at:
)*v(Ll)05?
000
000-
000
<
Ll)*sec(srr)rramsrr)、(Rnh)00;
h
GH【cnb.Neros(3);
zerossnnbLzeros?
6)L
fn30
2<
L2)、(Rmh)0
0MRmh)0
sec(_ac/(Rnh)
nIL卜q=卜qcvzeros(3)Neros(3)kag(【Rmh》Rnh*cos?
t:
r】)Neros(3》6)】;
决HI奏F2"
nnb.Neros(3);
zeros(3)nn5
zeros(3)Neros(3)】;
F"
FLF2j
zeros?
」5二;
次ka-man繇需藏厩#愁fhf*t“流蛰薄尖remplHeye(15);
disFHeye(15)jforjn1:
10
remsHF*rems一"
disFndisF+remp。
;
sd
Empl"
Q*T;
disQHrempl"
forjH2三
gmp2HF
Empqn(remp2+remp2-)、j;
wd・・
4J
对123456
『/V
ezzzzzz
disQ=disQ+tempi;
Vx-gps_v(i,1);
%试测量为纯惯导与GPS的速度差、位置差
Vy-gps_v(i,2);
Vz•gps_v(i,3);
(lat-gps_pos(i,1))*Rmh;
%纬经度化为位移,单位m
(Ion・gps_pos(i,2))*Rnh*cos(lat);
height.gps_pos(i,3);
X=disF*X_o;
%kalman滤波五个公式
P=disF*P_k*disF+disQ;
K=P*H7(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*18O;
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,1O)=X_k(1O)/pi*180*3600;
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)"
0人6/g;
x_kf(i,14)=X_k(14)*10A6/g;
x_kf(i,15)=X_k(15)r(T6/g;
%平台误差角
%速度误差
%位置误差
%陀螺随机常值漂移,单位°
/h
%加计随机常值偏置,单位Ug
P_kf(iJ)=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)))*10A6/g;
p_kf(i,14)=sqrt(abs(P_k(14,14)))*10A6/g;
p_kf(iJ5)=sqrt(abs(P_k(15,15)))*10A6/g;
Vx=Vx-X_k⑷;
%速度校正
Vy=Vy・X_k(5);
Vz=Vz•X_k(6);
v(i,:
)=[Vx,Vy,Vz];
lat=lat-X_k⑺;
Ion=Ion-X_k(8);
height=height-X_k(9);
pos(i,:
)=[lat,Ion,height];
Atheta=X_k
(1);
Agama=X_k
(2);
Afai=X_k⑶;
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<
fai=fai+2*pi;
%位置校正
%kalman滤波估计得出的失准角theta%kalman滤波估II•得出的失准角gania%kalman滤波估讣得出的失准角fai
%更新姿态阵
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
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 北航 惯性 导航 综合 实验 报告