资源简介
采用联邦滤波进行的惯性、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
- 上一篇:JPEG图像压缩编码
- 下一篇:光伏电池输出特性曲线MATLAB代码
相关资源
- 3点GPS定位MATLAB仿真
- GPS信号的码捕获matlab代码.7z
- 提取图片纹理特征能量、熵、惯性矩
- GPS定位程序代码
- GPS基本原理及其Matlab仿真235516
- 压缩感知之GPSR算法
- GPS信号捕获跟踪的MATLAB仿真,里面有
- gps基本原理及其matlab仿真199992
- RINEX格式的GPS观测文件matlab读取程序附
- ps基本原理及其matlab仿真(西安电子科
- GPS基本原理及MATLAB仿真
- 基于惯性传感器的室内导航的MATLAB轨
- INS_GPS组合导航matlab仿真程序
- GPS仿真-matlab工具箱
- GPS周跳探测matlab
- SINS、GPS、DR仿真
- 新手适用简易GPS matlab代码
- 扩展Kalman滤波在INS/GPS组合导航系统
- Matlab GPS Toolbox
- 卫星位置计算器实验材料.zip
- 导航卫星坐标定位和伪距测量结算及
- PPPH源码matlab有说明书
- 《GNSS与惯性及多传感器组合导航系统
- GNSS 与惯性及多传感器组合导航系统原
- GPS基本原理及其MATLAB仿真178018
- 软件定义的GPS和伽利略接收机163752
- GNSS与惯性及多传感器组合导航系统原
- 惯性导航matlab开源程序GPS&IMU;数据融合
- GPS+IMU室内外无缝定位数据融合MATLAB程
- 英文原版全球定位系统—信号、测量
评论
共有 条评论