惯导与ＧＰＳ的组合导航程序

利用捷联惯导与ＧＰＳ进行组合，得到导航参数误差输出

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）=-bet

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      14109  2002-01-11 21:50  sinsgpstcm2kf.m

----------- ---------  ---------- -----  ----

                14327                    2