diff --git a/Analisys_With_Features.m b/Analisys_With_Features.m
new file mode 100644
index 0000000000000000000000000000000000000000..b62aae33bd596a16a68f63adc197eb9c8f23e93e
--- /dev/null
+++ b/Analisys_With_Features.m
@@ -0,0 +1,78 @@
+clear
+close all
+clc
+
+%% Setup
+
+ warning('off', 'all'); format longG;
+ set(0,'DefaultTextInterpreter','latex');
+ set(0,'DefaultAxesFontSize', 16);
+
+% Mac
+ restoredefaultpath
+ addpath('../useful_functions/');
+ addpath('../dynamical_model/');
+ addpath(genpath('../mice/'));
+ addpath(genpath('../computer-vision/'));
+ addpath(genpath('../paper/'));
+ addpath(genpath('../generic_kernels/'));
+ addpath(genpath('../paper/MMX_Fcn_CovarianceAnalyses/'));
+ addpath(genpath('../paper/MMX_Product/MMX_BSP_Files_GravLib/'));
+ MMX_InitializeSPICE
+ cspice_furnsh(which('mar097.bsp'));
+ cspice_furnsh(which('MARPHOFRZ.txt'));
+ cspice_furnsh(which('MMX_QSO_049_2x2_826891269_828619269.bsp'));
+ cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
+
+
+%% Load observations and path for syntethic pictures
+
+ load("YObs.mat");
+
+
+%% Initial conditions for the analysis
+
+% Model parameters
+ [par, units] = MMX_DefineNewModelParametersAndUnits;
+
+% Time of the analysis
+ data = '2026-03-16 00:00:00 (UTC)';
+ data = cspice_str2et(data);
+ day = 86400;
+ par.et0 = data;
+ [Ph,par] = Phobos_States_NewModel(data,par);
+
+% Covariance analysis parameters
+ [par, units] = CovarianceAnalysisParameters(par, units);
+ par.sigma = 1e-10/(units.vsf*units.tsf);
+ par.sigmaPh = 0/(units.vsf*units.tsf);
+
+% Initial Phobos's state vector
+ Ph0 = Ph./units.sfVec2;
+
+% Initial MMX's State vector
+ MMX0 = cspice_spkezr('-34', data, 'MarsIAU', 'none', '499');
+ MMX0 = MMX0./units.sfVec;
+
+% Analysis initial state vector
+ St0 = [MMX0; Ph0; par.I2; par.bias];
+
+ Est0.X = St0;
+ Est0.dx = zeros(size(St0,1),1);
+ Est0.P0 = par.P0;
+ Est0.t0 = data*units.tsf;
+
+%% Analysis
+
+ par.alpha = 1;
+ par.beta = 2;
+ file_features = 'Principal_Phobos_Ogni10.mat';
+
+ [Est] = UKF_Tool(Est0,@Dynamics_MPHandMMX_Inertia,...
+ @Cov_Dynamics_Good, @Observables_model_with_Features,...
+ par.R,YObs,par,units,file_features);
+
+
+ Results(Est, YObs, par, units)
+
+
diff --git a/CovarianceAnalysisParameters.m b/CovarianceAnalysisParameters.m
index 3c31b036b214aabfa784a99a48866a18121ad01c..0e1d8dfff9bd97253f09e1275690d230974ff529 100644
--- a/CovarianceAnalysisParameters.m
+++ b/CovarianceAnalysisParameters.m
@@ -1,7 +1,7 @@
function [pars, units] = CovarianceAnalysisParameters(pars, units)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
-% [pars, units] = New_MMX_CovarianceAnalysisParameters(pars, units)
+% [pars, units] = CovarianceAnalysisParameters(pars, units)
%
% This function adds some units and parameters useful to the covariance
% analysis
@@ -16,6 +16,9 @@ function [pars, units] = CovarianceAnalysisParameters(pars, units)
% pars.ObsNoise.range_rate Standard deviation of the relative velocity measure
% pars.ObsNoise.lidar Standard deviation of the lidar measure
% pars.ObsNoise.camera Standard deviation of the camera measure
+% pars.ObsNoise.pixel Standard deviation of the pixel error
+% pars.camera_intrinsic Camera intrinsic matrix
+% pars.FOV Camera Filed Of View
% pars.interval Seconds between observations
% pars.Clm Vector with all the Clm coefficients
% pars.Slm Vector with all the Slm coefficients
@@ -99,7 +102,7 @@ units.CovDim = (repmat(units.Dim, 1, length(units.Dim)).*...
% A-priori covariance
pars.P0 = diag(([.3*ones(3,1); .3e-3*ones(3,1);...
- 1e-1; 1e-4; 1e-5; 1e-7; 3e-1; 1e-3; 3e-1; 1e-3; 1e-2*ones(pars.nCoeff,1);...
+ 1e-1; 1e-4; 1e-5; 1e-7; 1; 1e-2; 1e-1; 1e-3; 1e-2*ones(pars.nCoeff,1);...
1; 1e-3; 1]./units.Dim).^2);
end
\ No newline at end of file
diff --git a/Measurement_read.m b/Measurement_read.m
new file mode 100644
index 0000000000000000000000000000000000000000..89729c3d88144cf8868e5b76362eae32e22fc3a2
--- /dev/null
+++ b/Measurement_read.m
@@ -0,0 +1,68 @@
+function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID] = Measurement_read(Measures,O,units)
+
+ Y_obs = [];
+ R = [];
+ Stat_ID_Range = [];
+ Stat_ID_Rate = [];
+ Features_ID = [];
+
+ if ~isempty(Measures.Range)
+ which = ~isnan(Measures.Range);
+ count = sum(~isnan(Measures.Range));
+
+ if ~isnan(Measures.Range(1))
+ Stat_ID_Range = [Stat_ID_Range; 1];
+ end
+
+ if ~isnan(Measures.Range(2))
+ Stat_ID_Range = [Stat_ID_Range; 2];
+ end
+
+ if ~isnan(Measures.Range(3))
+ Stat_ID_Range = [Stat_ID_Range; 3];
+ end
+
+ Ranges = Measures.Range(which);
+ Y_obs = Ranges./units.lsf;
+ R = ones(count,1)*O(1,1);
+
+ end
+
+ if ~isempty(Measures.Rate)
+ which = ~isnan(Measures.Rate);
+ count = sum(~isnan(Measures.Rate));
+
+ if ~isnan(Measures.Rate(1))
+ Stat_ID_Rate = [Stat_ID_Rate; 1];
+ end
+
+ if ~isnan(Measures.Rate(2))
+ Stat_ID_Rate = [Stat_ID_Rate; 2];
+ end
+
+ if ~isnan(Measures.Rate(3))
+ Stat_ID_Rate = [Stat_ID_Rate; 3];
+ end
+
+ Rates = Measures.Rate(which);
+ Y_obs = [Y_obs; Rates./units.vsf];
+ R = [R; ones(count,1)*O(2,2)];
+ end
+
+ if ~isempty(Measures.Lidar)
+ check_Lidar = 1;
+ Lidar = Measures.Lidar;
+ Y_obs = [Y_obs; Lidar./units.lsf];
+ R = [R; O(3,3)];
+ end
+
+ Yobs.meas = Y_obs;
+
+ if ~isempty(Measures.Features)
+ Features_ID = Measures.Features(end,:);
+ Yobs.cam = Measures.Features;
+ else
+ Yobs.cam = [];
+ end
+
+end
\ No newline at end of file
diff --git a/Observables_model_with_Features.m b/Observables_model_with_Features.m
new file mode 100644
index 0000000000000000000000000000000000000000..0443873024ea9a28819cef1ec75ff45cfa236fbc
--- /dev/null
+++ b/Observables_model_with_Features.m
@@ -0,0 +1,189 @@
+function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rate,...
+ Lidar_check, Feature_ID, pars, units, O, file_features)
+%==========================================================================
+% [G, R] = Observables_model_with_Features(et,Obs,X,St_ID_Range,St_ID_Rate, Feature_ID, par,units)
+%
+% Compute the Range, Range Rate, Lidar's and camera's measures
+%
+% INPUT: Description Units
+%
+% et - Time Epoch s
+%
+% stationID - Available Observables at the epoch t
+%
+% X - State Vector defined in the Phobos rotating frame
+% at the time being considered (42x1)
+% .MMX Position Vector (3x1) km
+% .MMX Velocity Vector (3x1) km/s
+% .Phobos Position Vector (3x1) km
+% .Phobos Velocity Vector (3x1) km/s
+% .Clm harmonics coefficients
+% .Slm Hamronics coefficients
+% .State Transition Matrix
+%
+% par - Parameters structure
+%
+%
+% OUTPUT:
+%
+% G - G
+%
+% DO NOT FORGET TO USE RESHAPE LATER IN THE PROJECT
+%
+% Coupling:
+% None
+%
+% Author: Edoardo Ciccarelli
+% Date: 06/2022
+%
+%==========================================================================
+
+% Dimensions of the problem
+ n = pars.d/2;
+ d = pars.d;
+
+% Output initialization
+ range_Camb = [];
+ range_rate_Camb = [];
+ range_Gold = [];
+ range_rate_Gold = [];
+ range_Mad = [];
+ range_rate_Mad = [];
+ lidar = [];
+
+ Rm = [];
+
+% Unpacking of the state vector
+ X_MMX = X(1:d);
+ r_MMX = X_MMX(1:3);
+ v_MMX = X_MMX(4:6);
+ r_Phobos = pars.perifocal2MARSIAU*X(d+1)*[cos(X(d+3)); sin(X(d+3)); 0];
+
+ switch size(X,1)
+ case 17
+ bias = pars.bias;
+ case 18
+ bias = X(16:end);
+ case 20
+ bias = X(18:end);
+ end
+
+% Definition of Mars position WRT Earth J2000
+ [Mars, ~] = cspice_spkezr('499',et,'MARSIAU','none','399');
+ Mars = Mars./units.sfVec;
+
+
+% Which station is working
+% Camberra's Position in the J2000 Earth-centered
+ [Camb, ~] = cspice_spkezr('DSS-45', et, 'MARSIAU', 'none', 'EARTH');
+ Camb = Camb./units.sfVec;
+% Goldstone's Position in the J2000 Earth-centered
+ [Gold, ~] = cspice_spkezr('DSS-24', et, 'MARSIAU', 'none', 'EARTH');
+ Gold = Gold./units.sfVec;
+% Madrid's Position in the J2000 Earth-centered
+ [Mad, ~] = cspice_spkezr('DSS-65', et, 'MARSIAU', 'none', 'EARTH');
+ Mad = Mad./units.sfVec;
+
+ if sum((St_ID_Range == 1)==1)
+% Camberra antennas available
+ r = norm(Mars(1:n) + r_MMX - Camb(1:n));
+ range_Camb = r + bias(1);
+ Rm = [Rm; O(1,1)];
+ end
+
+ if sum((St_ID_Rate == 1)==1)
+% Camberra antennas available
+ R = norm(Mars(1:n) + r_MMX - Camb(1:n));
+ r_dot = 1/R*sum((Mars(n+1:d) + v_MMX - Camb(n+1:d)).*(Mars(1:n) + r_MMX - Camb(1:n)));
+ range_rate_Camb = r_dot + bias(2);
+ Rm = [Rm; O(2,2)];
+ end
+
+ if sum((St_ID_Range == 2)==1)
+% Goldstone antennas available
+ r = norm(Mars(1:n) + r_MMX - Gold(1:n));
+ range_Gold = r + bias(1);
+ Rm = [Rm; O(1,1)];
+ end
+
+ if sum((St_ID_Rate == 2)==1)
+% Goldstone antennas available
+ R = norm(Mars(1:n) + r_MMX - Gold(1:n));
+ r_dot = 1/R*sum((Mars(n+1:d) + v_MMX - Gold(n+1:d)).*(Mars(1:n) + r_MMX - Gold(1:n)));
+ range_rate_Gold = r_dot + bias(2);
+ Rm = [Rm; O(2,2)];
+ end
+
+ if sum((St_ID_Range == 3)==1)
+% Madrid antennas available
+ r = norm(Mars(1:n) + r_MMX - Mad(1:n));
+ range_Mad = r + bias(1);
+ Rm = [Rm; O(1,1)];
+ end
+
+ if sum((St_ID_Rate == 3)==1)
+% Camberra antennas available
+ R = norm(Mars(1:n) + r_MMX - Mad(1:n));
+ r_dot = 1/R*sum((Mars(n+1:d) + v_MMX - Mad(n+1:d)).*(Mars(1:n) + r_MMX - Mad(1:n)));
+ range_rate_Mad = r_dot + bias(2);
+ Rm = [Rm; O(2,2)];
+ end
+
+ if Lidar_check
+
+% Lidar rispetto alla superficie
+ I = r_Phobos/X(7);
+ k = pars.perifocal2MARSIAU*[0; 0; 1];
+ j = cross(k,I);
+ NO = [I,j,k];
+ ON = NO';
+
+ Xsi = X(13);
+ BO = [cos(Xsi), sin(Xsi), 0; -sin(Xsi), cos(Xsi), 0; 0, 0, 1];
+
+ rsb = r_MMX-r_Phobos;
+
+% Posizione di MMX nel Phobos Body-fixed reference frame
+ r_bf = BO*ON*rsb;
+
+ lat = asin(r_bf(3)/norm(r_bf));
+ lon = atan2(r_bf(2), r_bf(1));
+
+% Phobos radius as function of latitude and longitude
+ alpha = pars.Phobos.alpha;
+ beta = pars.Phobos.beta;
+ gamma = pars.Phobos.gamma;
+
+ R_latlon = (alpha*beta*gamma)/sqrt(beta^2*gamma^2*cos(lon)^2 +...
+ gamma^2*alpha^2*sin(lon)^2*cos(lat)^2 + alpha^2*beta^2*sin(lon)^2*sin(lat));
+
+ lidar = norm(rsb) - R_latlon/units.lsf + bias(3);
+
+ Rm = [Rm; O(3,3)];
+
+ end
+
+ if ~isempty(Feature_ID)
+
+% There should be features
+ [Sun, ~] = cspice_spkezr('10',et,'MARSIAU','none','499');
+
+ R2Rot = [cos(X(9)), sin(X(9)), 0; -sin(X(9)), cos(X(9)), 0; 0, 0, 1];
+ v_Phobos = pars.perifocal2MARSIAU*R2Rot'*([X(8); 0; 0] + ...
+ cross([0; 0; X(10)],[X(7)*cos(X(9)); X(7)*sin(X(9)); 0]));
+ Phobos = [r_Phobos.*units.lsf; v_Phobos.*units.vsf];
+
+ [~, Y_pix] = visible_features(X_MMX.*units.sfVec, Phobos, Sun, Xsi, file_features, pars, units);
+ else
+ Y_pix = [];
+ end
+
+% Ready to exit
+ G.meas = [range_Camb; range_rate_Camb; range_Gold; range_rate_Gold; ...
+ range_Mad; range_rate_Mad; lidar];
+ G.cam = Y_pix;
+
+ G.meas(isnan(G.meas)) = [];
+
+
+end
\ No newline at end of file
diff --git a/Observations_Generation.m b/Observations_Generation.m
index 708b8df21bcf4d8954f867a42c9b6740f6628845..eb51eb651df9b5e61cacad9aa58e59359948a63e 100644
--- a/Observations_Generation.m
+++ b/Observations_Generation.m
@@ -23,11 +23,12 @@ clc
cspice_furnsh(which('MMX_QSO_049_2x2_826891269_828619269.bsp'));
cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
+%% Initial conditions per la analisi
- % Time of the analysis
- data = '2026-03-21 00:00:00 (UTC)';
+% Time of the analysis
+ data = '2026-03-16 00:00:00 (UTC)';
data = cspice_str2et(data);
- Ndays = 1;
+ Ndays = 4;
t = Ndays*86400;
date_end = data+t;
@@ -37,18 +38,15 @@ clc
% Covariance analysis parameters
pars.et0 = data;
[pars, units] = CovarianceAnalysisParameters(pars, units);
-
+ file_features = 'Principal_Phobos_Ogni10.mat';
% Positin of my target bodies at that date
Sun = cspice_spkezr('10', data, 'J2000', 'none', 'MARS');
MMX = cspice_spkezr('-34', data, 'J2000', 'none', 'MARS');
Phobos = cspice_spkezr('-401', data, 'J2000', 'none', 'MARS');
- Phi = deg2rad(0);
-
-% [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi,...
-% 'Principal_Phobos_Ogni10.mat', par, units);
+
+
+%% Creazione lista osservabili
- file_features = 'Principal_Phobos_Ogni2.mat';
YObs = Observations_with_features(data, date_end, file_features, pars, units);
-
-% Plotfeatures_Pic(YObs(685).Features, pars)
+ save('YObs');
\ No newline at end of file
diff --git a/Observations_with_features.m b/Observations_with_features.m
index a72a4cbbdc1143e96d264d15990aa01e2fc3ada6..9207438495f71310b915695b5bbe8b837a06ef50 100644
--- a/Observations_with_features.m
+++ b/Observations_with_features.m
@@ -1,20 +1,23 @@
-function YObs = Observations_with_features(date0, date_end, file_features, pars, units)
+function YObs = Observations_with_features(date_0, date_end, file_features, pars, units)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
-% dx = Mars_Phobos_Circular_FullState(t,x,pars)
+% YObs = Observations_with_features(date0, date_end, file_features, pars, units)
%
-% Calculate the acceleration vector in the N-Body Problem with Mars and
-% Phobos only and the derivative of the coefficients to be estimated
+% Creation of the structure containing the timestamp, of the observables
+% and the available observables to be processed
%
% Input:
%
-% orbit String with the name of the type of orbit
-% date0 Date of the beginning of the analysis
-% date_end Date of the end of the analysis
+% date0 Date of the beginning of the analysis
+% date_end Date of the end of the analysis
+% file_features Name of the file containing the features positions in
+% the body-fixed reference frame
+% pars Struct with useful parameters
+% units Struct with useful units
%
% Output:
%
-% YObs_Full Vector of observations and time of observations
+% Obs Struct of observations and time of observations
%
% Author: E.Ciccarelli
%
@@ -36,21 +39,36 @@ function YObs = Observations_with_features(date0, date_end, file_features, pars,
sigma_range_rate = pars.ObsNoise.range_rate*units.vsf;
sigma_lidar = pars.ObsNoise.lidar*units.lsf;
+% Minimum interval between two different observables of the same kind
+ min_interval = min([interval_Range; interval_Range_rate;...
+ interval_lidar; interval_camera]);
+
+
+%% Needed to find the Phobos libration angle at different timestamps
+
+ [Ph, pars] = Phobos_States_NewModel(date_0,pars);
+ Ph = Ph./units.sfVec2;
+ St0 = [Ph; reshape(eye(pars.d2),[pars.d2^2,1])];
+ tspan = (0:min_interval:date_end-date_0)*units.tsf;
+ RelTol = 1e-13;
+ AbsTol = 1e-16;
+ opt = odeset('RelTol',RelTol,'AbsTol',AbsTol);
+ [~,X] = ode113(@(t,X) MP_CoupledLibration(t,X,pars),tspan,St0,opt);
+ t = (0:min_interval:date_end-date_0);
+ Phi_t = X(:,7);
+
%% Observations
- et0 = date0;
- et_end = date_end;
-
+ et0 = date_0;
+ et_end = date_end;
+
+% Inizzializzazione struttura
YObs = [];
- min_interval = min([interval_Range; interval_Range_rate; interval_lidar; interval_camera]);
-
-
for i = et0:min_interval:et_end
- got_it = 0;
-
+ got_it = 0;
%--------------------------------------------------------------------------
% LIDAR OBSERVABLES WRT PHOBOS SURFACE
@@ -63,15 +81,15 @@ function YObs = Observations_with_features(date0, date_end, file_features, pars,
if (norm(MMX(1:3)-Phobos(1:3)) < LIDARlimit) && (rem((i-et0),interval_lidar)==0)
rPh = Phobos(1:3);
- [Ph, pars] = Phobos_States_NewModel(i,pars);
- I = rPh/Ph(1);
+ I = rPh/norm(rPh);
k = pars.perifocal2MARSIAU*[0; 0; 1];
j = cross(k,I);
NO = [I,j,k];
ON = NO';
- BO = [cos(Ph(end-1)), sin(Ph(end-1)), 0; -sin(Ph(end-1)), cos(Ph(end-1)), 0; 0, 0, 1];
+ Phi = Phi_t(t == (i-et0));
+ BO = [cos(Phi), sin(Phi), 0; -sin(Phi), cos(Phi), 0; 0, 0, 1];
rsb = MMX(1:3)-rPh;
@@ -189,8 +207,11 @@ function YObs = Observations_with_features(date0, date_end, file_features, pars,
Sun = cspice_spkezr('10', i, 'J2000', 'none', 'MARS');
MMX = cspice_spkezr('-34', i, 'J2000', 'none', 'MARS');
Phobos = cspice_spkezr('-401', i, 'J2000', 'none', 'MARS');
- Phi = deg2rad(0);
+
+% What's the Phobos libration angle at that t?
+ Phi = Phi_t(t == (i-et0));
+% Identification of the features
[~, Y_pix] = visible_features(MMX, Phobos, Sun, Phi,...
file_features, pars, units);
got_it = 1;
@@ -208,12 +229,19 @@ function YObs = Observations_with_features(date0, date_end, file_features, pars,
if (got_it == 1)
Obs.t = i-et0;
- DSN = [R_Cam; R_dot_Cam; R_Gold; R_dot_Gold; R_Mad; R_dot_Mad];
- if sum(~isnan(DSN))>0
- Obs.DSN = [R_Cam; R_dot_Cam; R_Gold; R_dot_Gold; R_Mad; R_dot_Mad];
+ Range = [R_Cam; R_Gold; R_Mad];
+ if sum(~isnan(Range))>0
+ Obs.Range = [R_Cam; R_Gold; R_Mad];
else
- Obs.DSN = [];
+ Obs.Range = [];
end
+ Rate = [R_dot_Cam; R_dot_Gold; R_dot_Mad];
+ if sum(~isnan(Rate))>0
+ Obs.Rate = [R_dot_Cam; R_dot_Gold; R_dot_Mad];
+ else
+ Obs.Rate = [];
+ end
+
Obs.Lidar = Lidar;
Obs.Features= Y_pix;
YObs = [YObs; Obs];
diff --git a/PlotGeometryAndLight.m b/PlotGeometryAndLight.m
index 2fd3a9b566a02968e3676e4874767fc7c107127a..47f5b32cc8f5b2e4678dd73ec1010aeb8b011b43 100644
--- a/PlotGeometryAndLight.m
+++ b/PlotGeometryAndLight.m
@@ -15,11 +15,12 @@ function PlotGeometryAndLight(points, point_in_light, visible, r_sb_Ph, r_PhSun,
R_CV = R_bCam2CV*T;
- figure()
+ figure(1)
+ hold off
+ plot3(points(1,:), points(2,:), points(3,:),'.','Color','b')
+ hold on
grid on
axis equal
- hold on
- plot3(points(1,:), points(2,:), points(3,:),'.','Color','b')
plot3(point_in_light(1,:), point_in_light(2,:), point_in_light(3,:), 'x','Color','g')
if ~isempty(visible)
plot3(visible(1,:), visible(2,:), visible(3,:), 'o','Color','r')
@@ -39,5 +40,4 @@ function PlotGeometryAndLight(points, point_in_light, visible, r_sb_Ph, r_PhSun,
-r_sb_Ph(1)/units.tsf,-r_sb_Ph(2)/units.tsf,-r_sb_Ph(3)/units.tsf,'LineWidth',1.5,'Color','r')
plotCamera('location',r_sb_Ph,'orientation',R_CV,'Color','r')
-
end
\ No newline at end of file
diff --git a/Results.m b/Results.m
new file mode 100644
index 0000000000000000000000000000000000000000..7c2fb7e34704db9bcb4e112c2cb0f85d2c8bc4d4
--- /dev/null
+++ b/Results.m
@@ -0,0 +1,330 @@
+function Results(Est, YObs_Full, pars, units)
+%==========================================================================
+% NewCov_ResultsPlot(Est, YObs_Full, pars)
+%
+% This function plots the results of the covariance analysis
+%
+% INPUT: Description Units
+%
+% Est - Estimation of:
+% .X, States at final t
+% .x, States' deviation vector at final t
+% .P, Covariance matrix at final t
+% .t, Time observation vector
+% .err, Post-fit residuals
+% .pre, Pre-fit residuals
+% .y_t, Pre-fit residuals
+% .X_t, States at different t
+% .x_t, States deviation vector at different t
+% .Phi_t, STM at different t
+% .P_t, Covariance matrix at different t
+% .Pbar_t, Covariance matrix time update at different t
+%
+% YObs_Full Vector of observations and time of observations
+%
+% pars - Structure containing problem's parameters
+%
+%
+% Coupling:
+% None
+%
+% Author: Edoardo Ciccarelli
+% Date: 09/2021
+%
+%==========================================================================
+
+ set(0,'DefaultTextInterpreter','latex');
+ set(0,'DefaultAxesFontSize', 16);
+
+ t_obs = (0:100:YObs_Full(end).t);
+
+% Plot of the post-fit residuals
+
+% figure(1)
+% subplot(1,2,1)
+% plot(t_obs/3600,Est.pre(1,:),'x','Color','b')
+% grid on;
+% hold on;
+% plot(t_obs/3600,Est.pre(3,:),'x','Color','b')
+% plot(t_obs/3600,Est.pre(5,:),'x','Color','b')
+% plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(1,:),2)),'.-','Color','b')
+% plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(1,:),2)),'.-','Color','b')
+% plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(3,:),2)),'.-','Color','b')
+% plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(3,:),2)),'.-','Color','b')
+% plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(5,:),2)),'.-','Color','b')
+% plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(5,:),2)),'.-','Color','b')
+% xlabel('$[hour]$')
+% ylabel('$[km]$')
+% title('Range Pre-fit residual')
+% figure(2)
+% subplot(1,2,1)
+% plot(t_obs/3600,Est.pre(2,:),'x','Color','r')
+% grid on;
+% hold on;
+% plot(t_obs/3600,Est.pre(4,:),'x','Color','r')
+% plot(t_obs/3600,Est.pre(6,:),'x','Color','r')
+% plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(2,:),2)),'.-','Color','r')
+% plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(2,:),2)),'.-','Color','r')
+% plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(4,:),2)),'.-','Color','r')
+% plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(4,:),2)),'.-','Color','r')
+% plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(6,:),2)),'.-','Color','r')
+% plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(6,:),2)),'.-','Color','r')
+% xlabel('$[hour]$')
+% ylabel('$[km/s]$')
+% title('RangeRate Pre-fit residual')
+% figure(3)
+% subplot(1,2,1)
+% plot(t_obs/3600,Est.pre(7,:),'x','Color','g')
+% grid on;
+% hold on;
+% plot(t_obs/3600,3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.pre(7,:),2)),'.-','Color','g')
+% plot(t_obs/3600,-3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.pre(7,:),2)),'.-','Color','g')
+% xlabel('$[hour]$')
+% ylabel('$[km]$')
+% title('Lidar Pre-fit residual')
+% figure(4)
+% subplot(1,2,1)
+% plot(t_obs/3600,Est.pre(8,:),'x','Color','m')
+% grid on;
+% hold on;
+% plot(t_obs/3600,3*pars.ObsNoise.camera*ones(size(Est.pre(8,:),2)),'.-','Color','m')
+% plot(t_obs/3600,-3*pars.ObsNoise.camera*ones(size(Est.pre(8,:),2)),'.-','Color','m')
+% xlabel('$[hour]$')
+% ylabel('$[rad]$')
+% title('Camera Pre-fit residual')
+% figure(5)
+% subplot(1,2,1)
+% plot(t_obs/3600,Est.pre(9,:),'x','Color','k')
+% grid on;
+% hold on;
+% plot(t_obs/3600,3*pars.ObsNoise.camera*ones(size(Est.pre(9,:),2)),'.-','Color','k')
+% plot(t_obs/3600,-3*pars.ObsNoise.camera*ones(size(Est.pre(9,:),2)),'.-','Color','k')
+% xlabel('$[hour]$')
+% ylabel('$[rad]$')
+% title('Camera Pre-fit residual')
+
+
+% figure(1)
+% subplot(1,2,2)
+% histfit([Est.pre(1,:), Est.pre(3,:), Est.pre(5,:)])
+% xlabel('$[km]$')
+% title('Range Pre-fit residual')
+% figure(2)
+% subplot(1,2,2)
+% histfit([Est.pre(2,:),Est.pre(4,:), Est.pre(6,:)])
+% xlabel('$[km/s]$')
+% title('Range Rate Pre-fit residual')
+% figure(3)
+% subplot(1,2,2)
+% histfit(Est.pre(7,:))
+% xlabel('$[km]$')
+% title('Lidar Pre-fit residual')
+% figure(4)
+% subplot(1,2,2)
+% histfit(Est.pre(8,:))
+% xlabel('$[rad]$')
+% title('Camera Pre-fit residual')
+% figure(5)
+% subplot(1,2,2)
+% histfit(Est.pre(9,:))
+% xlabel('$[rad]$')
+% title('Camera Pre-fit residual')
+
+% Square-Root of the MMX covariance error
+
+ sqx = squeeze(Est.P_t(1,1,1:end-1));
+ sqy = squeeze(Est.P_t(2,2,1:end-1));
+ sqz = squeeze(Est.P_t(3,3,1:end-1));
+ SQRT_X = 3*sqrt(sqx + sqy + sqz);
+ sqvx = squeeze(Est.P_t(4,4,1:end-1));
+ sqvy = squeeze(Est.P_t(5,5,1:end-1));
+ sqvz = squeeze(Est.P_t(6,6,1:end-1));
+ SQRT_V = 3*sqrt(sqvx + sqvy + sqvz);
+
+
+% 3sigma Envelopes
+ figure()
+ subplot(1,2,1)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(1,1,1:end-1)))),'Color','b','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(2,2,1:end-1)))),'Color','r','LineWidth',1)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(3,3,1:end-1)))),'Color','g','LineWidth',1)
+ semilogy(t_obs(1:end-1)/3600,SQRT_X,'Color','k','LineWidth',2)
+ xlabel('time $[hour]$')
+ ylabel('$[km]$')
+ title('MMX position vector $3\sigma$ envelopes','Interpreter','latex','FontSize',16)
+ legend('$3\sigma_{x}$','$3\sigma_{y}$','$3\sigma_{z}$','$3 RMS$','Interpreter','latex','FontSize',14)
+ subplot(1,2,2)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(4,4,1:end-1)))),'Color','b','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(5,5,1:end-1)))),'Color','r','LineWidth',1)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(6,6,1:end-1)))),'Color','g','LineWidth',1)
+ semilogy(t_obs(1:end-1)/3600,SQRT_V,'Color','k','LineWidth',2)
+ xlabel('time $[hour]$')
+ ylabel('$[km/s]$')
+ title('MMX velocity vector $3\sigma$ envelopes','Interpreter','latex','FontSize',16)
+ legend('$3\sigma_{\dot{x}}$','$3\sigma_{\dot{y}}$','$3\sigma_{\dot{z}}$','$3 RMS$','Interpreter','latex','FontSize',14)
+
+
+% Phobos's states uncertainties evolution
+
+% % 3sigma Envelopes
+
+ figure()
+ subplot(2,4,1)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(7,7,1:end-1)))),'Color','b','LineWidth',1)
+ grid on
+ xlabel('time [hour]')
+ ylabel('$R_{Ph}$ [km]')
+ subplot(2,4,2)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(9,9,1:end-1)))),'Color','r','LineWidth',1)
+ grid on
+ xlabel('time [hour]')
+ ylabel('$\theta_{Ph}$ [rad]')
+ subplot(2,4,3)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(11,11,1:end-1)))),'Color','g','LineWidth',1)
+ grid on
+ xlabel('time [hour]')
+ ylabel('$\Phi_{M}$ [rad]')
+ subplot(2,4,4)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(13,13,1:end-1)))),'Color','m','LineWidth',1)
+ grid on
+ xlabel('time [hour]')
+ ylabel('$\Phi_{Ph}$ [rad]')
+
+ subplot(2,4,5)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(8,8,1:end-1)))),'Color','b','LineWidth',1)
+ grid on
+ xlabel('time [hour]')
+ ylabel('$\dot{R}_{Ph}$ [km/s]')
+ subplot(2,4,6)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(10,10,1:end-1)))),'Color','r','LineWidth',1)
+ grid on
+ xlabel('time [hour]')
+ ylabel('$\dot{\theta}_{Ph}$ [rad/s]')
+ subplot(2,4,7)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(12,12,1:end-1)))),'Color','g','LineWidth',1)
+ grid on
+ xlabel('time [hour]')
+ ylabel('$\dot{\Phi}_{M}$ [rad/s]')
+ subplot(2,4,8)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(14,14,1:end-1)))),'Color','m','LineWidth',1)
+ grid on
+ xlabel('time [hour]')
+ ylabel('$\dot{\Phi}_{Ph}$ [rad/s]')
+
+% Harmonics 3sigma Envelopes
+
+% place0 = size(Est.X,1)-pars.nCoeff-pars.nBias;
+ corr_label = {'$x$','$y$','$z$','$\dot{x}$','$\dot{y}$','$\dot{z}$',...
+ '$R_{Ph}$','$\dot{R}_{Ph}$','$\theta_{Ph}$','$\dot{\theta}_{Ph}$',...
+ '$\Phi_{M}$','$\dot{\Phi}_{M}$','$\Phi_{Ph}$','$\dot{\Phi}_{Ph}$'};
+
+
+% Moments of inertia
+% figure()
+% subplot(1,3,1)
+% semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(15,15,1:end-1)))),'LineWidth',1);
+% grid on;
+% hold on;
+ corr_label{end+1} = ['$I_{PhX}$'];
+% xlabel('time $[hour]$')
+% legend('$I_{PhX}$','Interpreter','latex','FontSize',14)
+% subplot(1,3,2)
+% semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(16,16,1:end-1)))),'LineWidth',1);
+% grid on;
+% hold on;
+ corr_label{end+1} = ['$I_{PhY}$'];
+% xlabel('time $[hour]$')
+% legend('$I_{PhY}$','Interpreter','latex','FontSize',14)
+% subplot(1,3,3)
+% semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(16,16,1:end-1)))),'LineWidth',1);
+% grid on;
+% hold on;
+ corr_label{end+1} = ['$I_{PhZ}$'];
+% xlabel('time $[hour]$')
+% legend('$I_{PhZ}$','Interpreter','latex','FontSize',14)
+
+
+% Biases
+
+ place0 = size(Est.X,1)-pars.nBias;
+ corr_label(end+1) = {'$\rho$'};
+ corr_label(end+1) = {'$\dot{\rho}$'};
+ corr_label(end+1) = {'$LIDAR_b$'};
+
+% figure()
+% subplot(1,pars.nBias,1)
+% semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(place0+1,place0+1,1:end-1)))),'LineWidth',1,...
+% 'DisplayName',('$\rho$'),'Color','b');
+% grid on;
+% hold on;
+% xlabel('time $[hour]$')
+% ylabel('$[km]$')
+% subplot(1,pars.nBias,2)
+% semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(place0+2,place0+2,1:end-1)))),'LineWidth',1,...
+% 'DisplayName',('$\dot{\rho}$'),'Color','r');
+% grid on;
+% hold on;
+% xlabel('time $[hour]$')
+% ylabel('$[km/s]$')
+% subplot(1,pars.nBias,3)
+% semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(place0+3,place0+3,1:end-1)))),'LineWidth',1,...
+% 'DisplayName',('$LIDAR_b$'),'Color','g');
+% grid on;
+% hold on;
+% xlabel('time $[hour]$')
+% ylabel('$[km]$')
+
+
+%
+% % Harmonics Coefficients
+%
+ P_t_I2x = squeeze(Est.P_t(15,15,1:end-1));
+ P_t_I2y = squeeze(Est.P_t(16,16,1:end-1));
+ P_t_I2z = squeeze(Est.P_t(17,17,1:end-1));
+ Cov_I2xI2y = squeeze(Est.P_t(15,16,1:end-1));
+ Cov_I2yI2z = squeeze(Est.P_t(16,17,1:end-1));
+ Cov_I2xI2z = squeeze(Est.P_t(15,17,1:end-1));
+
+ P_t_C_20 = .25*(P_t_I2y + P_t_I2x - 2*Cov_I2xI2y);
+ P_t_C_22 = .5*(4*P_t_I2z + P_t_I2y + P_t_I2x - 4*Cov_I2xI2z - 4*Cov_I2yI2z + 2*Cov_I2xI2y);
+
+ figure()
+ subplot(1,2,1)
+ semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_20)),'LineWidth',1,'Color','b');
+ grid on;
+ hold on;
+ xlabel('time $[hour]$')
+ legend('$C_{20}$','Interpreter','latex','FontSize',14)
+ subplot(1,2,2)
+ semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_22)),'LineWidth',1,'Color','r');
+ grid on;
+ hold on;
+ xlabel('time $[hour]$')
+ legend('$C_{22}$','Interpreter','latex','FontSize',14)
+
+
+
+% Correlations coefficients
+
+ [~,corr] = readCovMatrix(real(Est.P));
+
+ figure();
+ imagesc(real(corr))
+ title('Correlation Coefficients','FontSize',16);
+ set(gca,'FontSize',16);
+ colormap(hot);
+ colorbar;
+ set(gca,'TickLabelInterpreter','latex');
+ set(gca,'XTick',(1:size(Est.X,1)));
+ set(gca,'XTickLabel',corr_label);
+ set(gca,'YTick',(1:size(Est.X,1)));
+ set(gca,'YTickLabel',corr_label);
+ axis square;
+ freezeColors;
+
+
+end
\ No newline at end of file
diff --git a/Test_Features.m b/Test_Features.m
new file mode 100644
index 0000000000000000000000000000000000000000..b56068001f8410d8e03490be4760a31ac14411dc
--- /dev/null
+++ b/Test_Features.m
@@ -0,0 +1,53 @@
+clear
+close all
+clc
+
+%% Setup
+
+ warning('off', 'all'); format longG;
+ set(0,'DefaultTextInterpreter','latex');
+ set(0,'DefaultAxesFontSize', 16);
+
+% Mac
+ restoredefaultpath
+ addpath('../useful_functions/');
+ addpath(genpath('../mice/'));
+ addpath(genpath('../computer-vision/'));
+ addpath(genpath('../paper/'));
+ addpath(genpath('../generic_kernels/'));
+ addpath(genpath('../paper/MMX_Fcn_CovarianceAnalyses/'));
+ addpath(genpath('../paper/MMX_Product/MMX_BSP_Files_GravLib/'));
+ MMX_InitializeSPICE
+ cspice_furnsh(which('mar097.bsp'));
+ cspice_furnsh(which('MARPHOFRZ.txt'));
+ cspice_furnsh(which('MMX_QSO_049_2x2_826891269_828619269.bsp'));
+ cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
+
+%% Initial conditions per la analisi
+
+% Time of the analysis
+ data = '2026-03-21 00:00:00 (UTC)';
+ data = cspice_str2et(data);
+ Ndays = 1;
+ t = Ndays*86400;
+ date_end = data+t;
+
+% Model parameters
+ [pars, units] = MMX_DefineNewModelParametersAndUnits;
+
+% Covariance analysis parameters
+ pars.et0 = data;
+ [pars, units] = CovarianceAnalysisParameters(pars, units);
+ file_features = 'Principal_Phobos_Ogni10.mat';
+
+ for i=data:100:date_end
+
+% Positin of my target bodies at that date
+ Sun = cspice_spkezr('10', i, 'J2000', 'none', 'MARS');
+ MMX = cspice_spkezr('-34', i, 'J2000', 'none', 'MARS');
+ Phobos = cspice_spkezr('-401', i, 'J2000', 'none', 'MARS');
+ Phi = 0;
+
+ [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features, pars, units);
+
+ end
\ No newline at end of file
diff --git a/UKF_Tool.m b/UKF_Tool.m
new file mode 100644
index 0000000000000000000000000000000000000000..2decf5485612b81c8c723773739cddb56e140227
--- /dev/null
+++ b/UKF_Tool.m
@@ -0,0 +1,301 @@
+function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_features)
+%==========================================================================
+%
+% [Est] = UKF_Tool(Est0,f,G_dG,R,Measures,par)
+%
+%
+% INPUT: Description Units
+%
+% Est0 - Apriori initial guess of
+% .X, States
+% .P0, Covariance matrix
+% .t0, initial time
+%
+% @f - Dynamical model
+%
+% @G - Observation's model and partial derivatives wrt the states
+%
+% O - Weight matrix
+% Nsigma - Number of sigma points
+% Measures - Observations matrix: the first column contains observation
+% times, then every column has a different type of observaion
+% type
+% par - Structure containing parameters required by @f and @G_dG
+% units - Structure containing lenght and time units
+%
+% OUTPUT:
+%
+% Est - Estimation of:
+% .X, States at final t
+% .P, Covariance matrix at final t
+% .t, Time observation vector
+% .X_t, States at different t
+% .P_t, Covariance matrix at different t
+% .Pbar_t, Covariance matrix time update at different t
+%
+%
+% Coupling:
+% None
+%
+% Author: Edoardo Ciccarelli
+% Date: 04/2021
+%
+%==========================================================================
+
+
+% Apriori initial guess, deviation and covariance matrix.
+ Xold = Est0.X;
+ Pold = Est0.P0;
+
+% Column of the measures time.
+ told = Measures(1).t;
+
+% The measures must be organized in colums depending on the type of
+% measure; each row is reffered to a given measure's time.
+ obs = Measures;
+
+
+% n,l and m are respectively the number of states, number of different
+% types of measures and number of measures per each type.
+ n = size(Xold,1);
+ m = size(obs,1);
+ q = 3;
+
+% err is the vector of residuals, the others are needed to build the
+% fitting residuals.
+ err = NaN(size(O,1),m);
+ prefit = NaN(size(O,1),m);
+ y_t = NaN(size(O,1),m);
+ X_t = zeros(n,size(Measures,1));
+ x_t = zeros(n,size(Measures,1));
+ P_t = zeros(n,n,m);
+ Pbar_t = zeros(n,n,m);
+
+% Initialization of the filter
+ k = 3 - n;
+ alpha = pars.alpha;
+ lambda = alpha^2*(n+k) - n;
+ gamma = sqrt(n+lambda);
+
+% Sigma points' weights
+ W0c = zeros(2*n, 1);
+ W0c(:) = 1./(2*(n+lambda));
+
+% Reference orbit propagation
+ interval_Range = pars.interval_Range;
+ interval_Range_rate = pars.interval_Range_rate;
+ interval_lidar = pars.interval_lidar;
+ interval_camera = pars.interval_camera;
+ min_interval = min([interval_Range; interval_Range_rate; interval_lidar; interval_camera]);
+
+ switch n
+ case 14
+ St0 = [Xold; pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar];
+ case 17
+ St0 = Xold(1:17);
+ case 18
+ St0 = pars.X0_reference;
+ case 20
+ St0 = Xold(1:17);
+ end
+
+ tspan = (0:min_interval:Measures(end).t)*units.tsf;
+ RelTol = 1e-13;
+ AbsTol = 1e-16;
+ opt = odeset('RelTol',RelTol,'AbsTol',AbsTol,'event',@(t,X) landing_Phobos(t,X,pars,units));
+ [t,X] = ode113(@(t,X) f(t,X,pars,units),tspan,St0,opt);
+ fprintf('\nOrbit propagation complete...');
+
+
+% For-loop on the observations
+ for i = 1:m
+
+% Look for the observation to process
+ if Measures(i).t == told
+ idx = 1;
+ switch n
+ case 14
+ Xmean_bar = X(idx,1:size(Est0.X,1))';
+ case 17
+ Xmean_bar = X(idx,:)';
+ case 18
+ Xmean_bar = [X(idx,1:10)'; X(idx,13:end)'; 0; 0; 0];
+ pars.PhiM = X(idx,11);
+ case 20
+ Xmean_bar = [X(idx,:)'; 0; 0; 0];
+ end
+ elseif Measures(i).t > told
+ tobs = Measures(i).t;
+ idx = find(round(t/units.tsf)==tobs);
+ switch n
+ case 14
+ Xmean_bar = X(idx,1:size(Est0.X,1))';
+ case 17
+ Xmean_bar = X(idx,:)';
+ case 18
+ Xmean_bar = [X(idx,1:10)'; X(idx,13:end)'; 0; 0; 0];
+ case 20
+ Xmean_bar = [X(idx,:)'; 0; 0; 0];
+ end
+ end
+
+% Useful quantities's initialization
+ et = Est0.t0/units.tsf + Measures(i).t;
+
+% TIME UPDATE
+% Sigma points' definition
+ S0 = real(sqrtm(Pold));
+ X0 = [(repmat(Xold,1,n)-gamma.*S0),...
+ (repmat(Xold,1,n)+gamma.*S0)];
+
+% Need to know how many observations are available
+ [Y_obs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID] = Measurement_read(Measures(i),O,units);
+
+
+ if Measures(idx).t == told
+
+% The sigma point dont need to be proppagated
+ Xbar = X0;
+
+ elseif Measures(idx).t > told
+
+% Propagation of the sigma points' trajectories
+ St0 = reshape(X0,[n*(2*n),1]);
+ tspan = (told:Measures(idx).t)*units.tsf;
+ opt = odeset('RelTol',1e-13,'AbsTol',1e-16,'event',@(t,X) landing_Phobos_UKF(t,X,pars,units));
+ [~,X_sigma] = ode113(@(t,X) fsigma(t,X,pars,units),tspan,St0,opt);
+ Xbar = reshape(X_sigma(end,1:n*(2*n))',[n,(2*n)]);
+
+ end
+
+ fprintf('\nProcesso osservazione n. : %d', i);
+
+% Process noise if needed
+ deltaT = (Measures(idx).t-told)*units.tsf;
+ Q = pars.sigma^2*eye(3);
+ switch n
+ case 14
+ Gamma = [eye(q)*deltaT^2/2; eye(q)*deltaT; zeros(n-6,q)];
+ Q = Gamma*Q*Gamma' + [zeros(6,n); zeros(n-6,6), pars.sigmaPh^2*...
+ diag([deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT])];
+ case 17
+ Gamma = [eye(q)*deltaT^2/2; eye(q)*deltaT; zeros(n-6,q)];
+ Q = Gamma*Q*Gamma' + [zeros(6,n); zeros(n-6,6), pars.sigmaPh^2*...
+ diag([deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, zeros(1,n-14)])];
+ case 18
+ Gamma = [eye(q)*deltaT^2/2; eye(q)*deltaT; zeros(n-6,q)];
+ Q = Gamma*Q*Gamma' + [zeros(6,n); zeros(n-6,6), pars.sigmaPh^2*...
+ diag([deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, zeros(1,n-12)])];
+ case 20
+ Gamma = [eye(q)*deltaT^2/2; eye(q)*deltaT; zeros(n-6,q)];
+ Q = Gamma*Q*Gamma' + [zeros(6,n); zeros(n-6,6), pars.sigmaPh^2*...
+ diag([deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, zeros(1,n-14)])];
+ end
+
+% Covariance matrix's time update
+ P_bar = Q;
+
+ for j = 1:2*n
+ P_bar = P_bar + W0c(j)*((Xbar(:,j) - Xmean_bar) * (Xbar(:,j) - Xmean_bar)');
+ end
+
+% Sigma point redefinition
+ S_bar = real(sqrtm(P_bar));
+ X0 = [(repmat(Xmean_bar,1,n)-gamma.*S_bar),...
+ (repmat(Xmean_bar,1,n)+gamma.*S_bar)];
+
+ Y = zeros(size(Y_obs.meas,1),2*n);
+ Y_feat = cell(2*n);
+
+ for j = 1:2*n
+% G(j-th sigma point)
+ [Y_sigma, ~] = G(et, X0(:,j), Stat_ID_Range, Stat_ID_Rate,...
+ check_Lidar, Features_ID, pars, units, O, file_features);
+ Y(:,j) = Y_sigma.meas;
+ Y_feat{j} = Y_sigma.cam;
+
+ if ~isempty(Y_feat{j})
+ Features_ID = intersect(Features_ID, Y_feat{j}(3,:));
+ end
+
+ end
+
+ Y_sigma = [];
+
+ if ~isempty(Y_feat{1})
+ for j = 1:2*n
+ features = features_list(Y_feat{j}, Features_ID);
+ Y_sigma = [Y_sigma, [Y(:,j); features]];
+ end
+ else
+ Y_sigma = Y;
+ end
+
+% Mean predicted measurement
+ [Y_mean,R] = G(et, Xmean_bar, Stat_ID_Range, Stat_ID_Rate,...
+ check_Lidar, Features_ID, pars, units, O, file_features);
+
+ features= features_list(Y_mean.cam, Features_ID);
+ Y_mean = [Y_mean.meas; features];
+
+ features= features_list(Y_obs.cam, Features_ID);
+ Y_obs = [Y_obs.meas; features];
+
+% Innovation covariance and cross covariance
+
+ R = [R; O(4,4)*ones(size(features,1),1)];
+ Py = diag(R);
+ Pxy = zeros(n,size(Y_obs,1));
+
+ for j = 1:2*n
+ Py = Py + W0c(j)*(Y_sigma(:,j) - Y_mean)*(Y_sigma(:,j) - Y_mean)';
+ Pxy = Pxy + W0c(j)*(X0(:,j) - Xmean_bar)*(Y_sigma(:,j) - Y_mean)';
+ end
+
+% Kalman gain
+ K = Pxy/Py;
+
+% MEASUREMENT UPDATE
+ y = (Y_obs - Y_mean);
+ Xstar = Xmean_bar + K*y;
+ P = (P_bar - K*Py*K');
+
+% Next iteration preparation
+ Xold = Xmean_bar;
+ told = Measures(idx).t;
+ Pold = P;
+
+
+% err(IDX,i) = (Y_obs - G(et, Xmean_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID, par,units)');
+% prefit(IDX,i) = (Y_obs - G(et, Xmean_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID, par,units)');
+% y_t(IDX,i) = y;
+
+
+% Storing the results
+ P_t(:,:,i) = abs(real(P)).*units.CovDim;
+ Pbar_t(:,:,i) = P_bar.*units.CovDim;
+ X_t(:,i) = Xold.*units.Dim;
+ x_t(:,i) = K*y.*units.Dim;
+
+
+ end
+
+ fprintf('\nAnalysis complete!\n');
+
+
+
+% Initial states' estimation, deviation and covariance matrix
+ Est.X = Xold.*units.Dim;
+ Est.P = P.*units.CovDim;
+
+% Residuals and fit-residuals at different observation times
+ Est.t = tobs;
+ Est.err = err;
+ Est.pre = prefit;
+ Est.y_t = y_t;
+ Est.X_t = X_t;
+ Est.x_t = x_t;
+ Est.P_t = P_t;
+ Est.Pbar_t = Pbar_t;
+
+end
\ No newline at end of file
diff --git a/YObs.mat b/YObs.mat
new file mode 100644
index 0000000000000000000000000000000000000000..b26142e8117fa866eed8dc4cdec26702598bb5d7
Binary files /dev/null and b/YObs.mat differ
diff --git a/features_list.m b/features_list.m
new file mode 100644
index 0000000000000000000000000000000000000000..58abdefbc001590af0517bec3f8754cb8b731800
--- /dev/null
+++ b/features_list.m
@@ -0,0 +1,11 @@
+function features = features_list(Y_feat, Features_ID)
+
+ features = [];
+ for k=1:size(Features_ID,2)
+ common_feat = Y_feat(1:2, Y_feat(3,:)==Features_ID(k));
+ if ~isempty(common_feat)
+ features = [features; common_feat];
+ end
+ end
+
+end
\ No newline at end of file
diff --git a/visible_features.m b/visible_features.m
index 4c437f4a7a6d5fa243a6264358147ad0c5eab602..a63cd7ed576002e5a7e08156c272ce543f86b845 100644
--- a/visible_features.m
+++ b/visible_features.m
@@ -13,7 +13,8 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
%
% Output:
%
-% Y Vector of features' pixel coordinates in the picture
+% Y_LOS Vector of features' Line Of Sight and features' ID
+% Y_pix Vector of features' position in the Pic and features' ID
%
% Author: E.Ciccarelli
%
@@ -34,7 +35,7 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
% Rotation from J200 to Phobos radial reference frame
- i_feat = Phobos(1:3)/norm(Phobos(1:3));
+ i_feat = Phobos(1:3)/norm(Phobos(1:3));
v = Phobos(4:6)/norm(Phobos(4:6));
k = cross(i_feat, v)/norm(cross(i_feat, v));
j = cross(k, i_feat)/norm(cross(k, i_feat));