www.pudn.com > SINS-MatlabImplement.rar > test_SINS_GPS.m, change:2009-02-05,size:2452b


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('\itt\rm / s'); ylabel('\it\phi \rm / arcmin'); grid on 
subplot(2,3,2),hold on,plot(Xkk(:,4:6)),  
xlabel('\itt\rm / s'); ylabel('\it\delta V \rm / m/s'); grid on 
subplot(2,3,3),hold on,plot([Xkk(:,7)*glv.Re,Xkk(:,8)*glv.Re,Xkk(:,9)]),  
xlabel('\itt\rm / s'); ylabel('\it\delta P \rm / m'); grid on 
subplot(2,3,4),plot(Xkk(:,10:12)/glv.dph),  
xlabel('\itt\rm / s'); ylabel('\it\epsilon \rm / \circ/h'); grid on 
subplot(2,3,5),plot(Xkk(:,13:15)/glv.ug),  
xlabel('\itt\rm / s'); ylabel('\it\nabla \rm / ug'); grid on