资源简介
采用联邦滤波进行的惯性、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代码
相关资源
- GPS计算用户位置
- 精密单点定位的Matlab code
- 一阶惯性延迟系统的PID自整定
- 基于GPS_IMU组合定位的kalman滤波
- SINS捷联惯导解算程序
- GPSC/A码程序
- GPS信号产生及捕获
- 论文研究-GPS系统C/A码的产生与捕获算
- GPS接受机中的卡尔曼滤波MATLAB代码
- GPS信号产生、捕获、追踪全套程序
- INS/GPS组合导航MATLAB处理库
- INS+GPS组合导航matlab程序
- gps捕获程序
- 基于matlab的GPS数据读取
- 惯导六位置法标定解算Matlab程序
- 生成GPS信号
- 基于matlab读取GPS中GPGGA信息(包含经纬
- GPS_INS位置组合Matlab仿真源码
- GPS中CA码产生方法
- 惯性导航扩展卡尔曼滤波MATLAB
- 地心地固坐标系ECEF转地心惯性系ECI
- GPS周跳检测程序
- 基于MATLAB的GPS网平差
- GPS卫星坐标计算的简单matlab程序自己
- GY-85惯性模块的实时数据绘图与处理包
- 用四元数法的捷联惯性导航姿态解算
- 基线解算的matlab源码
- 捷联惯性导航的轨迹生成
- matlab编写的GPS和惯导的组合程序
- gps最小二乘定位
评论
共有 条评论