2021-04-21

#捷联惯导算法与组合导航（2020）
惯性/卫星组合导航仿真

gvar;
nn=2;ts=0.1;nts=nn*ts;
att0=[0;0;30]*arcdeg;qnb0=a2qua(att0);%将姿态角转换为四元数
vn0=[0;0;0];pos0=[34*arcdeg;108*arcdeg;100];
qnb=qnb0;vn=vn0;pos=pos0;%姿态、速度和位置初始化
eth=earth(pos,vn);%求出地球导航参数：wnie:地球自转角速度   wnin:机体坐标系相对于惯性坐标系的旋转   gn:机体坐标系下的重力  gcc：有害加速度
wm=qmulv(qconj(qnb),eth.wnie)*ts;%b系坐标到n系坐标系的旋转角度增量（在一次采样时间内）
vm=qmulv(qconj(qnb),-eth.gn)*ts;%比力由导航系换算到机体系，得到机体比力，并乘上时间得到机体的速度增量
wm=repmat(wm',nn,1);vm=repmat(vm',nn,1);%静态IMU数据仿真

phi=[0.1;0.2;3]*arcmin;%失准角误差
qnb=qaddphi(qnb,phi);%由导航系换算到机体系的四元数加上失准角
eb=[0.01;0.015;0.02]*dph;%陀螺常值零偏
web=[0.001;0.001;0.001]*dpsh;%角度随机游走
db=[80;90;100]*ug;%加速度计常值零偏
wdb=[1;1;1]*ugpsHz;%速度随机游走

Qk=diag([web;wdb;zeros(9,1)])^2*nts;%系统噪声的方差
rk=[[0.1;0.1;10];[[10;10]/Re;10]];
Rk=diag(rk)^2;%量测噪声的方差
P0=diag([[0.1;0.1;10]*arcdeg;[1;1;1];[[10;10]/Re;10];[0.1;0.1;0.1]*dph;[80;90;100]*ug])^2;
Hk=[zeros(6,3),eye(6),zeros(6)];%量测矩阵
kf=kfinit(Qk,Rk,P0,zeros(15),Hk);%kf滤波器初始化
len=fix(3600/ts);%仿真时长   仿真1个小时，得到了len个字样
err=zeros(len,10);xkpk=zeros(len,2*kf.n+1);kk=1;t=0;%记录导航结果
for k=1:nn:len
    t=t + nts;
    [wm1,vm1]=imuadderr(wm,vm,eb,web,db,wdb,ts);%给角度增量和速度增量添加误差
    [qnb,vn,pos,eth]=insupdate(qnb,vn,pos,wm1,vm1,ts);%姿态，速度，位置更新（含有圆锥和划船误差补偿）
    kf.Phikk_1=eye(15)+kfft15(eth,q2mat(qnb),sum(vm1,1)'/nts)*nts;%状态一步转移矩阵（省略了臂杆和时间不同步误差）
    kf=kfupdate(kf);%仅使用IMU对状态的估计，
    if mod(t,1)<nts
        gps=[vn0;pos0]+rk.*randn(6,1);%GPS数据的仿真
        kf=kfupdate(kf,[vn;pos]-gps,'M');
    end
    qnb=qdelphi(qnb,kf.Xk(1:3));%姿态角去除掉kalman输出的误差值。（kalman对IMU姿态的修正）
    kf.Xk(1:3)=0;
    vn=vn-kf.Xk(4:6);%kalman对IMU速度的修正
    kf.Xk(4:6)=0;
    pos=pos-kf.Xk(7:9);%kalman对IMU位置的修正
    kf.Xk(7:9)=0;
    err(kk,:)=[qq2phi(qnb,qnb0);vn-vn0;pos-pos0;t]';%记录IMU修正后的数据，与真实值的差（kalman滤波的效果）
    xkpk(kk,:)=[kf.Xk;diag(kf.Pk);t];%记录均方误差阵的对角线元素
    kk=kk+1;
    if mod(t,500)<nts, disp(fix(t));end %每500次显示一下进度
end
err(kk:end,:)=[];
xkpk(kk:end,:)=[];
tt=err(:,end);