clear
close
glvs
Ts = 0.1;
att = [0; 0; 30]*glv.deg;   qnb0 = a2qnb(att);   % 姿態真值
phi = [10; 10; 60]*glv.min;  qnb = qaddphi(qnb0, phi); % 加入姿態誤差
Lat = 34*glv.deg;
wnie = glv.wie*[0; cos(Lat); sin(Lat)];
wbib = qmulv(qconj(qnb0),wnie);
gn = [0; 0; -glv.g0];
fb = qmulv(qconj(qnb0),-gn);

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; % 加速度計誤差
% 濾波器參數,參見《捷聯慣導系統靜基座初始對準精度分析及仿真》
Qt = diag([web; 0;0])^2;
Rk = diag([wdb(1);wdb(2)])^2;
xk = zeros(5,1);
Pk = diag([[1;1;10]*glv.deg; [1;1]*glv.dph])^2;
Phi = [ 0 wnie(3) -wnie(2)   0 0 
      -wnie(3) 0 0    -1 0 
      wnie(2) 0 0     0 -1 
      zeros(2,5) ];
Phikk_1 = eye(5)+Phi*Ts;
Hk = [ 0 -glv.g0  0 0 0
      glv.g0 0   0 0 0 ];
 
t = 300;  % 總時間長度
len = fix(t/Ts); res=zeros(len,3);
feedback=0.05;  % 反饋系數
s = 1.001; % 遺忘因子

for k=1:1:len
    wbe = wbib+eb+web.*randn(3,1);
    fbe = fb+db+wdb.*randn(3,1);
    qnb = qmul(qnb, rv2q( (wbe-qmulv(qconj(qnb),wnie))*Ts ));  % 姿態更新
    fn = qmulv(qnb, fbe);  
    zk = fn(1:2);  % 以水平比力為觀測量
    [xk, Pk, Kk] = kalman(Phikk_1, Qt*Ts, xk, Pk, Hk, Rk, zk);
%     if k*Ts>50 
%         feedback=0.01;
%     else
%         feedback=0;
%     end
%     qnb = qdelphi(qnb, feedback*xk(1:3,1));  xk(1:3,1) = (1-feedback)*xk(1:3,1); % 反饋
%     Pk = Pk*s;  % 遺忘濾波
    res(k,:) = [xk(1:3)-qq2phi(qnb,qnb0)]';  % 濾波估計值 - 計算平臺誤差
end
time = [1:length(res)]*Ts;
figure;
subplot(3,1,1); plot(time,res(:,1)/glv.min); ylabel('\it\delta\phi_E\rm / arcmin'); grid on
subplot(3,1,2); plot(time,res(:,2)/glv.min); ylabel('\it\delta\phi_N\rm / arcmin'); grid on
subplot(3,1,3); plot(time,res(:,3)/glv.min); ylabel('\it\delta\phi_U\rm / arcmin'); grid on
xlabel('\itt \rm / s');