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');