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


clear 
% close 
glvs 
ts = 0.1;        %采样周期 
n = 3;    % 子样数 
att0=[0;0;0]*glv.deg; vn0=[0;0;0]; pos0=[0*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); 
wm=wbib*ts; vm=fb*ts;   %静止时角增量和比力增量 
eb = [0.01; 0.01; 0.01]*glv.dph; web =  [0.001; 0.001; 0.001]*glv.dph; % 陀螺误差 
db =  [100; 100; 100]*glv.ug;  wdb =  [10; 10; 10]*glv.ug; % 加速度计误差 
phi = [0.5;0.5;3]*glv.min; qnb = qaddphi(qnb0,phi); % 加入姿态误差 
kk = 1; 
t = 3600;  % 总时间长度 
len = fix(t/ts/n); errphi=zeros(len,3); errvn=errphi; errpos=errphi; 
for k=0:n:len*n 
    for k1=1:n 
        wme(:,k1) = wm+(eb+web.*randn(3,1))*ts;  % 构造多子样数据 
        vme(:,k1) = vm+(db+wdb.*randn(3,1))*ts; 
    end 
    [qnb,vn,pos]=sins(qnb,vn,pos,wme,vme,ts);  
    vn(3) = 0;  % 限定高度发散 
    errphi(kk,:) = qq2phi(qnb,qnb0)'; %求姿态误差 
    errvn(kk,:) = (vn-vn0)'; 
    errpos(kk,:) = (pos-pos0)'; 
    kk = kk+1; 
end 
time = [1:length(errphi)]*ts*n; 
figure, 
subplot(3,1,1),plot(time,errphi/glv.min), ylabel('\it\phi\rm / arcmin'); grid on 
subplot(3,1,2),plot(time,errvn), ylabel('\it\delta V\rm / m/s');  grid on 
subplot(3,1,3),plot(time,[errpos(:,1:2)*glv.Re,errpos(:,3)]), ylabel('\it\delta P\rm / m'); grid on 
xlabel('\itt\rm / s');