% test the effect of EKF.
clear
clc
t = 3600;  % 1小時 == 3600秒
deta = 9;	 % 時間間隔 ????
y0 = [6878.137000 0 0 0 5.382926862 5.382926862];  % y0: 初始軌道位置和速度
% 5,5,5是初始位置誤差; 0.005,0.005,0.005是初始速度誤差
deta0 = [5 5 5 0.005 0.005 0.005];
y = y0 + deta0;
wk = diag(deta0);
P = wk^2;  % P: 初始方差陣 & ^: 矩陣的乘方符號

%================ 核心代碼程序 ================
for k = 0:deta:t  % deta = 9
    if k==0;
        x_EKF(1, :) = y;  % y = y0 + deta0; & x_EKF: ?????
    else                % 什么意思 ????
        Rs = sun_ECI(k);   % 原為k-deta
        Rm = moon_ECI(k);  % 原為k-deta
        Z = sen(k);		% Sen函數: function Z = sen(t)

        % ???? ode常微分方程相關
        options = odeset('RelTol', 1e-9, 'AbsTol', 1e-6);  % RelTol: 相對精度; AbsTol: 絕對精度
        [tt, M_y] = ode45('orbitstate', [k-deta, k], y, options);  % [t, y] = solver(odefun, tspan, y0, options)
        yubao_y = M_y(end, :);	% 狀態方程的預報值 & end 為取最后一行

        [ym, Pm] = EKF(P, y, Rm, Rs, Z, yubao_y, deta);  % EKF()函數
        x_EKF(k/deta+1, :) = ym;	% x_EKF的賦值 !!!!
        P = Pm;  % ym為函數EKF求得的濾波誤差方差陣
        y = ym;  % Pm為函數EKF求得的最優濾波值
    end  % end of "else".
end  % end of "for".

% ==================== 檢驗濾波效果 ====================
rr = [x_EKF(:, 1:3)];	% x_EKF: 濾波后的位置和速度
vv = [x_EKF(:, 4:6)];
for i = 1:t/deta+1  % t/deta + 1: i的上限
    y_r = orbitinECI(deta*(i-1));	% y_r: 赤道慣性坐標系下衛星的位置和速度
    rr_r = y_r(1:3);		% 赤道慣性坐標系下衛星的位置????
    vv_r = y_r(4:6);		% 赤道慣性坐標系下衛星的速度????
    standard_r = norm(rr_r);	% 標稱星地距離；norm: 向量和矩陣的范數
    standard_v = norm(vv_r);	% 標稱速度大小
    filted_r = norm(rr(i, :));	% rr: 濾波后位置
    filted_v = norm(vv(i, :));	% vv: 濾波后速度
    m = filted_r - standard_r;
    n = filted_v - standard_v;
    detarv(i, :) = [m n*1000];	% detarv: ???? & n為什么要乘以1000 ????
    xx = rr(i, 1) - y_r(1);
    yy = rr(i, 2) - y_r(2);
    zz = rr(i,3) - y_r(3);
    detaR(i, :) = [xx yy zz];	% detaR: ????
    vxx = vv(i, 1) - y_r(4);
    vyy = vv(i, 2) - y_r(5);
    vzz = vv(i, 3) - y_r(6);
    detaV(i, :) = [vxx*1000 vyy*1000 vzz*1000];	% 為什么各分量要乘以1000 ????
    % n*1000: 速度從km/s到m/s的轉換, 應乘3600 ????
end

%================ 畫圖顯示結果 ================
clf			% 清除圖形窗口
figure(1)		% 指定1號圖形窗
subplot(1, 2, 1);	% 指定1號子圖
plot(0:deta:t, detarv(:,1), 'g');
xlabel('t(s)');		% 軸名
ylabel('deta r (km)');
grid on;		% 畫坐標分格線
subplot(1, 2, 2);	% 指定2號子圖
plot(0:deta:t, detarv(:,2), 'r');
xlabel('t(s)');
ylabel('deta v (m/s)');
grid on;
figure(2)		% 指定2號圖形窗
subplot(1, 2, 1);
plot(0:deta:t, detaR(:,1), 'r', 0:deta:t, detaR(:,2), 'g', 0:deta:t, detaR(:,3), 'b');
xlabel('t(s)');
ylabel('deta RxRyRz (km)');
grid on;
subplot(1, 2, 2);
plot(0:deta:t, detaV(:,1), 'r', 0:deta:t, detaV(:,2), 'g', 0:deta:t, detaV(:,3), 'b');
xlabel('time(s)');
ylabel('deta VxVyVz (km) ');
grid on;