diff --git a/Analisys_With_Features.m b/Analisys_With_Features.m
index 01e3e50b272fcb527c73e2e9fd7fe27b54477d64..7785a4761eed7995795bde14afdf1c931bd71825 100644
--- a/Analisys_With_Features.m
+++ b/Analisys_With_Features.m
@@ -21,7 +21,7 @@ clc
     MMX_InitializeSPICE
     cspice_furnsh(which('mar097.bsp'));
     cspice_furnsh(which('MARPHOFRZ.txt'));
-    cspice_furnsh(which('MMX_QSO_027_2x2_826891269_828619269.bsp'));
+    cspice_furnsh(which('MMX_QSO_198_2x2_826891269_828619269.bsp'));
     % cspice_furnsh(which('MMX_3DQSO_031_009_2x2_826891269_828619269.bsp'));
     % cspice_furnsh(which('MMX_SwingQSO_031_011_2x2_826891269_828619269.bsp'));
     cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
@@ -59,7 +59,6 @@ clc
 %   Analysis initial state vector
     St0     = [MMX0; Ph0; pars.I2; pars.bias];
     % St0     = [MMX0; Ph0(1:4); Ph0(7:8); pars.I2; pars.bias];
-    pars.X0_reference = [MMX0; Ph0; pars.I2; pars.bias];
     
     Est0.dx = zeros(size(St0,1),1);
     pars.flag_FILTERING = 0;
@@ -92,17 +91,17 @@ clc
 %   Questo è l'UKF puro, funziona giocando con l'alpha! Ma non tutti gli
 %   stati sono visibilissimi. Vedo il libration ma non la sua velocita'
 
-    % pars.alpha = 1e-1;
+    % pars.alpha = 1e0;
     % pars.beta  = 2;
     % [Est] = UKF_features(Est0, @SigmaPoints_Dynamics_Good,...
     %     @Observables_model_with_Features,...
-    %     pars.R,YObs,pars,units,file_features);
+    %     pars.R,YObs,pars,units,file_features,file_features_Mars);
 
-    % pars.alpha = 1e-1;    
+    % pars.alpha = 1e0;    
     % pars.beta  = 2;
     % [Est] = UKF_features(Est0, @SigmaPoints_Dynamics_Good_NoPhi,...
     %     @Observables_model_with_Features,...
-    %     pars.R,YObs,pars,units,file_features);
+    %     pars.R,YObs,pars,units,file_features,file_features_Mars);
 
     Results(Est, pars, units)
     
diff --git a/CovarianceAnalysisParameters.m b/CovarianceAnalysisParameters.m
index 4c52a8a353b8a6eae3d4b026ff392602b956e35a..04b9a20bc20eb3b3f2941fa892a80bf48e894aad 100644
--- a/CovarianceAnalysisParameters.m
+++ b/CovarianceAnalysisParameters.m
@@ -113,7 +113,7 @@ units.CovDim    = (repmat(units.Dim, 1, length(units.Dim)).*...
 %     1e-1; 1e-4; 1e-5; 1e-7; 1e-1; 1e-3; 1e-2*ones(pars.nCoeff,1);...
 %     1; 1e-3; 1; 1e-2; 1e-2]./units.Dim).^2);
 pars.P0         = diag(([.3*ones(3,1); .3e-3*ones(3,1);...    
-    1e-1; 1e-4; 1e-5; 1e-7; 1; 1e-2; 1e-1; 1e-3; 1e-2*ones(pars.nCoeff,1);...
+    1e-1; 1e-4; 1e-5; 1e-7; 1e-3; 1e-5; 1e-1; 1e-3; 1e-2*ones(pars.nCoeff,1);...
     1; 1e-3; 1; 1e-2; 1e-2]./units.Dim).^2);
 
 end
\ No newline at end of file
diff --git a/New Model/Dynamics_MPHandMMX_Relative.m b/New Model/Dynamics_MPHandMMX_Relative.m
new file mode 100644
index 0000000000000000000000000000000000000000..6754cc76c8cdd3b6a9edd9c8792f4facdccffc7c
--- /dev/null
+++ b/New Model/Dynamics_MPHandMMX_Relative.m	
@@ -0,0 +1,118 @@
+function dx = Dynamics_MPHandMMX_Relative(~,x,pars,~)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% dx = Dynamics_MMXonly_Relative(t,x,parss,units)
+%
+% Calculate the acceleration vector in the N-Body Problem with Mars and
+% Phobos only and the derivative of the coefficients to be estimated
+%
+% Input:
+% .t        Elapsed time since epoch (-)
+% .x        State Vector 
+% .parss     Problem parsameters
+%           .d          Dimension of the Problem
+%           .et0        Initial epoch (-)
+%           .refcenter  Origin of Frame (e.g., '399')
+%           .refframe   Coordinate Frame (e.g., 'ECLIPJ2000' or 'J2000')
+%           .Ntb        No. of third bodies
+%           .BodiesID   Bodies' SPICE ID (e.g., '10','399', ...)
+%           .GM         Bodies' Gravitational parsameters (-)
+%           .Nsh        No. of Spherical Harmonic Bodies
+%           .SH_BodiesID Spherical Harmonic Bodies' SPICE ID (e.g., '10','399', ...)
+%           .SH         Spherical Harmonic Structure
+%           .lsf        Length scale factor (km)
+%           .tsf        Time scale factor (s)
+%           .STM        true or false depending if you calculate STM or not
+%           .nCoeff     Number if coefficients in the state vector
+%
+% Output:
+% .dx       Vector containing accelerations and STM (d x 2 + nCoeff, -)
+%
+% Author: E.Ciccarelli
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    
+%%  Useful quantities
+
+    
+%   Spacecraft state vector
+    rsb = x(1:3);     % Pos. vector wrt central body
+    vsb = x(4:6);     % Vel. vector wrt central body
+
+    Rsb = norm(rsb);                        % Norm of pos. vector wrt central body
+   
+%%  Phobos's orbit
+
+    RPh         = x(7);
+    RPh_dot     = x(8);
+    theta       = x(9);
+    theta_dot   = x(10);
+    Phi         = x(11);
+    Phi_dot     = x(12);
+    Xsi         = x(13);
+    Xsi_dot     = x(14);
+    
+    IPhx_bar    = x(15);
+    IPhy_bar    = x(16);
+    IPhz_bar    = x(17);
+
+%   Position of Mars wrt Phobos
+    rPh = RPh*[cos(Xsi); sin(Xsi); 0];
+    
+%   Rotation matrix from Phobos fixed to Mars pointing reference coordinates frame
+    PhM      = [cos(Xsi), sin(Xsi), 0; -sin(Xsi), cos(Xsi), 0; 0, 0, 1];
+    MPh      = PhM';
+
+%%  Phobos's orbit
+    
+%   Potential's first order parstials
+    dVdRPh      = pars.ni/RPh^2*(1 + 3/(2*RPh^2)*((pars.IMz_bar - pars.Is) -...
+            .5*IPhx_bar - .5*IPhy_bar + IPhz_bar + ...
+            1.5*(IPhy_bar - IPhx_bar)*cos(2*Xsi)));
+    dVdXsi      = 1.5*pars.ni/RPh^3 * (IPhy_bar - IPhx_bar)*sin(2*Xsi);
+
+%   Phobos equations of motions
+    RPh_ddot    = theta_dot^2*RPh - dVdRPh/pars.ni;
+    theta_ddot  = dVdXsi/(pars.ni*RPh^2) - 2*RPh_dot*theta_dot/RPh;
+    Phi_ddot    = - dVdXsi/(pars.ni*RPh^2) + 2*RPh_dot*theta_dot/RPh;
+    Xsi_ddot    = -(1 + pars.ni*RPh^2/IPhz_bar)*dVdXsi/(pars.ni*RPh^2) +...
+                    + 2*RPh_dot*theta_dot/RPh; 
+        
+    
+%%  MMX 2BP with Mars + 3rd body Phobos 
+    
+%   The gravitational effect of Mars on MMX is 2BP+J2
+    
+    r       = rPh - rsb; 
+    R       = norm(r);
+    gM1     = pars.G*pars.MMars*r/R^3;
+    gJ2M    = 1.5*pars.G*pars.MMars*pars.J2*pars.RMmean/R^5*...
+        [(1-5*(r(3)/R)^2)*r(1); (1-5*(r(3)/R)^2)*r(2);...
+        (3-5*(r(3)/R)^2)*r(3)];
+
+    gM      = gM1 + gJ2M - pars.G*pars.MMars*rPh/norm(rPh)^3;
+    
+%   The gravitational effect of Phobos on MMX includes the Ellipsoid
+%   harmonics
+
+    C_22_Ph     = .25*(IPhy_bar - IPhx_bar);
+    C_20_Ph     = -.5*(2*IPhz_bar - IPhx_bar - IPhy_bar);
+
+    pars.SH.Clm = [1, 0, 0; 0, 0, 0; C_20_Ph, 0, C_22_Ph]./pars.SH.Norm(1:3,1:3);
+    [gPh,~,~,~]  = CGM(rsb, pars.SH);
+
+
+%   Combined effect on MMX    
+    W       = [0; 0; theta_dot-Xsi_dot];
+    W_dot   = [0; 0; theta_ddot-Xsi_ddot];
+    
+    g_tot   = gPh - cross(W_dot,rsb) - 2 * cross(W,vsb) - cross(W,cross(W,rsb)) + gM;
+
+
+%%  Output for the integrator
+    
+    dx = [vsb; g_tot; RPh_dot; RPh_ddot; theta_dot; theta_ddot;...
+        Phi_dot; Phi_ddot; Xsi_dot; Xsi_ddot; 0; 0; 0];
+    
+    
+end
\ No newline at end of file
diff --git a/New Model/Dynamics_MPHandMMX_Relative2.m b/New Model/Dynamics_MPHandMMX_Relative2.m
new file mode 100644
index 0000000000000000000000000000000000000000..6fa278a1d3fc42c26357d2c0fd4c63e4603d733d
--- /dev/null
+++ b/New Model/Dynamics_MPHandMMX_Relative2.m	
@@ -0,0 +1,112 @@
+function dx = Dynamics_MPHandMMX_Relative2(~,x,pars,~)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% dx = Dynamics_MMXonly_Relative(t,x,parss,units)
+%
+% Calculate the acceleration vector in the N-Body Problem with Mars and
+% Phobos only and the derivative of the coefficients to be estimated
+%
+% Input:
+% .t        Elapsed time since epoch (-)
+% .x        State Vector 
+% .parss     Problem parsameters
+%           .d          Dimension of the Problem
+%           .et0        Initial epoch (-)
+%           .refcenter  Origin of Frame (e.g., '399')
+%           .refframe   Coordinate Frame (e.g., 'ECLIPJ2000' or 'J2000')
+%           .Ntb        No. of third bodies
+%           .BodiesID   Bodies' SPICE ID (e.g., '10','399', ...)
+%           .GM         Bodies' Gravitational parsameters (-)
+%           .Nsh        No. of Spherical Harmonic Bodies
+%           .SH_BodiesID Spherical Harmonic Bodies' SPICE ID (e.g., '10','399', ...)
+%           .SH         Spherical Harmonic Structure
+%           .lsf        Length scale factor (km)
+%           .tsf        Time scale factor (s)
+%           .STM        true or false depending if you calculate STM or not
+%           .nCoeff     Number if coefficients in the state vector
+%
+% Output:
+% .dx       Vector containing accelerations and STM (d x 2 + nCoeff, -)
+%
+% Author: E.Ciccarelli
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    
+%%  Useful quantities
+
+    
+%   Spacecraft state vector
+    rsb = x(1:3);     % Pos. vector wrt central body
+    vsb = x(4:6);     % Vel. vector wrt central body
+
+    Rsb = norm(rsb);                        % Norm of pos. vector wrt central body
+   
+%%  Phobos's orbit
+
+    RPh         = x(7);
+    RPh_dot     = x(8);
+    Xsi         = x(9);
+    Xsi_dot     = x(10);
+    
+    IPhx_bar    = x(11);
+    IPhy_bar    = x(12);
+    IPhz_bar    = x(13);
+
+%   Position of Mars wrt Phobos
+    rPh = RPh*[cos(Xsi); sin(Xsi); 0];
+    
+%   Rotation matrix from Phobos fixed to Mars pointing reference coordinates frame
+    PhM      = [cos(Xsi), sin(Xsi), 0; -sin(Xsi), cos(Xsi), 0; 0, 0, 1];
+    MPh      = PhM';
+
+%%  Phobos's orbit
+    
+%   Potential's first order parstials
+    dVdRPh      = pars.ni/RPh^2*(1 + 3/(2*RPh^2)*((pars.IMz_bar - pars.Is) -...
+            .5*IPhx_bar - .5*IPhy_bar + IPhz_bar + ...
+            1.5*(IPhy_bar - IPhx_bar)*cos(2*Xsi)));
+    dVdXsi      = 1.5*pars.ni/RPh^3 * (IPhy_bar - IPhx_bar)*sin(2*Xsi);
+    Iz          = IPhz_bar + pars.ni*RPh^2;
+
+%   Phobos equations of motions
+    RPh_ddot    = (pars.K - IPhz_bar*Xsi_dot)^2*RPh/Iz^2 - dVdRPh/pars.ni;
+    Xsi_ddot    = -(1 + pars.ni*RPh^2/IPhz_bar)*dVdXsi/(pars.ni*RPh^2) +...
+                    + 2*(pars.K - IPhz_bar*Xsi_dot)*RPh_dot/(RPh*Iz);
+        
+    
+%%  MMX 2BP with Mars + 3rd body Phobos 
+    
+%   The gravitational effect of Mars on MMX is 2BP+J2
+    
+    r       = rPh - rsb; 
+    R       = norm(r);
+    gM1     = pars.G*pars.MMars*r/R^3;
+    gJ2M    = 1.5*pars.G*pars.MMars*pars.J2*pars.RMmean/R^5*...
+        [(1-5*(r(3)/R)^2)*r(1); (1-5*(r(3)/R)^2)*r(2);...
+        (3-5*(r(3)/R)^2)*r(3)];
+
+    gM      = gM1 + gJ2M - pars.G*pars.MMars*rPh/norm(rPh)^3;
+    
+%   The gravitational effect of Phobos on MMX includes the Ellipsoid
+%   harmonics
+
+    C_22_Ph     = .25*(IPhy_bar - IPhx_bar);
+    C_20_Ph     = -.5*(2*IPhz_bar - IPhx_bar - IPhy_bar);
+
+    pars.SH.Clm = [1, 0, 0; 0, 0, 0; C_20_Ph, 0, C_22_Ph]./pars.SH.Norm(1:3,1:3);
+    [gPh,~,~,~]  = CGM(rsb, pars.SH);
+
+
+%   Combined effect on MMX   
+    theta_dot = (pars.K - IPhz_bar*Xsi_dot)/Iz;
+    W       = [0; 0; theta_dot-Xsi_dot];
+    
+    g_tot   = gPh - 2 * cross(W,vsb) - cross(W,cross(W,rsb)) + gM;
+
+
+%%  Output for the integrator
+    
+    dx = [vsb; g_tot; RPh_dot; RPh_ddot; Xsi_dot; Xsi_ddot; 0; 0; 0];
+    
+    
+end
\ No newline at end of file
diff --git a/New Model/MARPHOFRZ.txt b/New Model/MARPHOFRZ.txt
new file mode 100644
index 0000000000000000000000000000000000000000..db722d0e2c3839c223ddb77c50f2c9a0a51feef1
--- /dev/null
+++ b/New Model/MARPHOFRZ.txt	
@@ -0,0 +1,22 @@
+\begindata
+	FRAME_MARPHOFRZ = 14499401
+	FRAME_14499401_NAME = 'MARPHOFROZEN'
+	FRAME_14499401_CLASS = 5
+	FRAME_14499401_CLASS_ID = 14499401
+	FRAME_14499401_CENTER = 499
+	FRAME_14499401_RELATIVE = 'J2000'
+	FRAME_14499401_DEF_STYLE = 'PARAMETERIZED'
+	FRAME_14499401_FAMILY = 'TWO-VECTOR'
+	FRAME_14499401_PRI_AXIS = 'X'
+	FRAME_14499401_PRI_VECTOR_DEF = 'OBSERVER_TARGET_POSITION'
+	FRAME_14499401_PRI_OBSERVER = 499
+	FRAME_14499401_PRI_TARGET = 401
+	FRAME_14499401_PRI_ABCORR = 'NONE'
+	FRAME_14499401_SEC_AXIS = 'Y'
+	FRAME_14499401_SEC_VECTOR_DEF = 'OBSERVER_TARGET_VELOCITY'
+	FRAME_14499401_SEC_OBSERVER = 499
+	FRAME_14499401_SEC_TARGET = 401
+	FRAME_14499401_SEC_ABCORR = 'NONE'
+	FRAME_14499401_SEC_FRAME = 'J2000'
+	FRAME_14499401_FREEZE_EPOCH = @2026-MAR-16/00:00:00.00
+\begintext
diff --git a/New Model/MMX_SwingQSOM_Blender.txt b/New Model/MMX_SwingQSOM_Blender.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/New Model/PHMARFRZ.txt b/New Model/PHMARFRZ.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f11a3e9f6de06e8f7b73f016c22fe116fa452cdc
--- /dev/null
+++ b/New Model/PHMARFRZ.txt	
@@ -0,0 +1,22 @@
+\begindata
+	FRAME_PHMARFRZ = 14499401
+	FRAME_14499401_NAME = 'PHOMARFROZEN'
+	FRAME_14499401_CLASS = 5
+	FRAME_14499401_CLASS_ID = 14499401
+	FRAME_14499401_CENTER = 401
+	FRAME_14499401_RELATIVE = 'J2000'
+	FRAME_14499401_DEF_STYLE = 'PARAMETERIZED'
+	FRAME_14499401_FAMILY = 'TWO-VECTOR'
+	FRAME_14499401_PRI_AXIS = 'X'
+	FRAME_14499401_PRI_VECTOR_DEF = 'OBSERVER_TARGET_POSITION'
+	FRAME_14499401_PRI_OBSERVER = 401
+	FRAME_14499401_PRI_TARGET = 499
+	FRAME_14499401_PRI_ABCORR = 'NONE'
+	FRAME_14499401_SEC_AXIS = 'Y'
+	FRAME_14499401_SEC_VECTOR_DEF = 'OBSERVER_TARGET_VELOCITY'
+	FRAME_14499401_SEC_OBSERVER = 401
+	FRAME_14499401_SEC_TARGET = 499
+	FRAME_14499401_SEC_ABCORR = 'NONE'
+	FRAME_14499401_SEC_FRAME = 'J2000'
+	FRAME_14499401_FREEZE_EPOCH = @2026-MAR-16/00:00:00.00
+\begintext
diff --git a/New Model/Test_relativeDyn.m b/New Model/Test_relativeDyn.m
new file mode 100644
index 0000000000000000000000000000000000000000..f8633ea5238bed8918426163f9f8a61a989efbeb
--- /dev/null
+++ b/New Model/Test_relativeDyn.m	
@@ -0,0 +1,194 @@
+clear
+close all
+clc
+
+%%  Test model With MMX
+
+    warning('on', 'all'); format longG;
+    set(0,'DefaultTextInterpreter','latex');
+    set(0,'DefaultAxesFontSize', 16);
+
+%   Mac
+    restoredefaultpath
+    addpath('../../useful_functions/');
+    addpath('../../useful_functions/planet_textures/');
+    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('MARPHOFRZ.txt');
+    cspice_furnsh('PHMARFRZ.txt');
+    cspice_furnsh(which('mar097.bsp'));
+    cspice_furnsh(which('MMX_QSO_198_2x2_826891269_828619269.bsp'));
+    % cspice_furnsh(which('MMX_3DQSO_031_009_2x2_826891269_828619269.bsp'));
+    % cspice_furnsh(which('MMX_SwingQSO_094_006_2x2_826891269_828619269.bsp'));
+    cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
+    
+%   Model parsameters
+    [pars, units] = MMX_DefineNewModelParametersAndUnits;
+    alpha       = 13.03;                          %km
+    beta        = 11.4;                           %km
+    gamma       = 9.14;                           %km
+    Phobos      = struct('alpha',alpha/units.lsf,'beta',beta/units.lsf,'gamma',gamma/units.lsf);
+    Geom.alpha  = alpha;
+    Geom.beta   = beta;
+    Geom.gamma  = gamma;
+    
+%%  Integration Phobos's new model + MMX
+
+%   Initial MMX's state vector
+    data        = '2026-03-16 00:00:00 (UTC)';
+    freq        = 300;
+    data        = cspice_str2et(data);
+    day         = 86400;
+    pars.et0    = data;
+    [Ph,pars]   = Phobos_States_NewModel(data,pars);
+
+%   Initial Phobos's state vector
+    Ph0     = Ph./units.sfVec2;
+    Iz      = pars.IPhz_bar + pars.ni*Ph0(1)^2;
+    K       = Iz*Ph0(4) + pars.IPhz_bar*Ph0(8);
+    pars.K  = K;
+    
+%   Initial MMX's State vector
+    MMX0    = cspice_spkezr('-34', data, 'IAU_PHOBOS', 'none', '-401');
+    MMX0    = MMX0./units.sfVec;
+
+%   Analysis initial state vector
+    St0     = [MMX0; Ph0; pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar];
+    St02    = [MMX0; Ph0(1:2); Ph0(7:8); pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar];
+
+%   Integration
+    tspan   = (0:freq:2*day)*units.tsf;
+    RelTol  = 1e-13;
+    AbsTol  = 1e-16; 
+    opt     = odeset('RelTol',RelTol,'AbsTol',AbsTol,'event', @(t,X) landing_Phobos_relative(t,X,pars,units));
+    [t,X]   = ode113(@(t,X) Dynamics_MPHandMMX_Relative(t,X,pars,units),tspan,St0,opt);
+    t       = t/units.tsf;
+    [t1,X1] = ode113(@(t,X) Dynamics_MPHandMMX_Relative2(t,X,pars,units),tspan,St02,opt);
+    
+
+
+%%  Plot
+
+    
+%   QSO orbit
+    figure(1)
+    plot3(X(:,1)*units.lsf,X(:,2)*units.lsf,X(:,3)*units.lsf,'LineWidth',1.3)
+    hold on;
+    grid on;
+    plot3(X1(:,1)*units.lsf,X1(:,2)*units.lsf,X1(:,3)*units.lsf,'LineWidth',1.3)
+    planetPlot('asteroid',[0;0;0],pars.Phobos,1);
+    view(2)
+    axis equal
+    ylabel('Along Track [km]','FontSize',26,'Interpreter','latex')
+    xlabel('Radial [km]','FontSize',26,'Interpreter','latex')
+    zlabel('Z [km]','FontSize',26,'Interpreter','latex')
+    
+
+%     figure(2)
+%     subplot(2,3,1)
+%     plot(t/3600, X(:,1)-X1(:,1))
+%     grid on
+%     subplot(2,3,2)
+%     plot(t/3600, X(:,2)-X1(:,2))
+%     grid on
+%     subplot(2,3,3)
+%     plot(t/3600, X(:,3)-X1(:,3))
+%     grid on
+%     subplot(2,3,4)
+%     plot(t/3600, X(:,4)-X1(:,4))
+%     grid on
+%     subplot(2,3,5)
+%     plot(t/3600, X(:,5)-X1(:,5))
+%     grid on
+%     subplot(2,3,6)
+%     plot(t/3600, X(:,6)-X1(:,6))
+%     grid on
+% 
+%     figure(3)
+%     subplot(2,3,1)
+%     plot(t/3600, X(:,1))
+%     hold on
+%     grid on
+%     plot(t1/3600, X1(:,1))
+%     subplot(2,3,2)
+%     plot(t/3600, X(:,2))
+%     hold on
+%     grid on
+%     plot(t1/3600, X1(:,2))
+%     subplot(2,3,3)
+%     plot(t/3600, X(:,3))
+%     hold on
+%     grid on
+%     plot(t1/3600, X1(:,3))
+%     subplot(2,3,4)
+%     plot(t/3600, X(:,4))
+%     hold on
+%     grid on
+%     plot(t1/3600, X1(:,4))
+%     subplot(2,3,5)
+%     plot(t/3600, X(:,5))
+%     hold on
+%     grid on
+%     plot(t1/3600, X1(:,5))
+%     subplot(2,3,6)
+%     plot(t/3600, X(:,6))
+%     hold on
+%     grid on
+%     plot(t1/3600, X1(:,6))
+% 
+% 
+%     figure(4)
+%     subplot(2,4,1)
+%     plot(t/3600, X(:,7)-X1(:,7))
+%     grid on
+%     subplot(2,4,2)
+%     plot(t/3600, X(:,8)-X1(:,8))
+%     grid on
+%     subplot(2,4,3)
+%     plot(t/3600, X(:,9)-X1(:,9))
+%     grid on
+%     subplot(2,4,4)
+%     plot(t/3600, X(:,10)-X1(:,10))
+%     grid on
+%     subplot(2,4,5)
+%     plot(t/3600, X(:,11)-X1(:,11))
+%     grid on
+%     subplot(2,4,6)
+%     plot(t/3600, X(:,12)-X1(:,12))
+%     grid on
+%     subplot(2,4,7)
+%     plot(t/3600, X(:,13)-X1(:,13))
+%     grid on
+%     subplot(2,4,8)
+%     plot(t/3600, X(:,14)-X1(:,14))
+%     grid on
+
+
+
+%%  Difference wrt SPICE
+
+    
+    % MMX_t   = cspice_spkezr('-34', data+t', 'MARSIAU', 'none', '499')./units.sfVec;
+    % 
+    % 
+    % 
+    % figure()
+    % plot(t/3600, X(:,1) - MMX_t(1,:)')
+    % grid on
+    % hold on
+    % plot(t/3600, X(:,2) - MMX_t(2,:)')
+    % plot(t/3600, X(:,3) - MMX_t(3,:)')
+    % 
+    % figure()
+    % plot(t/3600, X(:,4) - MMX_t(4,:)')
+    % grid on
+    % hold on
+    % plot(t/3600, X(:,5) - MMX_t(5,:)')
+    % plot(t/3600, X(:,6) - MMX_t(6,:)')
+    % 
\ No newline at end of file
diff --git a/New Model/landing_Phobos_relative.m b/New Model/landing_Phobos_relative.m
new file mode 100644
index 0000000000000000000000000000000000000000..8d786d1b5db82ffb677b39924cd39bf1269ac33c
--- /dev/null
+++ b/New Model/landing_Phobos_relative.m	
@@ -0,0 +1,26 @@
+function [value,isterminal,direction] = landing_Phobos_relative(~,X,par,units)
+
+
+%   Landing on Phobos? Integration is blocked if the spacecraft touch the ellipsoid 
+    alpha   = par.Phobos.alpha/units.lsf;                 % Phobos' largest semi-axis (km) 
+    beta    = par.Phobos.beta/units.lsf;                  % Phobos' intermediate semi-axis (km)
+    gamma   = par.Phobos.gamma/units.lsf;                 % Phobos' smallest semi-axis (km)
+    
+    x       = X(1:3) ;
+    value1  = x(1)^2/alpha^2 + x(2)^2/beta^2 +...
+        x(3)^2/gamma^2 - 1;
+
+%   Landing on Mars?
+    alpha   = par.Mars.alpha/units.lsf;                 % Mars' largest semi-axis (km) 
+    beta    = par.Mars.beta/units.lsf;                  % Mars' intermediate semi-axis (km)
+    gamma   = par.Mars.gamma/units.lsf;                 % Mars' smallest semi-axis (km)
+    
+    rPh     = X(7)*[cos(X(13)); sin(X(13)); 0];
+    value2  = (x(1) - rPh(1))^2/alpha^2 + (x(2) - rPh(2))^2/beta^2 +...
+        (x(3) - rPh(3))^2/gamma^2 - 1;
+    
+    value = value1*value2;
+    isterminal = 1;
+    direction = 0;
+
+end
\ No newline at end of file
diff --git a/Observables_model_with_Features.m b/Observables_model_with_Features.m
index 9271a719a11be63da82560f3f0f41ae52a41436f..ccd87e68521f5b1d72bf2b3aff6adef3e88c0787 100644
--- a/Observables_model_with_Features.m
+++ b/Observables_model_with_Features.m
@@ -253,8 +253,8 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat
        if ~isempty(Y_pix_Mars)
             Y_pix_Mars = Y_pix_Mars + repmat([bias(4); bias(5); 0], 1, size(Y_pix_Mars,2));
        else
-           fprintf('\nTracking delle features su Marte perso!!\n');
-           return
+           % fprintf('\nTracking delle features su Marte perso!!');
+           % return
        end
        
     else
diff --git a/Plot_Nico.m b/Plot_Nico.m
index a1e75058002bbdb6e3adb997077f35771126df21..2ff7eacf78c950c8b5feed49af8a6886ebaaf938 100644
--- a/Plot_Nico.m
+++ b/Plot_Nico.m
@@ -411,27 +411,27 @@ C_20_Swing_perc= abs(3*real(sqrt(P_t_C_20_Swing))/C_20);
 C_22_Swing_perc= abs(3*real(sqrt(P_t_C_22_Swing))/C_22);
 
 figure()
-subplot(1,2,1)
+subplot(1,2,2)
 semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_20_QSO)),'LineWidth',1.2);
 grid on;
 hold on;
 semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_20_3D)),'LineWidth',1.2);
 semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_20_Swing)),'LineWidth',1.2);
 xlabel('Time $[hour]$','Fontsize',26)
-ylabel('$3\sigma_{C_{20}}$','Fontsize',30)
+ylabel('$3\sigma_{C_{22}}$','Fontsize',30)
 legend('QSO','3D-QSO','Swing-QSO','Interpreter','latex','Fontsize',26)
-subplot(1,2,2)
+subplot(1,2,1)
 semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_22_QSO)),'LineWidth',1.2);
 grid on;
 hold on;
 semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_22_3D)),'LineWidth',1.2);
 semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_22_Swing)),'LineWidth',1.2);
 xlabel('Time $[hour]$','Fontsize',26)
-ylabel('$3\sigma_{C_{22}}$','Fontsize',30)
+ylabel('$3\sigma_{C_{20}}$','Fontsize',30)
 legend('QSO','3D-QSO','Swing-QSO','Interpreter','latex','Fontsize',26)
 
 figure()
-subplot(1,2,1)
+subplot(1,2,2)
 semilogy(t_obs(1:end-1)/3600,C_20_QSO_perc,'LineWidth',1.2);
 grid on;
 hold on;
@@ -440,7 +440,7 @@ semilogy(t_obs(1:end-1)/3600,C_20_Swing_perc,'LineWidth',1.2);
 xlabel('Time $[hour]$','Fontsize',26)
 ylabel('%','Fontsize',26)
 legend('QSO','3D-QSO','Swing-QSO','Interpreter','latex','Fontsize',26)
-subplot(1,2,2)
+subplot(1,2,1)
 semilogy(t_obs(1:end-1)/3600,C_22_QSO_perc,'LineWidth',1.2);
 grid on;
 hold on;
diff --git a/Results.m b/Results.m
index 24ceda83567983f46f9efefc13c5cf4969790dee..b570daacb09a93bde6d06bcbc73275183788c2ff 100644
--- a/Results.m
+++ b/Results.m
@@ -804,32 +804,32 @@ if size(Est.X_t,1) == 20
         Cov_I2yI2z = squeeze(Est.P_t(14,15,1:end-1));
         Cov_I2xI2z = squeeze(Est.P_t(13,15,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);
+        P_t_C_22 = .25*(P_t_I2y + P_t_I2x - 2*Cov_I2xI2y);
+        P_t_C_20 = .5*(4*P_t_I2z + P_t_I2y + P_t_I2x - 4*Cov_I2xI2z - 4*Cov_I2yI2z + 2*Cov_I2xI2y);
     
        
     if pars.flag_FILTERING == 1
     
-        C_20_t  = .25*(Est.X_t(14,:) - Est.X_t(13,:));
-        C_22_t  = -.5*(2*Est.X_t(15,:) - Est.X_t(14,:) - Est.X_t(13,:));
+        C_22_t  = .25*(Est.X_t(14,:) - Est.X_t(13,:));
+        C_20_t  = -.5*(2*Est.X_t(15,:) - Est.X_t(14,:) - Est.X_t(13,:));
     
         err_Css = abs([pars.SH.Clm(3,1)*pars.SH.Norm(3,1); pars.SH.Clm(3,3)*pars.SH.Norm(3,3)] - [C_20_t; C_22_t]);
     
         figure()
-        subplot(1,2,1)
+        subplot(1,2,2)
         semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_20)),'LineWidth',1,'Color','b');
         grid on;
         hold on;
         semilogy(t_obs(1:end-1)/3600,err_Css(1,1:end-1),'*','Color','b');
         xlabel('time $[hour]$')
-        legend('$C_{20}$','Interpreter','latex','FontSize',26)
-        subplot(1,2,2)
+        legend('$C_{22}$','Interpreter','latex','FontSize',26)
+        subplot(1,2,1)
         semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_22)),'LineWidth',1,'Color','r');
         grid on;
         hold on;
         semilogy(t_obs(1:end-1)/3600,err_Css(2,1:end-1),'*','Color','r');
         xlabel('time $[hour]$')
-        legend('$C_{22}$','Interpreter','latex','FontSize',26)
+        legend('$C_{20}$','Interpreter','latex','FontSize',26)
     
     
         figure()
@@ -858,18 +858,18 @@ if size(Est.X_t,1) == 20
     else
     
         figure()
-        subplot(1,2,1)
+        subplot(1,2,2)
         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',26)
-        subplot(1,2,2)
+        legend('$C_{22}$','Interpreter','latex','FontSize',26)
+        subplot(1,2,1)
         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',26)
+        legend('$C_{20}$','Interpreter','latex','FontSize',26)
     
         
     end
@@ -884,32 +884,32 @@ else
         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);
+        P_t_C_22 = .25*(P_t_I2y + P_t_I2x - 2*Cov_I2xI2y);
+        P_t_C_20 = .5*(4*P_t_I2z + P_t_I2y + P_t_I2x - 4*Cov_I2xI2z - 4*Cov_I2yI2z + 2*Cov_I2xI2y);
     
        
     if pars.flag_FILTERING == 1
     
-        C_20_t  = .25*(Est.X_t(16,:) - Est.X_t(15,:));
-        C_22_t  = -.5*(2*Est.X_t(17,:) - Est.X_t(16,:) - Est.X_t(15,:));
+        C_22_t  = .25*(Est.X_t(16,:) - Est.X_t(15,:));
+        C_20_t  = -.5*(2*Est.X_t(17,:) - Est.X_t(16,:) - Est.X_t(15,:));
     
         err_Css = abs([pars.SH.Clm(3,1)*pars.SH.Norm(3,1); pars.SH.Clm(3,3)*pars.SH.Norm(3,3)] - [C_20_t; C_22_t]);
     
         figure()
-        subplot(1,2,1)
+        subplot(1,2,2)
         semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_20)),'LineWidth',1,'Color','b');
         grid on;
         hold on;
         semilogy(t_obs(1:end-1)/3600,err_Css(1,1:end-1),'*','Color','b');
         xlabel('time $[hour]$')
-        legend('$C_{20}$','Interpreter','latex','FontSize',26)
-        subplot(1,2,2)
+        legend('$C_{22}$','Interpreter','latex','FontSize',26)
+        subplot(1,2,1)
         semilogy(t_obs(1:end-1)/3600,3.*real(sqrt(P_t_C_22)),'LineWidth',1,'Color','r');
         grid on;
         hold on;
         semilogy(t_obs(1:end-1)/3600,err_Css(2,1:end-1),'*','Color','r');
         xlabel('time $[hour]$')
-        legend('$C_{22}$','Interpreter','latex','FontSize',26)
+        legend('$C_{20}$','Interpreter','latex','FontSize',26)
     
     
         figure()
@@ -938,18 +938,18 @@ else
     else
     
         figure()
-        subplot(1,2,1)
+        subplot(1,2,2)
         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',26)
-        subplot(1,2,2)
+        legend('$C_{22}$','Interpreter','latex','FontSize',26)
+        subplot(1,2,1)
         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',26)
+        legend('$C_{20}$','Interpreter','latex','FontSize',26)
     
         
     end
diff --git a/UKF_Tool.m b/UKF_Tool.m
index 3f1580025c44d41d37ea887e53a362a0a0b6e0a2..73f5f93b6eaa4a50c2d6476d4f6350b75b771d12 100644
--- a/UKF_Tool.m
+++ b/UKF_Tool.m
@@ -226,9 +226,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
             Y_sigma  = Y;
         end
 
-        if ~isempty(Features_Mars_ID)
-                pip = [];
-        end
+        
 
 %       Mean predicted measurement
         [Y_mean, R]  = G(et, Xmean_bar, Stat_ID_Range, Stat_ID_Rate,...
@@ -239,8 +237,16 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
         features_Mars   = features_list(Y_mean.cam_Mars, Features_Mars_ID);
         Y_mean      = [Y_mean.meas; features; features_Mars];
 
-        features    = features_list(Y_obs.cam, Features_ID);
-        features_Mars   = features_list(Y_obs.cam_Mars, Features_Mars_ID);
+        if ~isempty(features)&&(~isempty(Y_obs.cam))
+            features    = features_list(Y_obs.cam, Features_ID);
+        elseif isempty(features)&&(~isempty(Y_obs.cam))
+            fprintf('\nTracking delle features su Phobos perso!!');
+        end
+        if ~isempty(features_Mars)&&(~isempty(Y_obs.cam_Mars))
+            features_Mars   = features_list(Y_obs.cam_Mars, Features_Mars_ID);
+        elseif isempty(features_Mars)&&(~isempty(Y_obs.cam_Mars))
+            fprintf('\nTracking delle features su Marte perso!!');
+        end
         Y_obs       = [Y_obs.meas; features; features_Mars];
 
 
diff --git a/UKF_features.m b/UKF_features.m
index e861c96e76491e63592e646c1f4446dc677391ca..54d4dc476d68bc520b0b8f7d5c70bdc0a69080f9 100644
--- a/UKF_features.m
+++ b/UKF_features.m
@@ -1,4 +1,5 @@
-function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_features)
+function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units,...
+    file_features, file_features_Mars)
 %==========================================================================
 % [Est] = UKF(Est0,f,G_dG,R,Measures,par)
 %
@@ -101,7 +102,8 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature
             (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, check_Limb, Features_ID, IDX] =...
+        [Y_obs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, check_Limb,...
+            check_Deimos, Features_ID, Features_Mars_ID, IDX] = ...
             Measurement_read(Measures(i),O,units);
 
         if Measures(i).t == told
@@ -151,44 +153,60 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature
         
         Y   = zeros(size(Y_obs.meas,1),2*n);
         Y_feat = cell(2*n);
+        Y_feat_Mars = cell(2*n);
 
         for j = 1:2*n+1
 %           G(j-th sigma point)
             [Y_sigma, ~] = G(et, X0(:,j), Stat_ID_Range, Stat_ID_Rate,...
-                check_Lidar, check_Limb, Features_ID, pars, units, O, file_features);
-            Y(:,j)  = Y_sigma.meas;
-            Y_feat{j} = Y_sigma.cam;
+                check_Lidar, check_Limb, check_Deimos, Features_ID, Features_Mars_ID,...
+                pars, units, O, file_features, file_features_Mars);
+            Y(:,j)          = Y_sigma.meas;
+            Y_feat{j}       = Y_sigma.cam;
+            Y_feat_Mars{j}  = Y_sigma.cam_Mars;
             
             if ~isempty(Y_feat{j})
                 Features_ID = intersect(Features_ID, Y_feat{j}(3,:));
             end
+            
+            if ~isempty(Y_feat_Mars{j})
+                Features_Mars_ID = intersect(Features_Mars_ID, Y_feat_Mars{j}(3,:));
+            end
+
         end
 
         Y_sigma = [];
 
-        if ~isempty(Y_feat{1})
+       if (~isempty(Y_feat{1}))||(~isempty(Y_feat_Mars{1}))
             for j = 1:2*n
                 features = features_list(Y_feat{j}, Features_ID);
-                Y_sigma  = [Y_sigma, [Y(:,j); features]];
+                features_Mars = features_list(Y_feat_Mars{j}, Features_Mars_ID);
+                Y_sigma  = [Y_sigma, [Y(:,j); features; features_Mars]];
             end
         else
             Y_sigma  = Y;
-        end
+       end
+
+       if ~isempty(Features_Mars_ID)
+                pip = [];
+       end
 
 %       Mean predicted measurement
-        [Y_mean,R]  = G(et, Xmean_bar, Stat_ID_Range, Stat_ID_Rate,...
-            check_Lidar, check_Limb, Features_ID, pars, units, O, file_features);
+        [Y_mean, R]  = G(et, Xmean_bar, Stat_ID_Range, Stat_ID_Rate,...
+            check_Lidar, check_Limb,  check_Deimos, Features_ID, Features_Mars_ID,...
+            pars, units, O, file_features, file_features_Mars);
         
-        features= features_list(Y_mean.cam, Features_ID);
-        Y_mean  = [Y_mean.meas; features];
+        features    = features_list(Y_mean.cam, Features_ID);
+        features_Mars   = features_list(Y_mean.cam_Mars, Features_Mars_ID);
+        Y_mean      = [Y_mean.meas; features; features_Mars];
 
-        features= features_list(Y_obs.cam, Features_ID);
-        Y_obs   = [Y_obs.meas; features];
+        features    = features_list(Y_obs.cam, Features_ID);
+        features_Mars   = features_list(Y_obs.cam_Mars, Features_Mars_ID);
+        Y_obs       = [Y_obs.meas; features; features_Mars];
 
 
 %       Innovation covariance and cross covariance
 
-        R       = [R; O(8,8)*ones(size(features,1),1)];
+        R       = [R; O(8,8)*ones(size(features,1),1); O(8,8)*ones(size(features_Mars,1),1)];
         Py      = diag(R);
         Pxy     = zeros(n,size(Y_obs,1));
 
@@ -222,7 +240,9 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature
 %       Residuals
         [err(:,i), prefit(:,i)] = Residuals_withCamera(Y_obs, G, Xmean_bar,...
             Xstar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, check_Limb, ...
-            Features_ID, O, IDX, et, pars, units, file_features);
+            check_Deimos, Features_ID, Features_Mars_ID, O, IDX, et, pars,...
+            units, file_features, file_features_Mars);
+
 
 
 %       Storing the results
diff --git a/YObs.mat b/YObs.mat
index a090b6d36d3717c7b1b6fcbeb63ac79000341bb7..7b7c1086d597fabb00cb1336278afe0f64a15d40 100644
Binary files a/YObs.mat and b/YObs.mat differ
diff --git a/visible_features_Mars_model.m b/visible_features_Mars_model.m
index 8e60f27a816fe11f6ce72561d9207960614f3fe7..70dd967e5d8c7f42852ded4edaad17509f264cf3 100644
--- a/visible_features_Mars_model.m
+++ b/visible_features_Mars_model.m
@@ -99,6 +99,11 @@ function [Y_LOS, Y_pix] = visible_features_Mars_model(MMX, Phobos, Mars, Phi,...
     visible = [];
     Y_LOS   = [];
     Y_pix   = [];
+    PhP = PM*[-Phobos_pole; 1];
+    PhP = PhP./repmat(PhP(3),3,1);
+    PhC = PM*[-Phobos_centre; 1];
+    PhC = PhC./repmat(PhC(3),3,1);
+    Mars_centre = - Phobos(1:3);
 
     for n = 1:size(point_in_light,2)
 
@@ -106,21 +111,21 @@ function [Y_LOS, Y_pix] = visible_features_Mars_model(MMX, Phobos, Mars, Phi,...
 
         r_sf = r_sb_Ph - candidate;
         
-        % i_feat = -r_sf/norm(r_sf);
+        i_feat = -r_sf/norm(r_sf);
         v      = [0; 0; 1];
         z_cam  = cross(v, I)/norm(cross(v, I));
         y_cam  = cross(z_cam, I)/norm(cross(z_cam, I));
 
-        % % Visibility checks
-        % angle_feat  = acos(dot(I, -i_feat));
-        % angle_sun   = acos(dot(I, r_MSun));
-        % feat_centre = candidate - Mars_centre;
-        % Phobos_range = norm(PhP - PhC);
-        % Phobos_ellipsoid = @(x,y) (x-PhC(1))^2 + (y-PhC(2))^2 - Phobos_range^2;
-        % 
+        % Visibility checks
+        angle_feat  = acos(dot(I, -i_feat));
+        angle_sun   = acos(dot(I, r_MSun));
+        feat_centre = candidate - Mars_centre;
+        Phobos_range = norm(PhP - PhC);
+        Phobos_ellipsoid = @(x,y) (x-PhC(1))^2 + (y-PhC(2))^2 - Phobos_range^2;
+
         
         % Observable creation
-        % if (angle_sun>pars.FOV)&&(dot(r_sb_Ph, candidate)<0)&&(dot(feat_centre,i_feat)<0)
+        if (angle_sun>pars.FOV)&&(dot(r_sb_Ph, candidate)<0)&&(dot(feat_centre,i_feat)<0)
             LOS_feat    = [y_cam'; z_cam'] * r_sf/norm(r_sf) + [random('Normal',0, pars.ObsNoise.camera);...
                                random('Normal',0, pars.ObsNoise.camera)];
 
@@ -129,18 +134,18 @@ function [Y_LOS, Y_pix] = visible_features_Mars_model(MMX, Phobos, Mars, Phi,...
             pix_feat    = [(pix_feat(1:2)+ [random('Normal',0, pars.ObsNoise.pixel);...
                 random('Normal',0, pars.ObsNoise.pixel)]); point_in_light(end,n)];
 
-            % if (Phobos_ellipsoid(pix_feat(1), pix_feat(2))>0)&&...
-                % (all(pix_feat>0))&&(pix_feat(1)<pars.cam.nPixX)&&...
-                % (pix_feat(2)<pars.cam.nPixY)
+            if (Phobos_ellipsoid(pix_feat(1), pix_feat(2))>0)&&...
+                (all(pix_feat>0))&&(pix_feat(1)<pars.cam.nPixX)&&...
+                (pix_feat(2)<pars.cam.nPixY)
                 visible = [candidate, visible];
                 Y_LOS   = [[LOS_feat; point_in_light(end,n)], Y_LOS];
                 Y_pix   = [pix_feat, Y_pix];
                 
-            % else
-            %     % fprintf('\nMars feature scaricata perche dietro Phobos...')
-            % end
+            else
+                % fprintf('\nMars feature scaricata perche dietro Phobos...')
+            end
 
-        % end
+        end
     end