交通/航空行业

开发平台：

Matlab

test_SINS_GPS.m：源码内容

							clear
glvs
ts=0.1;        %采样周期
att0=[0;0;0]*glv.deg; vn0=[0;0;0]; pos0=[30*glv.deg;0;0];   %初始值设置
qnb0 = a2qnb(att0);
vn = vn0; pos = pos0;
[wnie, wnen, RMh, RNh, gn] = earth(pos, vn0);
wbib = qmulv(qconj(qnb0),wnie);
fb = qmulv(qconj(qnb0),-gn);
phi=[5;5;30]*glv.min;, dvn=[0;0;0]; dpos=[1.0/glv.Re; 1.0/glv.Re; 1.0];  %加入误差
qnb = qaddphi(a2qnb(att0),phi); vn = vn0+dvn; pos = pos0+dpos;
eb = [0.01;0.01;0.01]*glv.dph; web = [0.01;0.01;0.01]*glv.dph; % IMU 误差
db = [100;100;100]*glv.ug; wdb = [10;10;10]*glv.ug;
% 滤波器设置
Qt = diag([web; wdb; [0.01/glv.Re;0.01/glv.Re;0.01]; zeros(6,1)])^2;
Rk = diag([1.0/glv.Re; 1.0/glv.Re; 1.0])^2;
Pk = diag([[1;1;10]*glv.deg; [10;10;10]; [100/glv.Re;100/glv.Re;100]; ...
        [1;1;1]*glv.dph; [1;1;1]*glv.mg])^2;
Xk = zeros(15,1);
Hk = [zeros(3,6),eye(3),zeros(3,6)];
t = 600;  % 总时间长度
len = t/ts; errphi=zeros(t,3); errvn=errphi; errpos=errphi; Xkk=zeros(t,15);
kk = 1;
for k=1:len
    wm = (wbib+eb+web.*randn(3,1))*ts;
    vm = (fb+db+wdb.*randn(3,1))*ts;
    [qnb,vn,pos]=sins(qnb,vn,pos,wm,vm,ts);
    Ft = getf(qnb, vn, pos, vm./ts); 
    [Fikk_1, Qk] = kfdis(Ft, Qt, ts, 2); %离散化
    if mod(k,10)==0
        posGPS = pos0 + dpos.*randn(3,1); % 构造GPS位置
        Zk = pos - posGPS; % 位置误差观测量
        [Xk, Pk, Kk] = kalman(Fikk_1, Qk, Xk, Pk, Hk, Rk, Zk);   
        errphi(kk,:) = qq2phi(qnb,qnb0)'; %求姿态误差
        errvn(kk,:) = (vn-vn0)';
        errpos(kk,:) = (pos-pos0)';
        Xkk(kk,:) = Xk';
        kk = kk+1;
        if mod(kk,100)==0    %进度显示
            disp(kk)
        end
    else
        [Xk, Pk] = kalman(Fikk_1, Qk, Xk, Pk);  %无观测时只进行预测      
    end
end
figure,
subplot(2,3,1),plot(errphi/glv.min,'m'), 
subplot(2,3,2),plot(errvn,'m'), 
subplot(2,3,3),plot([errpos(:,1:2)*glv.Re,errpos(:,3)],'m');
subplot(2,3,1),hold on,plot(Xkk(:,1:3)/glv.min), 
xlabel('ittrm / s'); ylabel('itphi rm / arcmin'); grid on
subplot(2,3,2),hold on,plot(Xkk(:,4:6)), 
xlabel('ittrm / s'); ylabel('itdelta V rm / m/s'); grid on
subplot(2,3,3),hold on,plot([Xkk(:,7)*glv.Re,Xkk(:,8)*glv.Re,Xkk(:,9)]), 
xlabel('ittrm / s'); ylabel('itdelta P rm / m'); grid on
subplot(2,3,4),plot(Xkk(:,10:12)/glv.dph), 
xlabel('ittrm / s'); ylabel('itepsilon rm / circ/h'); grid on
subplot(2,3,5),plot(Xkk(:,13:15)/glv.ug), 
xlabel('ittrm / s'); ylabel('itnabla rm / ug'); grid on

						