clear
close
glvs;
ws2 = glv.g0/glv.Re;    % 修拉周期平方
xi = sqrt(2)/2; Td = 20;  % 水平調平參數k1,k2,k3
sigma = 2*pi*xi/(Td*sqrt(1-xi^2));
k1 = 3*sigma; k2 = sigma^2*(2+1/xi^2)/ws2-1; k3 = sigma^3/(glv.g0*xi^2);
xi = sqrt(2)/2; Td = 50;  % 羅經回路控制參數kz1,kz2,kz3
sigma = 2*pi*xi/(Td*sqrt(1-xi^2));
kz1 = 2*sigma; kz4 = kz1; kz2 = 4*sigma^2/ws2-1; kz3 = 4*sigma^4/glv.g0;
VE = 0; VN = 0; PE = 0; PN = 0;  wnc = zeros(3,1); % 狀態初始值
L = 34*glv.deg; pos = [L; 0; 0];  % 初始位置
[wnie, wnen, RMh, RNh, gn] = earth(pos, [0;0;0]);
eb = [.01; .01; -.01]*glv.dph;  web =  [0.001; 0.001; 0.001]*glv.dph; % 陀螺漂移
db = [100; -100; 100]*glv.ug; wdb =  [10; 10; 10]*glv.ug; % 加速度計零偏
att = [0; 0; 30]*glv.deg; qnb0 = a2qnb(att); % 姿態真值
phi = [-10.00; 10.00; 60.0]*glv.min;
qnb = qaddphi(qnb0, phi);  % 加入姿態誤差的計算四元數初值
Ts = .1;   % 仿真步長
t = 300;  % 總時間長度
len = fix(t/Ts); phik=zeros(len,3);
for k=1:len
    wbib = qmulv(qconj(qnb0),wnie)+eb+web.*randn(3,1);  % 陀螺采樣角速度(假設載體靜止)
    fbsf = qmulv(qconj(qnb0),-gn)+db+wdb.*randn(3,1);  % 加速度計采樣比力

    qnb = qmul( qnb, rv2q((wbib-qmulv(qconj(qnb),wnie+wnc))*Ts) );  % 帶反饋控制項wnc的姿態更新
    fn = qmulv(qnb, fbsf);  % 比力變換
    phik(k,:) = qq2phi(qnb, qnb0)';
    
    if k<(50/Ts)     % 前50s水平調平
        VE = VE + (fn(1)-k1*VE)*Ts;
        PE = PE + VE*k3*Ts;
        wnc(2,1) = VE*(1+k2)/glv.Re + PE;
        VN = VN + (fn(2)-k1*VN)*Ts;
        PN = PN + VN*k3*Ts;
        wnc(1,1) = -VN*(1+k2)/glv.Re - PN;
    else        % 羅經對準
        VE = VE + (fn(1)-k1*VE)*Ts;
        PE = PE + VE*k3*Ts;
        wnc(2,1) = VE*(1+k2)/glv.Re + PE;
        VN = VN + (fn(2)-k1*VN)*Ts;
        PN = PN + VN*k3*Ts;
        VN = VN + (fn(2)-kz1*VN)*Ts;
        wnc(1,1) = -VN*(1+kz2)/glv.Re;
        wnc(3,1) = (VN*kz3*Ts/wnie(2)+wnc(3,1))/(1+kz4*Ts);
    end
end
time = [1:length(phik)]*Ts;
figure
subplot(3,1,1), plot(time,phik(:,1)/glv.min), ylabel('\it\phi_E\rm / arcmin'); grid on
subplot(3,1,2), plot(time,phik(:,2)/glv.min), ylabel('\it\phi_N\rm / arcmin'); grid on
subplot(3,1,3), plot(time,phik(:,3)/glv.min), ylabel('\it\phi_U\rm / arcmin'); grid on
xlabel('\itt \rm / s');