test.m

EKF的效果需要有test来测验。test中给定了初值来验证EKF的滤波效果

% 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;