GNSS/INS松组合导航Matlab程序

clc clear gvar; imu=load ('imu.mat'); % imu数据： avp=[wm, vm, tt(2:end)]; gx(rad) gy gz ax (m/s) ay az time trj=load ('trj.mat'); % trj=[att, vn, pos]; att(度) vn(米每秒) pos(米)；att -- 欧拉角 arcdeg = pi/180; % 1弧度等于多少度 ；arcdeg -- 度转换为弧度 nn=1; % 子样数 ts=0.01; % 单个采样间隔的采样时间长度 nts=nn*ts; % 采样周期，在nts时间内进行两次采样，每次采样 0.1秒 %初始化参数 att = [0;0;90]*arcdeg; vn0 = [0;0;0]; pos0 = [[34;108]*arcdeg;100]; % vn -- 速度；pos -- 位置（34°N，108°E，高100m） qnb0 = a2qua(att); % a2qua ----姿态角转化成四元数 qnb = qnb0; vn = vn0; pos = pos0; % 姿态、速度和位置初始化 %添加误差 phi = [0.1; 0.2; 3]*arcmin; qnb = qaddphi(qnb, phi); % qaddphi --四元数加失准角误差 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; % 加速度计常值偏值，速度随机游走 % ug=ge*1e-6; g0 = ge; ge -- 赤道重力； ugpsHz= ug/sqrt(1); %初始化kalman参数 Qk = diag([web; wdb; zeros(9,1)])^2*nts; % Qk系统噪声 rk = [[0.1;0.1;0.1];[[10;10]/Re;10]]; Rk = diag(rk)^2; % Rk量测噪声 P0 = diag([[0.1;0.1;10]*arcdeg; [1;1;1]; [[10;10]/Re;10]; % Re赤道长半轴 [0.1;0.1;0.1]*dph; [80;90;100]*ug])^2; % 协方差矩阵，x = [phi, delta_vn, delta_p, eb, db] Hk = [zeros(6,3),eye(6),zeros(6)]; kf = kfinit(Qk, Rk, P0, zeros(15), Hk); len=length(imu.avp); %length --取矩阵最长的维数 avps=zeros(fix(len/2),10); %fix是靠近0取整 avps(1,:)=[att;vn;pos;0]'; t=0;kk=1;err = zeros(len, 10); xkpk = zeros(len, 2*kf.n+1); for k=1:nn:len t=t+nts; delta_theta_m=imu.avp(k,1:3); delta_v_m=imu.avp(k,4:6); [wm_b, vm_b] = imuadderr(delta_theta_m, delta_v_m, eb, web, db, wdb, ts);% IMU添加零偏与游走误差 [qnb,vn,pos,eth]=insupdate(qnb,vn,pos,wm_b,vm_b,ts); kf.Phikk_1 = eye(15) + kfft15(eth, q2mat(qnb), sum(vm_b,1)'/nts)*nts; % kf.Phikk_1 = Phikk_1; kfft15 -- SINS误差转移矩阵 kf = kfupdate(kf); % Kalman滤波时间更新 if mod(t,1)<nts gps = trj.trj(k,4:9)' + rk.*randn(6,1); % GPS速度位置仿真 kf = kfupdate(kf, [vn;pos]-gps, 'M'); % Kalman滤波量测更新 end qnb = qdeiphi(qnb,kf.Xk(1:3)); kf.Xk(1:3) = 0; % kf.Xk = zeros(kf.n,1); qdelphi -- 四元数真实值=四元数计算值-失准角；误差反馈 vn = vn - kf.Xk(4:6); kf.Xk(4:6) = 0; pos = pos - kf.Xk(7:9); kf.Xk(7:9) = 0; err(kk,:) = [qq2phi(qnb,a2qua(trj.trj(k,1:3))'); vn-trj.trj(k,4:6)'; pos-trj.trj(k,7:9)'; t]'; % qq2phi -- 失准角误差=四元数计算值-四元数真值 xkpk(kk,:) = [kf.Xk; diag(kf.Pk); t]; % kf.Pk = P0; [kf.Xk; diag(kf.Pk); t] -- 反馈后的剩余状态、方差阵和时间 kk=kk+1; cnb=q2att(qnb); cnb(3)=mod(cnb(3),2*pi); % avp()=[θ γ ψ VE VN VU B L h t] avps(kk,:)=[cnb; vn; pos; t]'; if mod(t,1)<nts, disp(fix(t)); end % 显示进度 disp函数：显示文本或数组 end %姿态 tt=length(avps); tf=length(trj.trj); figure(1); subplot(321); plot(1:tt,(avps(1:tt,1:2)/arcdeg),1:tf,(trj.trj(1:tf,1:2)/arcdeg),'--'); title('俯仰角和横滚角');xlabel('d');ylabel('\theta,\gamma(\circ)'); legend('\it\theta','\it\gamma','\bf\theta','\bf\gamma');grid on; subplot(322); plot(1:tt,(avps(1:tt,3)/arcdeg),1:tf,(trj.trj(1:tf,3)/arcdeg),'--'); title('偏航角');xlabel('d');ylabel('\Phi(\circ)'); legend('\it\Phi','\bf\Phi') ;grid on; subplot(323); plot(1:tt,(avps(1:tt,4:5)/arcdeg),1:tf,(trj.trj(1:tf,4:5)/arcdeg),'--'); title('速度');xlabel('d');ylabel('Vel/(m.s^{-1})'); legend('\itv\rm_E','\itv\rm_N','\bfv\rm_E','\bfv\rm_N');grid on; subplot(324); plot(1:tt,(avps(1:tt,6)/arcdeg),1:tf,(trj.trj(1:tf,6)/arcdeg),'--'); title('速度');xlabel('d');ylabel('Vel/(m.s^{-1})'); legend('\itv\rm_U','\bfv\rm_U');grid on; subplot(325); plot(1:tt,deltapos(avps(1:tt,7:9)),1:tf,deltapos(trj.trj(1:tf,7:9)),'--'); title('位置');xlabel('d');ylabel('\DeltaPos / m'); legend('\Delta\itL', '\Delta\it\lambda', '\Delta\ith','\Delta\bfL', '\Delta\bf\lambda', '\Delta\bfh');grid on; err(kk:end,:) = []; xkpk(kk:end,:) = []; tt = err(:,end); % 状态真值与估计效果对比图 msplot(321, tt, err(:,1:2)/arcmin, '\it\phi\rm / ( \prime )'); % 俯仰角，滚转角误差 legend('\it\phi\rm_E', '\it\phi\rm_N') % 添加图例 msplot(322, tt, err(:,3)/arcmin, '\it\phi\rm_U\rm / ( \prime )'); % 偏航角误差 msplot(323, tt, err(:,4:6), '\delta\itv^n\rm / ( m.s^{-1} )'); % 三轴速度误差 legend('\delta\itv\rm_E', '\delta\itv\rm_N', '\delta\itv\rm_U') msplot(324, tt, [err(:,7)*Re,err(:,8)*Re*cos(pos(1)),err(:,9)], '\delta\itp\rm / m'); % 三轴位置误差 legend('\delta\itL', '\delta\it\lambda', '\delta\ith') msplot(325, tt, xkpk(:,10:12)/dph, '\it\epsilon\rm / ( (\circ).h^{-1} )');% 三轴陀螺仪常值误差 legend('\it\epsilon_x','\it\epsilon_y','\it\epsilon_z') msplot(326, tt, xkpk(:,13:15)/ug, '\it\nabla\rm / \mu\itg'); % 三轴加速度计误差 legend('\it\nabla_x','\it\nabla_y','\it\nabla_z') % 均方差收敛图 滤波器的可观测性 spk = sqrt(xkpk(:,16:end-1)); msplot(321, tt, spk(:,1:2)/arcmin, '\it\phi\rm / ( \prime )'); legend('\it\phi\rm_E', '\it\phi\rm_N'), msplot(322, tt, spk(:,3)/arcmin, '\it\phi\rm_U\rm / ( \prime )'); msplot(323, tt, spk(:,4:6), '\delta\itv^n\rm / ( m.s^{-1} )'); legend('\delta\itv\rm_E', '\delta\itv\rm_N', '\delta\itv\rm_U') msplot(324, tt, [[spk(:,7),spk(:,8)*pos(1)]*Re,spk(:,9)], '\delta\itp\rm / m'); legend('\delta\itL', '\delta\it\lambda', '\delta\ith') msplot(325, tt, spk(:,10:12)/dph, '\it\epsilon\rm / ( (\circ).h^{-1} )'); legend('\it\epsilon_x','\it\epsilon_y','\it\epsilon_z') msplot(326, tt, spk(:,13:15)/ug, '\it\nabla\rm / \mu\itg'); legend('\it\nabla_x','\it\nabla_y','\it\nabla_z')