clc;
clear all;
format long;
Wie=7.292115147e-5;
e=1/298;
Re=6378245;
longi=2.024581932313422;
lati=0.698132683058538;
gao=1000;
vx=0;
vy=500;
vz=0;
hangxiang=0; %姿态角
fuyang=0;
henggun=0;
hk=zeros(6,15);
g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;
g=g0*((1+gk1*sin(lati)^2)*(1-2*gao/Re)/sqrt(1-gk2*sin(lati)^2));
cnb=zeros(3,3);
cnb(1,1)=cos(henggun)*cos(hangxiang)-sin(henggun)*sin(fuyang)*sin(hangxiang);
cnb(1,2)=-cos(henggun)*sin(hangxiang)+cos(hangxiang)*sin(fuyang)*sin(henggun);
cnb(1,3)=-cos(fuyang)*sin(henggun);
cnb(2,1)=sin(hangxiang)*cos(fuyang);
cnb(2,2)=cos(hangxiang)*cos(fuyang);
cnb(2,3)=sin(fuyang);
cnb(3,1)=cos(hangxiang)*sin(henggun)-sin(hangxiang)*sin(fuyang)*cos(henggun);
cnb(3,2)=-sin(hangxiang)*sin(henggun)-cos(hangxiang)*sin(fuyang)*cos(henggun);
cnb(3,3)=cos(fuyang)*cos(henggun);
lamda=zeros(4,1); %初始四元数lamda值
lamda(1)=sqrt(1+cnb(1,1)+cnb(2,2)+cnb(3,3))/2;
if cnb(2,3)-cnb(3,2)<0
fuhao1=-1;
else fuhao1=1;
end
if cnb(3,1)-cnb(1,3)<0
fuhao2=-1;
else fuhao2=1;
end
if cnb(1,2)-cnb(2,1)<0
fuhao3=-1;
else fuhao3=1;
end
lamda(2)=fuhao1*sqrt(1+cnb(1,1)-cnb(2,2)-cnb(3,3))/2;
lamda(3)=fuhao2*sqrt(1-cnb(1,1)+cnb(2,2)-cnb(3,3))/2;
lamda(4)=fuhao3*sqrt(1-cnb(1,1)-cnb(2,2)+cnb(3,3))/2;%四元数值求解
xk=zeros(15,1);
x4=zeros(600,1);
pk4=zeros(600,1);
% p,q方差阵
pp=[(pi/180)^2,(pi/180)^2,(pi/180)^2,0.165^2,0.32^2,0.124^2,(0.00008)^2,(0.0000005)^2,2.9^2,(0.02*pi/(3600*180))^2,(0.02*pi/(3600*180))^2,(0.02*pi/(3600*180))^2,(9.8e-4)^2,(9.8e-4)^2,(9.8e-4)^2];
pk=diag(pp);
qq=[(0.01*pi/(3600*180))^2,(0.01*pi/(3600*180))^2,(0.01*pi/(3600*180))^2,(4.9e-4)^2,(4.9e-4)^2,(4.9e-4)^2];
qk=diag(qq);
G=zeros(15,6);
F=zeros(15,15);
fid1 = fopen('gps.dat', 'r');
fid2 = fopen('imu.dat', 'r');
for i=1:600
for n=1:80
dnav = fscanf(fid2, '%g ', [1 1]); % 去掉数据序号
nav = fscanf(fid2, '%lf ', [3 2]);
nav=nav';
wib_INSc=(nav(1,:))';
f_INSc=(nav(2,:))';
n_rxt =(1-e*sin(lati)*sin(lati))/Re; %卯酉主曲率半径倒数
n_ryt =(1+2*e-3*e*sin(lati)*sin(lati))/Re; %子午主曲率半径倒数
g=g0*((1+gk1*sin(lati)^2)*(1-2*gao/Re)/sqrt(1-gk2*sin(lati)^2));
winn=[-vy*n_ryt;Wie*cos(lati)+vx*n_rxt;Wie*sin(lati)+vx*n_rxt*tan(lati)];
winb=cnb*winn;
wnbb=wib_INSc*80-winb;
dcta=[0,-wnbb(1),-wnbb(2),-wnbb(3);wnbb(1),0,wnbb(3),-wnbb(2);wnbb(2),-wnbb(3),0,wnbb(1);wnbb(3),wnbb(2),-wnbb(1),0];%四阶角增量法求解
dcta0=sqrt((wnbb(1))^2+(wnbb(2))^2+(wnbb(3))^2)/100;
cs=1-dcta0^2/8-dcta0^4/384;
sn=0.5-dcta0^2/48;
lamda=(eye(4)*cs+dcta*sn)*lamda;
molamda=sqrt(lamda(1)*lamda(1)+lamda(2)*lamda(2)+lamda(3)*lamda(3)+lamda(4)*lamda(4));
lamda=lamda/molamda;
cnb(1,1)=lamda(1)*lamda(1)+lamda(2)*lamda(2)-lamda(3)*lamda(3)-lamda(4)*lamda(4);
cnb(1,2)=2*(lamda(2)*lamda(3)+lamda(1)*lamda(4));
cnb(1,3)=2*(lamda(2)*lamda(4)-lamda(1)*lamda(3));
cnb(2,1)=2*(lamda(2)*lamda(3)-lamda(1)*lamda(4));
cnb(2,2)=lamda(1)*lamda(1)-lamda(2)*lamda(2)+lamda(3)*lamda(3)-lamda(4)*lamda(4);
cnb(2,3)=2*(lamda(3)*lamda(4)+lamda(1)*lamda(2));
cnb(3,1)=2*(lamda(2)*lamda(4)+lamda(1)*lamda(3));
cnb(3,2)=2*(lamda(3)*lamda(4)-lamda(1)*lamda(2));
cnb(3,3)=lamda(1)*lamda(1)-lamda(2)*lamda(2)-lamda(3)*lamda(3)+lamda(4)*lamda(4);
cbn=cnb';
fibn=cbn*f_INSc;
ax=fibn(1)+(2*Wie*sin(lati)+vx*n_rxt*tan(lati))*vy-(2*Wie*cos(lati)+vx*n_rxt)*vz;%x方向加速度方程
dx=ax/80; %x方向速度增量
vx=vx+dx; %x方向速度
ay=fibn(2)-(2*Wie*sin(lati)+vx*n_rxt*tan(lati))*vx+vy*n_ryt*vz;%y方向加速度方程
dy=ay/80; %y方向速度增量
vy=vy+dy; %y方向速度增量
az=fibn(3)-g+(-2*Wie*cos(lati)+vx*n_rxt)*vx+vy*vy*n_ryt;
dz=az/80;
vz=vz+dz;
wx=vx*n_rxt*sec(lati); %x方向角速度
wy=vy*n_ryt; %y方向角速度
dlati=wy/80; %采样时间内纬度增量
lati=lati+dlati;
dlongi=wx/80; %采样时间内经度增量
longi=longi+dlongi;
dh=vz/80; %采样时间内高度增量
gao=gao+dh;
end
%以下开始滤波解算
vxnav=vx; ve=vx;
vynav=vy; vn=vy;
vznav=vz; vu=vz;
fe=fibn(1);
fn=fibn(2);
fu=fibn(3);
%GPS滤波修正前子午圈、卯酉圈半径计算
h=gao;
Rm=(Re+h)*(1-2*e+3*e*sin(lati)*sin(lati));
Rn=(Re+h)*(1+e*sin(lati)*sin(lati));
ee=[0.3^2,0.3^2,0.3^2,5^2,5^2,5^2];
rk=diag(ee); %速度位置误差方阵
%量测阵H
hk(1,4)=1;
hk(2,5)=1;
hk(3,6)=1;
hk(4,7)=Rm;
hk(5,8)=Rn*cos(lati);
hk(6,9)=1;
%给出 F 阵
F(1,2)=Wie*sin(lati)+ve*tan(lati)/(Rn+gao);
F(1,3)=-Wie*cos(lati)-ve/(Rn+gao);
F(1,5)=-1/(Rm+gao);
F(2,1)=-F(1,2);
F(2,3)=-vn/(Rm+gao);
F(2,4)=1/(Rn+gao);
F(2,7)=-Wie*sin(lati);
F(3,1)=-F(1,3);
F(3,2)=-F(2,3);
F(3,4)=tan(lati)/(Rn+gao);
F(3,7)=Wie*cos(lati)+ve*sec(lati)*sec(lati)/(Rn+gao);
F(4,2)=-fu;
F(4,3)=fn;
F(4,4)=vn*tan(lati)/(Rm+gao)-vu/(Rm+gao);
F(4,5)=2*Wie*sin(lati)+ve/(Rn+gao);
F(4,6)=-2*Wie*cos(lati)-ve/(Rn+gao);
F(4,7)=2*Wie*cos(lati)*vn+ve*vn*sec(lati)*sec(lati)/(Rn+gao)+2*Wie*sin(lati)*vu;
F(5,1)=fu;
F(5,3)=-fe;
F(5,4)=-2*(Wie*sin(lati)+ve*tan(lati)/(Rn+gao));
F(5,5)=-vu/(Rm+gao);
F(5,6)=-vn/(Rm+gao);
F(5,7)=-2*Wie*cos(lati)*ve-ve*ve*sec(lati)*sec(lati)/(Rn+gao);
F(6,1)=-fn;
F(6,2)=fe;
F(6,4)=-2*(Wie*cos(lati)+ve/(Rn+gao));
F(6,5)=2*vn/(Rm+gao);
F(6,7)=-2*ve*Wie*sin(lati);
F(7,5)=1/(Rm+gao);
F(7,9)=-vn/(Rm+gao)^2;
F(8,4)=sec(lati)/(Rn+gao);
F(8,7)=ve*sec(lati)*tan(lati)/(Rn+gao);
F(8,9)=-ve*sec(lati)/((Rn+gao)^2);
F(9,6)=1;
F(1,13)=1;
F(2,14)=1;
F(3,15)=1;
F(4,10)=1;
F(5,11)=1;
F(6,12)=1;
%G阵
G=[cbn zeros(3,3);
zeros(3,3) cbn;
zeros(9,3) zeros(9,3)];
fi=eye(15)+F+0.5*F*F;
%读入GPS数据
dgps = fscanf(fid1, '%g ', [1 1]); % 去掉数据序号
gps = fscanf(fid1, '%lf ', [3 2]);
gps=gps';
%GPS速度位置信息
vxgps(i)=gps(1,1);
vygps(i)=gps(1,2);
vzgps(i)=gps(1,3);
Lgps(i)=gps(2,1);
lamdagps(i)=gps(2,2);
Hgps(i)=gps(2,3);
%量测值
Z=zeros(6,1);
Z(1)=vxnav-vxgps(i);
Z(2)=vynav-vygps(i);
Z(3)=vznav-vzgps(i);
Z(4)=(lati-Lgps(i));%*Rm;
Z(5)=(longi-lamdagps(i));%*Rn*cos(lati);
Z(6)=gao-Hgps(i);
%进行滤波
xk=zeros(15,1);
pkk=fi*pk*fi'+G*qk*G';
kk=pkk*hk'*inv(hk*pkk*hk'+rk);
pk=(eye(15)-kk*hk)*pkk*(eye(15)-kk*hk)'+kk*rk*kk';
xkk=fi*xk;
xk=xkk+kk*(Z-hk*xkk);
x1(i)=xk(1)*180*3600/pi; % 变为角秒量级单位
x2(i)=xk(2)*180*3600/pi;
x3(i)=xk(3)*180*3600/pi;
x4(i)=xk(4);
x5(i)=xk(5);
x6(i)=xk(6);
vdong(i)=vx-xk(4);
vbei(i)=vy-xk(5);
vtian(i)=vz-xk(6);
vx=vdong(i);
vy=vbei(i);
vz=vtian(i);
x7(i)=xk(7)*Rm;
x8(i)=xk(8)*Rn*cos(lati);
x9(i)=xk(9);
latidu(i)=lati-xk(7);
longidu(i)=longi-xk(8);
gaodu(i)=gao-xk(9);
lati=latidu(i);
longi=longidu(i);
gao=gaodu(i);
x10(i)=xk(10);
x11(i)=xk(11);
x12(i)=xk(12);
x13(i)=xk(13);
x14(i)=xk(14);
x15(i)=xk(15);
%误差方差
pk1(i)=sqrt(pk(1,1))*180*3600/pi;
pk2(i)=sqrt(pk(2,2))*180*3600/pi;
pk3(i)=sqrt(pk(3,3))*180*3600/pi;
pk4(i)=sqrt(pk(4,4));
pk5(i)=sqrt(pk(5,5));
pk6(i)=sqrt(pk(6,6));
pk7(i)=sqrt(pk(7,7));
pk8(i)=sqrt(pk(8,8));
pk9(i)=sqrt(pk(9,9));
pk10(i)=sqrt(pk(10,10));
pk11(i)=sqrt(pk(11,11));
pk12(i)=sqrt(pk(12,12));
pk13(i)=sqrt(pk(13,13));
pk14(i)=sqrt(pk(14,14));
pk15(i)=sqrt(pk(15,15));
xk(4)=0;
xk(5)=0;
xk(6)=0;
xk(7)=0;
xk(8)=0;
xk(9)=0;
没有合适的资源?快使用搜索试试~ 我知道了~
资源推荐
资源详情
资源评论
收起资源包目录
matlab-GPS-AUV惯性导航系统,包括轨迹生成、gps和sins组合、gps和dvl组合 (108个子文件)
sins1gps.asv 12KB
husagps.asv 10KB
test_SINS_Dvl.asv 2KB
test_sins.asv 2KB
test_sins.asv 1KB
test_imu_sim.asv 1KB
IMU.dat 8.46MB
IMU.dat 8.46MB
lgimu.dat 3.23MB
lgimu.dat 3.23MB
GPS.dat 86KB
GPS.dat 86KB
sins1gps.m 12KB
husagps.m 10KB
test_trj.m 3KB
test_trj.m 2KB
test_SINS_Dvl.m 2KB
test_sins.m 2KB
test_sins.m 1KB
glvs.m 1KB
glvs.m 1KB
getf_DR.m 1KB
align_i0.m 1KB
test_imu_sim.m 1KB
getf.m 1KB
getf.m 1KB
rfu.m 1KB
rfu.m 1KB
markov.m 986B
test_sins_static.m 971B
allan.m 959B
drift.m 947B
compassCmd.m 939B
ukf.m 916B
ukf.m 916B
trj.m 858B
trj.m 858B
nav_init.m 853B
kfdis.m 683B
kfdis.m 683B
rv2q.m 599B
rv2q.m 599B
earth.m 595B
earth.m 578B
prls.m 576B
kalman.m 556B
compasskxyz.m 554B
ImuAddErr.m 511B
sins.m 482B
cnscl.m 482B
sins.m 482B
cnscl.m 482B
q2cnb.m 455B
q2cnb.m 455B
dv2atti.m 444B
kalman.m 440B
make_dll.m 419B
make_dll.m 419B
m2qnb.m 382B
m2qnb.m 382B
DR.m 365B
large_phi_model.m 347B
large_phi_model.m 347B
qmul.m 327B
qmul.m 327B
dv2atti.m 318B
meann.m 307B
a2cnb.m 297B
a2cnb.m 297B
nav_err.m 237B
ImuStatic.m 229B
p2cne.m 222B
a2caw.m 211B
a2caw.m 211B
RLS.m 201B
a2cwa.m 193B
a2cwa.m 193B
q2rv.m 177B
q2rv.m 177B
dms2r.m 160B
incre.m 156B
m2att.m 138B
m2att.m 138B
r2dms.m 137B
askew.m 131B
askew.m 131B
timedisp.m 108B
qmulv.m 102B
qmulv.m 102B
dm2r.m 100B
r2dm.m 95B
qq2phi.m 87B
qq2phi.m 87B
qaddphi.m 67B
qaddphi.m 67B
qdelphi.m 65B
qdelphi.m 65B
a2qnb.m 55B
q2att.m 55B
a2qnb.m 55B
共 108 条
- 1
- 2
资源评论
- 曾经的你CTW2023-10-19这个资源总结的也太全面了吧,内容详实,对我帮助很大。
- fagege0012023-08-20资源有一定的参考价值,与资源描述一致,很实用,能够借鉴的部分挺多的,值得下载。
- 2301_768882592024-04-01感谢大佬分享的资源给了我灵感,果断支持!感谢分享~
wouderw
- 粉丝: 273
- 资源: 2960
下载权益
C知道特权
VIP文章
课程特权
开通VIP
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功