%?GPS/INS/地磁組合導(dǎo)航，采用聯(lián)邦濾波算法

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;%陀螺漂移反向相關(guān)時間
atime=1/1800;
gyronoise=（0.001/3600）/180*pi;%陀螺漂移白噪聲
beta_d=1/6000.0;?????%速度偏移誤差反向相關(guān)時間
beta_drta=1/6000.0;??%偏流角誤差反向相關(guān)時間

%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