资源简介

采用联邦滤波进行的惯性、GPS、地磁组合导航,对于理解组合导航、地磁算法、联邦滤波算法有很好的意义。适用于任何导航领域。

资源截图

代码片段和文件信息

% GPS/INS/地磁组合导航,采用联邦滤波算法

clear
R=6378137;
omega=7292115.1467e-11;
g=9.78;
T=14.4;
time=3750;
yinzi1=0.5;
yinzi2=0.5;

%initial value
fai0=30*pi/180;
lamda0=30*pi/180;
vxe0=0.01;
vye0=0.01;

faie0=2.0/60*pi/180;
lamdae0=2.0/60*pi/180;
afae0=3.0/60*pi/180;
beitae0=3.0/60*pi/180;
gamae0=5.0/60*pi/180;

hxjz=pi/4;
vx=20*1852/3600*sin(hxjz);
vy=20*1852/3600*cos(hxjz);
%
weichagps=25;%GPS位置误差
suchagps=0.05;%GPS速度误差
gyroe0=(0.01/3600)*pi/180;
gyrotime=1/7200;%陀螺漂移反向相关时间
atime=1/1800;
gyronoise=(0.001/3600)/180*pi;%陀螺漂移白噪声
beta_d=1/6000.0;     %速度偏移误差反向相关时间
beta_drta=1/6000.0;  %偏流角误差反向相关时间

%matrix of system equation

fai=fai0;
lamada=lamda0;

zong=0*pi/180;
heng=0*pi/180;
hang=45*pi/180;


F(1616)=0;
G(169)=0;




%initial value
x1(161)=0;

%the error of sins

xx=x1;

xx(1)=faie0;  %ljn
xx(2)=lamdae0;

xx(5)=afae0;
xx(6)=beitae0;
xx(7)=gamae0;
xx(8)=(0.01/3600)*pi/180;
xx(9)=(0.01/3600)*pi/180;
xx(10)=(0.01/3600)*pi/180;
xx(11)=0.0005;
xx(12)=0.0005;
xx(13)=0.0005;


%w=[gyronoisegyronoisegyronoisegyronoisegyronoisegyronoiseg*1e-5g*1e-5]‘;
g1=randn(1time);
g2=randn(1time);
g3=randn(1time);
g4=randn(1time);
g5=randn(1time);
g6=randn(1time);
g7=randn(1time);
g8=randn(1time);
g9=randn(1time);


% attitude change matrix

cbn(11)=cos(zong)*cos(hang)+sin(zong)*sin(heng)*sin(hang);
cbn(12)=-cos(zong)*sin(hang)+sin(zong)*sin(heng)*cos(hang);
cbn(13)=-sin(zong)*cos(heng);
cbn(21)= cos(heng)*sin(hang);
cbn(22)=cos(heng)*cos(hang);
cbn(23)=sin(heng);
cbn(31)= sin(zong)*cos(hang)-cos(zong)*sin(heng)*sin(hang);
cbn(32)=-sin(zong)*sin(hang)-cos(zong)*sin(heng)*cos(hang);
cbn(33)=cos(zong)*cos(heng);

F(14)=1/R;
F(23)=1/(R*cos(fai));
%F(31)=2*omega*vx*cos(fai)+vx*vy*sec(fai)^2/R;
F(31)=2*omega*vy*cos(fai)+vx*vy*sec(fai)^2/R;
%F(33)=vx*tan(fai)/R;
F(33)=vy*tan(fai)/R;
F(34)=vx*tan(fai)/R+2*omega*sin(fai);
F(36)=-g;
%F(41)=-(2*omega*vx*cos(fai)+vx^2*sec(fai)^2/R);
F(41)=-(2*omega*vx*sin(fai)+vx^2*sec(fai)^2/R);
F(43)=-2*(vx*tan(fai)/R+omega*sin(fai));
F(45)=g;
%F(47)=-g;
F(54)=-1/R;
F(56)=omega*sin(fai)+vx*tan(fai)/R;
F(57)=-(omega*cos(fai)+vx/R);
F(58)=1;
F(61)=-omega*sin(fai);
%F(63)=-1/R;
F(63)=1/R;
F(65)=-(omega*sin(fai)+vx*tan(fai)/R);
%F(67)=-vx/R;
F(67)=-vy/R;
F(69)=1;
F(71)=omega*cos(fai)+vx*sec(fai)^2/R;
F(73)=tan(fai)/R;
F(75)=omega*cos(fai)+vx/R;
%F(76)=vx/R;
F(76)=vy/R;
F(710)=1;
F(88)=-gyrotime;
F(99)=-gyrotime;
F(1010)=-gyrotime;

F(311)=cbn(11);
F(312)=cbn(12);
F(313)=cbn(13);

F(411)=cbn(21);
F(412)=cbn(22);
F(413)=cbn(23);

F(58)=cbn(11);
F(59)=cbn(12);
F(510)=cbn(13);

F(68)=cbn(21);
F(69)=cbn(22);
F(610)=cbn(23);

F(78)=cbn(31);
F(79)=cbn(32);
F(710)=cbn(33);

F(1111)=-atime;
F(1212)=-atime;
F(1313)=-atime;
F(1414)=-beta_d;
F(1515)=-beta_drta;
F(1616)=0;


G=[000000000;
    0000000

评论

共有 条评论