Расширенный фильтр Калмана 3D, только подшипники Баллисти c Проблемы с траекторией

Question

Я пытаюсь внедрить базовый расширенный фильтр Калмана c для отслеживания только подшипников простой баллистической ракеты c и спутника на круговой наклонной орбите. Три файла с эфемеридами прилагаются ниже и представляют собой простой экспорт из STK с вставкой спутника по умолчанию на 500 км и объекта ракеты по умолчанию и получением эфемерид с шагом 1 секунда.

Код для запуска в Matlab приведен ниже но некоторые ключевые допущения для фильтра

1. Предположим, что модель почти постоянной скорости -> Используется постоянное ускорение белого гауссовского шума для шума процесса
2. Датчик имеет стандартное отклонение точности на 1 градус в Az и El
3. Вектор состояния x = [xyz x_dot y_dot z_dot] '
4. Информация о датчике находится в системе отсчета LVLH, поэтому матрица вращения рассчитывается и реализуется для преобразования оценки состояния в значения измерения

Ниже приведен код Matlab. Я даже установил инициализированное состояние на точное начальное значение и пробовал различные значения для ковариационных и шумовых матриц, но не могу получить каких-либо разумных результатов.

function [] = test_EKF()
clear all;close all;clc

%% import data from stk
tgt_eci = readtable('target_eci.csv');
tgt_eci = tgt_eci{1:end-1,:};

sat_eci = readtable('sat_eci.csv');
sat_eci = sat_eci{1:size(tgt_eci,1),:};

lvlh = readtable('sat2tgt_lvlh.csv');
lvlh = lvlh{1:size(tgt_eci,1),:};

dt = lvlh(2,1) - lvlh(1,1);

%% Create Filter 
[EKF] = Create_EKF('Constant_Velocity',3,dt,deg2rad(1)^2,1,1,2);

EKF.x = tgt_eci(1,2:end)';  %For testing initialize to the perfect state

estimated_states = [];

for i = 1:size(lvlh,1)-1

  %Create "sensor info"
  [az, el] = cart2sph(lvlh(i+1,5), lvlh(i+1,6), lvlh(i+1,7)); 

  % Induce measurement error to simulate real reading (Normal gausian(0,1 deg))
  az = az + normrnd(0,(deg2rad(1)));
  el = el + normrnd(0,(deg2rad(1)));

  [EKF] = ekf_propogate(EKF);

  [EKF] = ekf_update(EKF, [az el], sat_eci(i+1,2:end));

  estimated_states = [estimated_states; EKF.x']; %Store estimated states for plotting


end

plot(tgt_eci(2:end,2:4),'DisplayName','tgt_eci(:,2:4)')
hold all
plot(estimated_states(:,1:3),'DisplayName','measured_state(:,1:3)');
legend;

end

function [HJacob, y, T] = creat_az_el(d, x, o)
%
% Input:
%  d : integer   : Euclidean space dimensions
%  x : real[m,1] : Dynamics state
%  o : real[6,1] : Observer state (x y z vz vy vz]

x = reshape(x,[],1);
o = reshape(o,[],1);

m = size(x, 1);

% Build transformation matrix for ECI -> LVLH
% +X -> point opposit satelite position vecotor (Away from NADIR)
% +Z -> along orbit normal (Orbit angular momentum vector) (R cross V)
% +Y -> Z cross X to complete right handed system (same as velocity vector)

hA = cross(o(1:3),o(4:6));                      % Angular momentum of satellite orbit (R cross V)
Transform_i = o(1:3) ./ norm(o(1:3));           %X component
Transform_k = hA ./ norm(hA);                   %Z component
Transform_j = cross(Transform_k,Transform_i);   %Y component

T = [Transform_i'; Transform_j'; Transform_k'];

xo = T * (x(1:3) - o(1:3));

[az, el] = cart2sph(xo(1), xo(2), xo(3));

r   = hypot(hypot(xo(1), xo(2)), xo(3));
rxy = hypot(xo(1), xo(2));
Hxo = [ ...
    -xo(2)/rxy^2,         +xo(1)/rxy^2,               0;
    -xo(1)*xo(3)/rxy/r^2, -xo(2)*xo(3)/rxy/r^2, rxy/r^2];

HJacob = Hxo * T * eye(3, m);

y = [az; el];

end

function [EKF] = ekf_update(EKF, y, obs_state)

y = reshape(y,[],1); % "Simulated Sensor Input"

obs_state = reshape(obs_state,[],1);

[EKF.Hj, y_predict, ~] = creat_az_el(EKF.dim, EKF.x, obs_state); %Perform measurement prediction

Inn = y - y_predict;                    %Innovation (measuremment residual)

S = EKF.Hj * EKF.P * EKF.Hj' + EKF.R;   %Residuaul covariance

K = EKF.P * EKF.Hj' / S;                %Near Optimal Kalman gain

EKF.x = EKF.x + K * Inn;                %Updated State Estimate

EKF.P = EKF.P - K * EKF.Hj * EKF.P;     %Updated covariance estimate

end

function [EKF] = ekf_propogate(EKF)

EKF.x = EKF.F * EKF.x; %Predict (a priori) state estimate

EKF.P = EKF.F * EKF.P * EKF.F' + EKF.Q; 

end

function [EKF] = Create_EKF(DynModel,dim,dt,q,P_0,R,num_measures)

EKF.dim = dim;
EKF.DynModel = DynModel;

switch DynModel
    case 'Constant_Velocity'
        [EKF.F, EKF.Q] = nearly_constant_velocity(dim, dt, q);

    case 'Constant_Acceleration'
        [EKF.F, EKF.Q] = nearly_constant_acceleration(dim, dt, q);

end

%Initialize other parameters
EKF.x = zeros( size(EKF.F,2) ,1);    % Initial State

EKF.P = eye( length(EKF.F) ).* P_0;  % Uncertainty covariance

EKF.R = eye( num_measures ).* R;    % State Uncertainty

[EKF.Hj, ~, ~] = creat_az_el(dim, EKF.x, zeros(6,1)); %Initialize H jacobian

end

function [F, Q] = nearly_constant_acceleration(d, t, q)
% https://github.com/rlabbe/filterpy
F = constant_acceleration(d, t);

%Model for noise is based on continuous white noise method(different then
%discrete and require integration for calcuation)
% https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb
Q = kron([ ...
    t^5/20, t^4/8, t^3/6;
    t^4/8,  t^3/3, t^2/2;
    t^3/6,  t^2/2, t], eye(d)) * q;

end

function [F, Q] = nearly_constant_velocity(d, t, q)
% https://github.com/rlabbe/filterpy
F = constant_velocity(d, t);

%Model for noise is based on continuous white noise method(different then
%discrete and require integration for calcuation)

% https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb
Q = kron([ ...
    t^3/3, t^2/2;
    t^2/2, t], eye(d)) * q;
end

function [F] = constant_acceleration(d, t)
F = kron([ ...
    1, t, t^2/2;
    0, 1, t;
    0, 0, 1], eye(d));
end

function [F] = constant_velocity(d, t)
F = kron([ ...
    1, t;
    0, 1], eye(d));
end

Спутник для определения геометрии цели в LVLH (спутник ) frame

Координаты спутниковой информации ECI

Координация ракетной информации ECI