diff --git a/Analisys_With_Features.m b/Analisys_With_Features.m
index 5f201af116341d3b70e02d119ff430fb01032a37..9d3f1e9a40bd4c85935e57ab91dfaad58ab8abfc 100644
--- a/Analisys_With_Features.m
+++ b/Analisys_With_Features.m
@@ -10,14 +10,15 @@ clc
% Mac
restoredefaultpath
- addpath('../useful_functions/');
- addpath('../dynamical_model/');
+ addpath(genpath('../useful_functions/'));
+ addpath(genpath('../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/'));
+ addpath(genpath('./Utilities/'))
MMX_InitializeSPICE
cspice_furnsh(which('mar097.bsp'));
cspice_furnsh(which('MARPHOFRZ.txt'));
diff --git a/New Model/Analisys.m b/New Model/Analisys.m
index eeb1cc406fb77eaeab23d2e0196242f3335482ab..8a2429b7399e1c9a69c8d30b537cce6b1e55c5cb 100644
--- a/New Model/Analisys.m
+++ b/New Model/Analisys.m
@@ -16,7 +16,8 @@ clc
addpath(genpath('../../generic_kernels/'));
addpath(genpath('../../paper/MMX_Fcn_CovarianceAnalyses/'));
addpath(genpath('../../paper/MMX_Product/MMX_BSP_Files_GravLib/'));
- cspice_kclear
+ addpath(genpath('./Utilities_relative/'))
+ addpath(genpath('../Utilities/'))
MMX_InitializeSPICE
cspice_furnsh('MARPHOFRZ.txt');
cspice_furnsh(which('mar097.bsp'));
diff --git a/New Model/Analisys_harmonics.m b/New Model/Analisys_harmonics.m
index 1b14fdb3956545e11c315c34463d1978879076ea..d71847efd6145a861115a08a549433f3a431ff8a 100644
--- a/New Model/Analisys_harmonics.m
+++ b/New Model/Analisys_harmonics.m
@@ -5,24 +5,25 @@ clc
%% Setup
restoredefaultpath
- addpath('../');
- addpath('./MMX_BSP_Files_Relative/');
- addpath('../../useful_functions/');
- addpath('../../useful_functions/planet_textures/');
- addpath('../../dynamical_model/');
+ addpath(genpath('../'));
+ addpath(genpath('./MMX_BSP_Files_Relative/'));
+ addpath(genpath('../../useful_functions/'));
+ addpath(genpath('../../useful_functions/planet_textures/'));
+ addpath(genpath('../../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/'));
- cspice_kclear
+ addpath(genpath('./Utilities_relative/'))
+ addpath(genpath('../Utilities/'))
MMX_InitializeSPICE
- cspice_furnsh('MARPHOFRZ.txt');
+ cspice_furnsh(which('MARPHOFRZ.txt'));
cspice_furnsh(which('mar097.bsp'));
- cspice_furnsh(which('MMX_QSO_027_8x8_826891269_829483269_relative_right.bsp'));
- % cspice_furnsh(which('MMX_3DQSO_031_013_8x8_826891269_829483269_relative_right.bsp'));
- % cspice_furnsh(which('MMX_SwingQSO_027_011_8x8_826891269_829483269_relative_right.bsp'));
+ % cspice_furnsh(which('MMX_QSO_031_4x4_826891269_829483269_relative_right.bsp'));
+ % cspice_furnsh(which('MMX_3DQSO_031_013_4x4_826891269_829483269_relative_right.bsp'));
+ cspice_furnsh(which('MMX_SwingQSO_031_013_4x4_826891269_829483269_relative_right.bsp'));
cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
@@ -45,8 +46,7 @@ clc
% Covariance analysis parsameters
[pars, units] = CovarianceAnalysisParameters_harmonics(pars, units);
- pars.sigma = 1e-10/(units.vsf*units.tsf);
-
+
% Initial Phobos's state vector
Ph0 = Ph./units.sfVec2;
Iz = pars.IPhz_bar + pars.ni*Ph0(1)^2;
@@ -69,7 +69,7 @@ clc
%% Analysis
% Features catalogues
- file_features = 'Principal_Phobos_ogni2.mat';
+ file_features = 'Principal_Phobos_ogni10.mat';
file_features_Mars = 'Mars_4facets.mat';
% Analysis
@@ -81,7 +81,7 @@ clc
pars.R,YObs,pars,units,file_features,file_features_Mars);
- Results_relative(Est, pars, units)
+ Results_relative_harmonics(Est, [4,4], pars, units)
% save('./Results/QSOLc_6days_Auto.mat', 'Est')
diff --git a/New Model/CovarianceAnalysisParameters_harmonics.m b/New Model/CovarianceAnalysisParameters_harmonics.m
index 62b752597b04c85bf8d67722492a8af94b4a6745..6da02f50bce99b0ba855c3d36734cac948bc7105 100644
--- a/New Model/CovarianceAnalysisParameters_harmonics.m
+++ b/New Model/CovarianceAnalysisParameters_harmonics.m
@@ -118,7 +118,10 @@ units.CovDim = (repmat(units.Dim, 1, length(units.Dim)).*...
% A-priori covariance
pars.P0 = diag(([1*ones(3,1); .1e-4*ones(3,1);...
- 1e-1; 1e-5; 1e-4; 1e-7; 1e-3*ones(pars.nCoeff,1);...
+ 1e-1; 1e-5; 1e-4; 1e-7; 1*ones(pars.nCoeff,1);...
1; 1e-3; 1; 1e-2; 1e-2]./units.Dim).^2);
+% Process noise su MMX e Phobos
+pars.sigma = 1e-10/(units.vsf*units.tsf);
+pars.sigmaPh = 0/(units.vsf*units.tsf);
end
\ No newline at end of file
diff --git a/New Model/MMX_DefineNewModelParametersAndUnits_Harm.m b/New Model/MMX_DefineNewModelParametersAndUnits_Harm.m
index b948a5c69c9341dde5aa5dc65c757911ead548f1..2e2a24173db34463780ec8f11557d08856aab8a0 100644
--- a/New Model/MMX_DefineNewModelParametersAndUnits_Harm.m
+++ b/New Model/MMX_DefineNewModelParametersAndUnits_Harm.m
@@ -84,7 +84,7 @@ function [pars, units] = MMX_DefineNewModelParametersAndUnits_Harm
% Phobos gravitational harmonics WRT principal axis of inertia
- pars.SH = PhobosSphericalHarmonicCoefficients(8,8);
+ pars.SH = PhobosSphericalHarmonicCoefficients(4,4);
pars.SH.Re = pars.SH.Re/units.lsf;
pars.SH.GM = pars.SH.GM/units.GMsf;
diff --git a/New Model/Observations_Generation_relative.m b/New Model/Observations_Generation_relative.m
index 4af71cfaadeaff17d45043749e0e2b0ce5e146fb..e352ab90dce637ff63c6e5177863facbe8fb700c 100644
--- a/New Model/Observations_Generation_relative.m
+++ b/New Model/Observations_Generation_relative.m
@@ -21,13 +21,14 @@ clc
addpath(genpath('../../generic_kernels/'));
addpath(genpath('../../paper/MMX_Fcn_CovarianceAnalyses/'));
addpath(genpath('../../paper/MMX_Product/MMX_BSP_Files_GravLib/'));
- cspice_kclear
+ addpath(genpath('./Utilities_relative/'))
+ addpath(genpath('../Utilities/'))
MMX_InitializeSPICE
- cspice_furnsh('MARPHOFRZ.txt');
+ cspice_furnsh(which('MARPHOFRZ.txt'));
cspice_furnsh(which('mar097.bsp'));
- cspice_furnsh(which('MMX_QSO_027_8x8_826891269_829483269_relative_right.bsp'));
- % cspice_furnsh(which('MMX_3DQSO_031_013_2x2_826891269_829483269_relative_right.bsp'));
- % cspice_furnsh(which('MMX_SwingQSO_027_011_2x2_826891269_829483269_relative_right.bsp'));
+ % cspice_furnsh(which('MMX_QSO_031_4x4_826891269_829483269_relative_right.bsp'));
+ % cspice_furnsh(which('MMX_3DQSO_031_013_4x4_826891269_829483269_relative_right.bsp'));
+ cspice_furnsh(which('MMX_SwingQSO_031_013_4x4_826891269_829483269_relative_right.bsp'));
cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
@@ -36,7 +37,7 @@ clc
% Time of the analysis
data = '2026-03-16 00:00:00 (UTC)';
data = cspice_str2et(data);
- Ndays = 1;
+ Ndays = 3;
t = Ndays*86400;
date_end = data+t;
@@ -46,7 +47,7 @@ clc
% Covariance analysis parameters
pars.et0 = data;
[pars, units] = CovarianceAnalysisParameters_relative(pars, units);
- file_features = 'Principal_Phobos_ogni2.mat';
+ file_features = 'Principal_Phobos_ogni10.mat';
file_features_Mars = 'Mars_4facets.mat';
diff --git a/New Model/Utilities/BSP_generator.m b/New Model/Utilities_relative/BSP_generator.m
similarity index 97%
rename from New Model/Utilities/BSP_generator.m
rename to New Model/Utilities_relative/BSP_generator.m
index a1c1289cb49b237c5c138566a831f0a3ec1e68a5..70bce1b4b8d7f5dcb8e2b4d055b83691f348eb13 100644
--- a/New Model/Utilities/BSP_generator.m
+++ b/New Model/Utilities_relative/BSP_generator.m
@@ -87,7 +87,7 @@ clc
Model.units = units;
micePATH = '~/Documents/MATLAB/mice/';
- TargetFolder = './MMX_BSP_Files_Relative/';
+ TargetFolder = '../MMX_BSP_Files_Relative/';
if (exist(TargetFolder, 'dir') ~= 7)
command = sprintf('mkdir %s', TargetFolder);
system(command);
@@ -98,7 +98,7 @@ clc
%% Test risultati
- addpath(genpath('./MMX_BSP_Files_Relative/'))
+ addpath(genpath('../MMX_BSP_Files_Relative/'))
cspice_furnsh(which('MMX_QSO_031_8x8_826891269_829483269_relative_right.bsp'));
MMX_BSP = cspice_spkezr('-35', data+t', 'IAU_Phobos', 'none', '-401');
err_MMX = PB*Xmmx - PB*MMX_BSP;
diff --git a/New Model/Utilities/Constraints.m b/New Model/Utilities_relative/Constraints.m
similarity index 100%
rename from New Model/Utilities/Constraints.m
rename to New Model/Utilities_relative/Constraints.m
diff --git a/New Model/Utilities/CostFunction.m b/New Model/Utilities_relative/CostFunction.m
similarity index 100%
rename from New Model/Utilities/CostFunction.m
rename to New Model/Utilities_relative/CostFunction.m
diff --git a/New Model/Utilities/Cov_Dynamics_Rel.m b/New Model/Utilities_relative/Cov_Dynamics_Rel.m
similarity index 100%
rename from New Model/Utilities/Cov_Dynamics_Rel.m
rename to New Model/Utilities_relative/Cov_Dynamics_Rel.m
diff --git a/New Model/Utilities/Cov_Dynamics_Rel_harmonics.m b/New Model/Utilities_relative/Cov_Dynamics_Rel_harmonics.m
similarity index 88%
rename from New Model/Utilities/Cov_Dynamics_Rel_harmonics.m
rename to New Model/Utilities_relative/Cov_Dynamics_Rel_harmonics.m
index 5baafbd9a1c6590c2a0cc12426ad7b1ff856a032..031191129ded8e8ab1894d85cb2dc6a18519c8fe 100644
--- a/New Model/Utilities/Cov_Dynamics_Rel_harmonics.m
+++ b/New Model/Utilities_relative/Cov_Dynamics_Rel_harmonics.m
@@ -97,8 +97,26 @@ function dx = Cov_Dynamics_Rel_harmonics(~,x,pars,~)
% The gravitational effect of Phobos on MMX includes the Ellipsoid
% harmonics
- % VA TIRATA FUORI pars.SH dai sigma points!!
- [gPh,~,~,~] = SHGM_Full(rsb, pars.SH);
+ pars.SH_sigma = pars.SH;
+ pars.SH_sigma.Clm = pars.SH.Clm;
+ pars.SH_sigma.Slm = pars.SH.Slm;
+ count = 0;
+
+ for j=1:size(pars.SH.Clm,1)
+ for k=1:j
+ count = count+1;
+ pars.SH_sigma.Clm(j,k) = x(10+count+i*size_St);
+ end
+ end
+
+ for j=2:size(pars.SH.Clm,1)
+ for k=2:j
+ count = count+1;
+ pars.SH_sigma.Slm(j,k) = x(10+count+i*size_St);
+ end
+ end
+
+ [gPh,~,~,~] = SHGM_Full(rsb, pars.SH_sigma);
% Combined effect on MMX
theta_dot = (pars.K - IPhz_bar*Xsi_dot)/Iz;
diff --git a/New Model/Utilities/Deimos_Pic_relative.m b/New Model/Utilities_relative/Deimos_Pic_relative.m
similarity index 100%
rename from New Model/Utilities/Deimos_Pic_relative.m
rename to New Model/Utilities_relative/Deimos_Pic_relative.m
diff --git a/New Model/Utilities/Deimos_Pic_relative_model.m b/New Model/Utilities_relative/Deimos_Pic_relative_model.m
similarity index 100%
rename from New Model/Utilities/Deimos_Pic_relative_model.m
rename to New Model/Utilities_relative/Deimos_Pic_relative_model.m
diff --git a/New Model/Utilities/Dynamics_MPHandMMX_Relative.m b/New Model/Utilities_relative/Dynamics_MPHandMMX_Relative.m
similarity index 100%
rename from New Model/Utilities/Dynamics_MPHandMMX_Relative.m
rename to New Model/Utilities_relative/Dynamics_MPHandMMX_Relative.m
diff --git a/New Model/Utilities/Dynamics_MPHandMMX_Relative_harmonics.m b/New Model/Utilities_relative/Dynamics_MPHandMMX_Relative_harmonics.m
similarity index 100%
rename from New Model/Utilities/Dynamics_MPHandMMX_Relative_harmonics.m
rename to New Model/Utilities_relative/Dynamics_MPHandMMX_Relative_harmonics.m
diff --git a/New Model/Utilities/Dynamics_MPHandMMX_Relative_ridotta.m b/New Model/Utilities_relative/Dynamics_MPHandMMX_Relative_ridotta.m
similarity index 100%
rename from New Model/Utilities/Dynamics_MPHandMMX_Relative_ridotta.m
rename to New Model/Utilities_relative/Dynamics_MPHandMMX_Relative_ridotta.m
diff --git a/New Model/Utilities/Dynamics_MPHandMMX_Relative_tests.m b/New Model/Utilities_relative/Dynamics_MPHandMMX_Relative_tests.m
similarity index 100%
rename from New Model/Utilities/Dynamics_MPHandMMX_Relative_tests.m
rename to New Model/Utilities_relative/Dynamics_MPHandMMX_Relative_tests.m
diff --git a/New Model/Utilities/MARPHOFRZ.txt b/New Model/Utilities_relative/MARPHOFRZ.txt
similarity index 100%
rename from New Model/Utilities/MARPHOFRZ.txt
rename to New Model/Utilities_relative/MARPHOFRZ.txt
diff --git a/New Model/Utilities/MMX_GenerateBSPFiles_Relative.m b/New Model/Utilities_relative/MMX_GenerateBSPFiles_Relative.m
similarity index 93%
rename from New Model/Utilities/MMX_GenerateBSPFiles_Relative.m
rename to New Model/Utilities_relative/MMX_GenerateBSPFiles_Relative.m
index 1240fd7178cc8b83b36f027a2ddddec7ea19328a..084370bd76774408e5bc7ff0bf9152b5e6d1543a 100644
--- a/New Model/Utilities/MMX_GenerateBSPFiles_Relative.m
+++ b/New Model/Utilities_relative/MMX_GenerateBSPFiles_Relative.m
@@ -22,7 +22,7 @@ function MMX_GenerateBSPFiles_Relative(t, Xmmx, OrbitType, Amplitudes, PropTime,
if(exist(bspfilename,'file')~=2)
ID = -35;
MMX_MakeBSPFile_GravLib(bspfilename, ID, t, Xmmx, '-401', 'IAU_PHOBOS', Model, [micePATH, '/exe/']);
- movefile(bspfilename, './MMX_BSP_Files_Relative');
+ movefile(bspfilename, '../MMX_BSP_Files_Relative');
else
fprintf('MMX ephemeris file already exists!\n');
end
diff --git a/New Model/Utilities/MPH_Relative.m b/New Model/Utilities_relative/MPH_Relative.m
similarity index 100%
rename from New Model/Utilities/MPH_Relative.m
rename to New Model/Utilities_relative/MPH_Relative.m
diff --git a/New Model/Utilities/Mars_LimbRange_relative.m b/New Model/Utilities_relative/Mars_LimbRange_relative.m
similarity index 100%
rename from New Model/Utilities/Mars_LimbRange_relative.m
rename to New Model/Utilities_relative/Mars_LimbRange_relative.m
diff --git a/New Model/Utilities/Mars_LimbRange_relative_model.m b/New Model/Utilities_relative/Mars_LimbRange_relative_model.m
similarity index 100%
rename from New Model/Utilities/Mars_LimbRange_relative_model.m
rename to New Model/Utilities_relative/Mars_LimbRange_relative_model.m
diff --git a/New Model/Utilities/Observables_model_relative.m b/New Model/Utilities_relative/Observables_model_relative.m
similarity index 99%
rename from New Model/Utilities/Observables_model_relative.m
rename to New Model/Utilities_relative/Observables_model_relative.m
index 1dafddd9e1764e0dbb8dfb3973316cd88f1c1ebe..6964ee723bd88b529ccaea012f9d02845e7fb02e 100644
--- a/New Model/Utilities/Observables_model_relative.m
+++ b/New Model/Utilities_relative/Observables_model_relative.m
@@ -62,7 +62,7 @@ function [G, Rm] = Observables_model_relative(et, X, St_ID_Range, St_ID_Rate,...
r_MMX = X_MMX(1:3);
v_MMX = X_MMX(4:6);
- bias = X(d+4+pars.nCoeff:end);
+ bias = X(d+4+pars.nCoeff+1:end);
Xsi = X(9);
% Definition of Mars position WRT Earth J2000
diff --git a/New Model/Utilities/Observables_relative.m b/New Model/Utilities_relative/Observables_relative.m
similarity index 100%
rename from New Model/Utilities/Observables_relative.m
rename to New Model/Utilities_relative/Observables_relative.m
diff --git a/New Model/Utilities/Plot_InitialCondition.m b/New Model/Utilities_relative/Plot_InitialCondition.m
similarity index 100%
rename from New Model/Utilities/Plot_InitialCondition.m
rename to New Model/Utilities_relative/Plot_InitialCondition.m
diff --git a/New Model/Utilities/QSOs_Definition.m b/New Model/Utilities_relative/QSOs_Definition.m
similarity index 93%
rename from New Model/Utilities/QSOs_Definition.m
rename to New Model/Utilities_relative/QSOs_Definition.m
index c56b67c441fa059792c70332066697b3241be833..ac4fad468705def9cecb6345ba1ad6aa602b2165 100644
--- a/New Model/Utilities/QSOs_Definition.m
+++ b/New Model/Utilities_relative/QSOs_Definition.m
@@ -9,15 +9,15 @@ clc
set(0,'DefaultAxesFontSize', 16);
% Mac
- addpath('../../paper/');
- addpath('../../paper/MMX_Fcn_Miscellaneous/');
- addpath('../../paper/MMX_Product/');
- addpath('../../paper/MMX_Fcn_CovarianceAnalyses/');
- addpath('../../paper/Reference_Definition/');
- addpath(genpath('../../mice/'));
- addpath(genpath('../../generic_kernels/'));
- addpath(genpath('../../useful_functions/'));
- addpath(genpath('../../dynamical_model/'));
+ addpath(genpath('../../../paper/'));
+ addpath(genpath('../../../paper/MMX_Fcn_Miscellaneous/'));
+ addpath(genpath('../../../paper/MMX_Product/'));
+ addpath(genpath('../../../paper/MMX_Fcn_CovarianceAnalyses/'));
+ addpath(genpath('../../../paper/Reference_Definition/'));
+ addpath(genpath('../../../mice/'));
+ addpath(genpath('../../../generic_kernels/'));
+ addpath(genpath('../../../useful_functions/'));
+ addpath(genpath('../../../dynamical_model/'));
MMX_InitializeSPICE
cspice_furnsh(which('MARPHOFRZ.txt'));
cspice_furnsh(which('mar097.bsp'));
@@ -303,10 +303,6 @@ clc
%% Ottimizzazione
-% Definition of the constraints
-% There are no linear inequality constraints nor linear equality
-% constraints, but only non-linear constraints that are specified in the
-% function @Constraints
options = optimoptions('fmincon','Display','iter','Algorithm','interior-point',...
'ConstraintTolerance',1e-9,...
@@ -315,10 +311,14 @@ clc
X_good = fmincon(@(x) CostFunction(x,pars,units),X0,[],[],[],[],[],[], ...
@(x) Constraints(x,pars,units,X_patch),options);
- save('QSO-Lc_5rev_8x8.mat','X_good')
+% File da salvare
+ filename = 'QSO-Lc_5rev_8x8.mat';
+ save(filename,'X_good')
+ movefile(filename, '../Initial_cond_perBSP/');
-%% Test della nuova initial condition found
+
+%% Plot della nuova initial condition found
[~] = Plot_InitialCondition(X_good,[],[],pars,units);
diff --git a/New Model/Results_relative.m b/New Model/Utilities_relative/Results_relative.m
similarity index 81%
rename from New Model/Results_relative.m
rename to New Model/Utilities_relative/Results_relative.m
index 2b637600190b959bc2b45d34159f84b977382234..3e99598a914a9cfed8f470b75dabccab0448f7da 100644
--- a/New Model/Results_relative.m
+++ b/New Model/Utilities_relative/Results_relative.m
@@ -44,106 +44,90 @@ function Results_relative(Est, pars, units)
% Plot of the pre and post fit residuals
- % figure(1)
- % subplot(2,4,1)
- % plot(t_obs/3600,Est.pre(1,:)*units.lsf,'x','Color','b')
- % grid on;
- % hold on;
- % plot(t_obs/3600,Est.pre(3,:)*units.lsf,'x','Color','b')
- % plot(t_obs/3600,Est.pre(5,:)*units.lsf,'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')
- % xlabel('$[hour]$')
- % ylabel('$[km]$')
- % title('Range Pre-fit residual')
- % subplot(2,4,2)
- % plot(t_obs/3600,Est.pre(2,:)*units.vsf,'x','Color','r')
- % grid on;
- % hold on;
- % plot(t_obs/3600,Est.pre(4,:)*units.vsf,'x','Color','r')
- % plot(t_obs/3600,Est.pre(6,:)*units.vsf,'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')
- % xlabel('$[hour]$')
- % ylabel('$[km/s]$')
- % title('RangeRate Pre-fit residual')
- % subplot(2,4,3)
- % plot(t_obs/3600,Est.pre(7,:)*units.lsf,'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')
- % subplot(2,4,4)
- % plot(t_obs/3600,Est.pre(8,:),'x','Color','m')
- % grid on;
- % hold on;
- % plot(t_obs/3600,3*pars.ObsNoise.pixel*ones(size(Est.pre(8,:),2)),'.-','Color','m')
- % plot(t_obs/3600,-3*pars.ObsNoise.pixel*ones(size(Est.pre(8,:),2)),'.-','Color','m')
- % xlabel('$[hour]$')
- % ylabel('$[N. pixel]$')
- % title('Camera Pre-fit residual')
- %
- % subplot(2,4,5)
- % plot(t_obs/3600,Est.err(1,:)*units.lsf,'x','Color','b')
- % grid on;
- % hold on;
- % plot(t_obs/3600,Est.err(3,:)*units.lsf,'x','Color','b')
- % plot(t_obs/3600,Est.err(5,:)*units.lsf,'x','Color','b')
- % plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.err(1,:),2)),'.-','Color','b')
- % plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.err(1,:),2)),'.-','Color','b')
- % xlabel('$[hour]$')
- % ylabel('$[km]$')
- % title('Range post-fit residual')
- % subplot(2,4,6)
- % plot(t_obs/3600,Est.err(2,:)*units.vsf,'x','Color','r')
- % grid on;
- % hold on;
- % plot(t_obs/3600,Est.err(4,:)*units.vsf,'x','Color','r')
- % plot(t_obs/3600,Est.err(6,:)*units.vsf,'x','Color','r')
- % plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.err(2,:),2)),'.-','Color','r')
- % plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.err(2,:),2)),'.-','Color','r')
- % xlabel('$[hour]$')
- % ylabel('$[km/s]$')
- % title('RangeRate post-fit residual')
- % subplot(2,4,7)
- % plot(t_obs/3600,Est.err(7,:)*units.lsf,'x','Color','g')
- % grid on;
- % hold on;
- % plot(t_obs/3600,3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.err(7,:),2)),'.-','Color','g')
- % plot(t_obs/3600,-3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.err(7,:),2)),'.-','Color','g')
- % xlabel('$[hour]$')
- % ylabel('$[km]$')
- % title('Lidar post-fit residual')
- % subplot(2,4,8)
- % plot(t_obs/3600,Est.err(8,:),'x','Color','m')
- % grid on;
- % hold on;
- % plot(t_obs/3600,3*pars.ObsNoise.pixel*ones(size(Est.err(8,:),2)),'.-','Color','m')
- % plot(t_obs/3600,-3*pars.ObsNoise.pixel*ones(size(Est.err(8,:),2)),'.-','Color','m')
- % xlabel('$[hour]$')
- % ylabel('$[N. pixel]$')
- % title('Camera post-fit residual')
-
-
- % subplot(2,4,5)
- % histfit([Est.pre(1,:), Est.pre(3,:), Est.pre(5,:)])
- % xlabel('$[km]$')
- % title('Range Pre-fit residual')
- % subplot(2,4,6)
- % histfit([Est.pre(2,:), Est.pre(4,:), Est.pre(6,:)])
- % xlabel('$[km/s]$')
- % title('Range Rate Pre-fit residual')
- % subplot(2,4,7)
- % histfit(Est.pre(7,:))
- % xlabel('$[km]$')
- % title('Lidar Pre-fit residual')
- % subplot(2,4,8)
- % histfit(Est.pre(8,:))
- % xlabel('$[rad]$')
- % title('Camera Pre-fit residual')
+ figure(1)
+ subplot(2,4,1)
+ plot(t_obs/3600,Est.pre(1,:)*units.lsf,'x','Color','b')
+ grid on;
+ hold on;
+ plot(t_obs/3600,Est.pre(3,:)*units.lsf,'x','Color','b')
+ plot(t_obs/3600,Est.pre(5,:)*units.lsf,'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')
+ xlabel('$[hour]$')
+ ylabel('$[km]$')
+ title('Range Pre-fit residual')
+ subplot(2,4,2)
+ plot(t_obs/3600,Est.pre(2,:)*units.vsf,'x','Color','r')
+ grid on;
+ hold on;
+ plot(t_obs/3600,Est.pre(4,:)*units.vsf,'x','Color','r')
+ plot(t_obs/3600,Est.pre(6,:)*units.vsf,'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')
+ xlabel('$[hour]$')
+ ylabel('$[km/s]$')
+ title('RangeRate Pre-fit residual')
+ subplot(2,4,3)
+ plot(t_obs/3600,Est.pre(7,:)*units.lsf,'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')
+ subplot(2,4,4)
+ plot(t_obs/3600,Est.pre(8,:),'x','Color','m')
+ grid on;
+ hold on;
+ plot(t_obs/3600,3*pars.ObsNoise.pixel*ones(size(Est.pre(8,:),2)),'.-','Color','m')
+ plot(t_obs/3600,-3*pars.ObsNoise.pixel*ones(size(Est.pre(8,:),2)),'.-','Color','m')
+ xlabel('$[hour]$')
+ ylabel('$[N. pixel]$')
+ title('Camera Pre-fit residual')
+
+ subplot(2,4,5)
+ plot(t_obs/3600,Est.err(1,:)*units.lsf,'x','Color','b')
+ grid on;
+ hold on;
+ plot(t_obs/3600,Est.err(3,:)*units.lsf,'x','Color','b')
+ plot(t_obs/3600,Est.err(5,:)*units.lsf,'x','Color','b')
+ plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.err(1,:),2)),'.-','Color','b')
+ plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.err(1,:),2)),'.-','Color','b')
+ xlabel('$[hour]$')
+ ylabel('$[km]$')
+ title('Range post-fit residual')
+ subplot(2,4,6)
+ plot(t_obs/3600,Est.err(2,:)*units.vsf,'x','Color','r')
+ grid on;
+ hold on;
+ plot(t_obs/3600,Est.err(4,:)*units.vsf,'x','Color','r')
+ plot(t_obs/3600,Est.err(6,:)*units.vsf,'x','Color','r')
+ plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.err(2,:),2)),'.-','Color','r')
+ plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.err(2,:),2)),'.-','Color','r')
+ xlabel('$[hour]$')
+ ylabel('$[km/s]$')
+ title('RangeRate post-fit residual')
+ subplot(2,4,7)
+ plot(t_obs/3600,Est.err(7,:)*units.lsf,'x','Color','g')
+ grid on;
+ hold on;
+ plot(t_obs/3600,3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.err(7,:),2)),'.-','Color','g')
+ plot(t_obs/3600,-3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.err(7,:),2)),'.-','Color','g')
+ xlabel('$[hour]$')
+ ylabel('$[km]$')
+ title('Lidar post-fit residual')
+ subplot(2,4,8)
+ plot(t_obs/3600,Est.err(8,:),'x','Color','m')
+ grid on;
+ hold on;
+ plot(t_obs/3600,3*pars.ObsNoise.pixel*ones(size(Est.err(8,:),2)),'.-','Color','m')
+ plot(t_obs/3600,-3*pars.ObsNoise.pixel*ones(size(Est.err(8,:),2)),'.-','Color','m')
+ xlabel('$[hour]$')
+ ylabel('$[N. pixel]$')
+ title('Camera post-fit residual')
+
+
%--------------------------------------------------------------------------
diff --git a/New Model/Utilities_relative/Results_relative_harmonics.m b/New Model/Utilities_relative/Results_relative_harmonics.m
new file mode 100644
index 0000000000000000000000000000000000000000..4311ab7791e2fd25794211cda682f8776cd4985d
--- /dev/null
+++ b/New Model/Utilities_relative/Results_relative_harmonics.m
@@ -0,0 +1,541 @@
+function Results_relative_harmonics(Est, PhobosGravityField, pars, units)
+%==========================================================================
+% Results_relative_harmonics(Est, pars, units)
+%
+% 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 = Est.t;
+
+
+%--------------------------------------------------------------------------
+
+
+% Plot of the pre and post fit residuals
+
+ figure(1)
+ subplot(2,4,1)
+ plot(t_obs/3600,Est.pre(1,:)*units.lsf,'x','Color','b')
+ grid on;
+ hold on;
+ plot(t_obs/3600,Est.pre(3,:)*units.lsf,'x','Color','b')
+ plot(t_obs/3600,Est.pre(5,:)*units.lsf,'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')
+ xlabel('$[hour]$')
+ ylabel('$[km]$')
+ title('Range Pre-fit residual')
+ subplot(2,4,2)
+ plot(t_obs/3600,Est.pre(2,:)*units.vsf,'x','Color','r')
+ grid on;
+ hold on;
+ plot(t_obs/3600,Est.pre(4,:)*units.vsf,'x','Color','r')
+ plot(t_obs/3600,Est.pre(6,:)*units.vsf,'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')
+ xlabel('$[hour]$')
+ ylabel('$[km/s]$')
+ title('RangeRate Pre-fit residual')
+ subplot(2,4,3)
+ plot(t_obs/3600,Est.pre(7,:)*units.lsf,'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')
+ subplot(2,4,4)
+ plot(t_obs/3600,Est.pre(8,:),'x','Color','m')
+ grid on;
+ hold on;
+ plot(t_obs/3600,3*pars.ObsNoise.pixel*ones(size(Est.pre(8,:),2)),'.-','Color','m')
+ plot(t_obs/3600,-3*pars.ObsNoise.pixel*ones(size(Est.pre(8,:),2)),'.-','Color','m')
+ xlabel('$[hour]$')
+ ylabel('$[N. pixel]$')
+ title('Camera Pre-fit residual')
+
+ subplot(2,4,5)
+ plot(t_obs/3600,Est.err(1,:)*units.lsf,'x','Color','b')
+ grid on;
+ hold on;
+ plot(t_obs/3600,Est.err(3,:)*units.lsf,'x','Color','b')
+ plot(t_obs/3600,Est.err(5,:)*units.lsf,'x','Color','b')
+ plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.err(1,:),2)),'.-','Color','b')
+ plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.err(1,:),2)),'.-','Color','b')
+ xlabel('$[hour]$')
+ ylabel('$[km]$')
+ title('Range post-fit residual')
+ subplot(2,4,6)
+ plot(t_obs/3600,Est.err(2,:)*units.vsf,'x','Color','r')
+ grid on;
+ hold on;
+ plot(t_obs/3600,Est.err(4,:)*units.vsf,'x','Color','r')
+ plot(t_obs/3600,Est.err(6,:)*units.vsf,'x','Color','r')
+ plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.err(2,:),2)),'.-','Color','r')
+ plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.err(2,:),2)),'.-','Color','r')
+ xlabel('$[hour]$')
+ ylabel('$[km/s]$')
+ title('RangeRate post-fit residual')
+ subplot(2,4,7)
+ plot(t_obs/3600,Est.err(7,:)*units.lsf,'x','Color','g')
+ grid on;
+ hold on;
+ plot(t_obs/3600,3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.err(7,:),2)),'.-','Color','g')
+ plot(t_obs/3600,-3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.err(7,:),2)),'.-','Color','g')
+ xlabel('$[hour]$')
+ ylabel('$[km]$')
+ title('Lidar post-fit residual')
+ subplot(2,4,8)
+ plot(t_obs/3600,Est.err(8,:),'x','Color','m')
+ grid on;
+ hold on;
+ plot(t_obs/3600,3*pars.ObsNoise.pixel*ones(size(Est.err(8,:),2)),'.-','Color','m')
+ plot(t_obs/3600,-3*pars.ObsNoise.pixel*ones(size(Est.err(8,:),2)),'.-','Color','m')
+ xlabel('$[hour]$')
+ ylabel('$[N. pixel]$')
+ title('Camera post-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]$','FontSize',26)
+ ylabel('$[km]$','FontSize',26)
+ title('MMX position vector $3\sigma$ envelopes','Interpreter','latex','FontSize',30)
+ legend('$3\sigma_{x}$','$3\sigma_{y}$','$3\sigma_{z}$','$3 RMS$','Interpreter','latex','FontSize',26)
+ 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]$','FontSize',26)
+ ylabel('$[km/s]$','FontSize',26)
+ title('MMX velocity vector $3\sigma$ envelopes','Interpreter','latex','FontSize',30)
+ legend('$3\sigma_{\dot{x}}$','$3\sigma_{\dot{y}}$','$3\sigma_{\dot{z}}$','$3 RMS$','Interpreter','latex','FontSize',26)
+
+
+ P_rel_RAC = zeros(3,3,size(Est.X_t(9,:),2));
+ V_rel_RAC = zeros(3,3,size(Est.X_t(9,:),2));
+
+ for i = 1:size(Est.X_t(1,:),2)
+
+ P_rel_RAC(:,:,i) = Radial_Along_Cross_Covariance(Est.P_t(1:3,1:3,i), Est.X_t(1:6,i), pars, units);
+ V_rel_RAC(:,:,i) = Radial_Along_Cross_Covariance(Est.P_t(4:6,4:6,i), Est.X_t(1:6,i), pars, units);
+
+ end
+
+
+ figure()
+ subplot(1,2,1)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_rel_RAC(1,1,1:end-1)))),'Color','b','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_rel_RAC(2,2,1:end-1)))),'Color','r','LineWidth',1)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_rel_RAC(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 $3\sigma$ in RAC directions','Interpreter','latex','FontSize',30)
+ legend('$3\sigma_{radial}$','$3\sigma_{along}$','$3\sigma_{cross}$','$3 RMS$','Interpreter','latex','FontSize',26)
+ subplot(1,2,2)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(V_rel_RAC(1,1,1:end-1)))),'Color','b','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(V_rel_RAC(2,2,1:end-1)))),'Color','r','LineWidth',1)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(V_rel_RAC(3,3,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 position $3\sigma$ in RAC directions','Interpreter','latex','FontSize',30)
+ legend('$3\sigma_{radial}$','$3\sigma_{along}$','$3\sigma_{cross}$','$3 RMS$','Interpreter','latex','FontSize',26)
+
+
+%--------------------------------------------------------------------------
+
+
+% Phobos's states uncertainties evolution
+% Phobos cartesian covaraiance
+
+
+
+
+ if pars.flag_FILTERING == 1
+
+ Phobos_real = cspice_spkezr('-401', pars.et0+t_obs, 'MARSIAU', 'none', 'MARS');
+ err = abs(Phobos_real - X_Phobos);
+
+
+ [Ph, pars] = Phobos_States_NewModel(pars.et0,pars);
+ Ph = Ph./units.sfVec2;
+ St0 = [MMX_real(:,1)./units.sfVec; Ph; pars.I2];
+ tspan = t_obs*units.tsf;
+ RelTol = 1e-13;
+ AbsTol = 1e-16;
+ opt = odeset('RelTol',RelTol,'AbsTol',AbsTol,'event',@(t,X) landing_Phobos(t,X,pars,units));
+ [~,X] = ode113(@(t,X) Dynamics_MPHandMMX_Inertia(t,X,pars,units),tspan,St0,opt);
+
+
+ if size(Est.X_t,1) == 20
+
+ Phi_t = X(:,13:14)'.*[1; units.tsf];
+ err_Phi = abs(Phi_t - Est.X_t(11:12,:));
+
+ % 3sigma Envelopes and error
+ figure()
+ subplot(2,4,1)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(1,1:end-1),'*','Color','b','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km]$')
+ title('$x_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,2)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhXYZ(2,2,1:end-1)))),'Color','r','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(2,1:end-1),'*','Color','r','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km]$')
+ title('$y_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,3)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhXYZ(3,3,1:end-1)))),'Color','g','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(3,1:end-1),'*','Color','g','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km]$')
+ title('$z_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,4)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(11,11,1:end-1)))),'Color','m','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err_Phi(1,1:end-1),'*','Color','m','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[rad]$')
+ title('$\Phi_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,5)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhVXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(4,1:end-1),'*','Color','b','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km/s]$')
+ title('$Vx_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,6)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhVXYZ(2,2,1:end-1)))),'Color','r','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(5,1:end-1),'*','Color','r','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km/s]$')
+ title('$Vy_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,7)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhVXYZ(3,3,1:end-1)))),'Color','g','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(6,1:end-1),'*','Color','g','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km/s]$')
+ title('$Vz_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,8)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(12,12,1:end-1)))),'Color','m','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err_Phi(2,1:end-1),'*','Color','m','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[rad/s]$')
+ title('$\dot{\Phi}_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+
+ else
+
+ Phi_t = X(:,13:14)'.*[1; units.tsf];
+ err_Phi = abs(Phi_t - Est.X_t(13:14,:));
+
+ % 3sigma Envelopes and error
+ figure()
+ subplot(2,4,1)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(1,1:end-1),'*','Color','b','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km]$')
+ title('$x_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,2)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhXYZ(2,2,1:end-1)))),'Color','r','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(2,1:end-1),'*','Color','r','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km]$')
+ title('$y_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,3)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhXYZ(3,3,1:end-1)))),'Color','g','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(3,1:end-1),'*','Color','g','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km]$')
+ title('$z_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ 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)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err_Phi(1,1:end-1),'*','Color','m','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[rad]$')
+ title('$\Phi_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,5)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhVXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(4,1:end-1),'*','Color','b','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km/s]$')
+ title('$Vx_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,6)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhVXYZ(2,2,1:end-1)))),'Color','r','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(5,1:end-1),'*','Color','r','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km/s]$')
+ title('$Vy_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ subplot(2,4,7)
+ semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhVXYZ(3,3,1:end-1)))),'Color','g','LineWidth',1)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err(6,1:end-1),'*','Color','g','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[km/s]$')
+ title('$Vz_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+ 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)
+ hold on;
+ grid on;
+ semilogy(t_obs(1:end-1)/3600,err_Phi(2,1:end-1),'*','Color','m','LineWidth',1)
+ xlabel('time $[hour]$')
+ ylabel('$[rad/s]$')
+ title('$\dot{\Phi}_{Ph}$ $3\sigma$ envelope and error','FontSize',30)
+
+ end
+
+ else
+
+
+
+ % 3sigma Envelopes
+ figure()
+ subplot(2,2,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,2,2)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(9,9,1:end-1)))),'Color','m','LineWidth',1)
+ grid on
+ xlabel('time [hour]')
+ ylabel('$\Phi_{Ph}$ [rad]')
+
+ subplot(2,2,3)
+ 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,2,4)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(10,10,1:end-1)))),'Color','m','LineWidth',1)
+ grid on
+ xlabel('time [hour]')
+ ylabel('$\dot{\Phi}_{Ph}$ [rad/s]')
+
+
+
+ end
+
+
+ %--------------------------------------------------------------------------
+
+
+
+% 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}$', '$\Phi_{Ph}$','$\dot{\Phi}_{Ph}$'};
+
+
+
+%--------------------------------------------------------------------------
+
+
+% 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]$')
+
+ subplot(1,pars.nBias,4)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(place0+4,place0+4,1:end-1)))),'LineWidth',1,...
+ 'DisplayName',('$Features_x$'),'Color','m');
+ grid on;
+ hold on;
+ xlabel('time $[hour]$')
+ ylabel('$[pix]$')
+
+ subplot(1,pars.nBias,5)
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(place0+5,place0+5,1:end-1)))),'LineWidth',1,...
+ 'DisplayName',('$Features_y$'),'Color','k');
+ grid on;
+ hold on;
+ xlabel('time $[hour]$')
+ ylabel('$[pix]$')
+
+
+
+%--------------------------------------------------------------------------
+
+
+
+% Harmonics Coefficients
+
+
+ place0 = 10;
+ figure()
+ for i = 0:PhobosGravityField(1)
+ subplot(1,PhobosGravityField(1)+1,i+1)
+ for j = 0:i
+ place0 = place0+1;
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(place0,place0,1:end-1)))),'LineWidth',1,...
+ 'DisplayName',['$C_{',num2str(i),num2str(j),'}$']);
+ grid on;
+ hold on;
+ corr_label{end+1} = ['$C_{',num2str(i),num2str(j),'}$'];
+ end
+ xlabel('time $[hour]$')
+ legend('Interpreter','latex','FontSize',14)
+ end
+
+% Slm coefficients
+ figure()
+ for i = 1:PhobosGravityField(1)
+ subplot(1,PhobosGravityField(1),i)
+ for j = 1:i
+ place0 = place0+1;
+ semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(place0,place0,1:end-1)))),'LineWidth',1,...
+ 'DisplayName',['$S_{',num2str(i),num2str(j),'}$'])
+ grid on;
+ hold on;
+ corr_label{end+1} = ['$S_{',num2str(i),num2str(j),'}$'];
+ end
+ xlabel('time $[hour]$')
+ legend('Interpreter','latex','FontSize',14)
+ end
+
+
+
+%--------------------------------------------------------------------------
+
+
+% 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;
+
+
diff --git a/New Model/Utilities/landing_Phobos_relative.m b/New Model/Utilities_relative/landing_Phobos_relative.m
similarity index 100%
rename from New Model/Utilities/landing_Phobos_relative.m
rename to New Model/Utilities_relative/landing_Phobos_relative.m
diff --git a/New Model/Utilities/landing_Phobos_relative_UKF.m b/New Model/Utilities_relative/landing_Phobos_relative_UKF.m
similarity index 100%
rename from New Model/Utilities/landing_Phobos_relative_UKF.m
rename to New Model/Utilities_relative/landing_Phobos_relative_UKF.m
diff --git a/New Model/Utilities/landing_Phobos_relative_ridotto.m b/New Model/Utilities_relative/landing_Phobos_relative_ridotto.m
similarity index 100%
rename from New Model/Utilities/landing_Phobos_relative_ridotto.m
rename to New Model/Utilities_relative/landing_Phobos_relative_ridotto.m
diff --git a/New Model/Utilities/visible_features_Mars_relative.m b/New Model/Utilities_relative/visible_features_Mars_relative.m
similarity index 100%
rename from New Model/Utilities/visible_features_Mars_relative.m
rename to New Model/Utilities_relative/visible_features_Mars_relative.m
diff --git a/New Model/Utilities/visible_features_model_relative.m b/New Model/Utilities_relative/visible_features_model_relative.m
similarity index 100%
rename from New Model/Utilities/visible_features_model_relative.m
rename to New Model/Utilities_relative/visible_features_model_relative.m
diff --git a/New Model/Utilities/visible_features_relative.m b/New Model/Utilities_relative/visible_features_relative.m
similarity index 100%
rename from New Model/Utilities/visible_features_relative.m
rename to New Model/Utilities_relative/visible_features_relative.m
diff --git a/New Model/YObs_rel.mat b/New Model/YObs_rel.mat
index ad91c2825099b7059cce4c12966f30b993e14902..459e0b747d22c7e3ecfff87bd96cf5c56b8a1f81 100644
Binary files a/New Model/YObs_rel.mat and b/New Model/YObs_rel.mat differ
diff --git a/Observations_Generation.m b/Observations_Generation.m
index df30ad7593b43ac230ffb8e5643bcdc60c9469ce..267b6452f6a08bfdd260f13a23197273c41dd048 100644
--- a/Observations_Generation.m
+++ b/Observations_Generation.m
@@ -17,6 +17,7 @@ clc
addpath(genpath('../generic_kernels/'));
addpath(genpath('../paper/MMX_Fcn_CovarianceAnalyses/'));
addpath(genpath('../paper/MMX_Product/MMX_BSP_Files_GravLib/'));
+ addpath(genpath('./Utilities/'))
MMX_InitializeSPICE
cspice_furnsh(which('mar097.bsp'));
cspice_furnsh(which('MARPHOFRZ.txt'));
diff --git a/UKF_Tool.m b/UKF_Tool.m
index 031e16f0e5e4fd7fbe7104e778ef01b8f136e516..8fe4593ac9a85b2763c514c16d02893af128983f 100644
--- a/UKF_Tool.m
+++ b/UKF_Tool.m
@@ -104,9 +104,9 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
St0 = Xold(1:17);
event = @landing_Phobos;
event_UKF = @landing_Phobos_UKF;
- case 96
- St0 = Xold(1:10);
- event = @landing_Phobos_relative_ridotto;
+ case {40,96}
+ St0 = Xold(1:10);
+ event = @landing_Phobos_relative_ridotto;
event_UKF = @landing_Phobos_relative_UKF;
end
@@ -123,31 +123,19 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
% Look for the observation to process
if Measures(i).t == told
- idx = idx_old;
- switch n
- case 18
- Xmean_bar = [X(idx,:)'; 0; 0; 0; 0; 0];
- case 20
- Xmean_bar = [X(idx,1:10)'; X(idx,13:end)'; 0; 0; 0; 0; 0];
- pars.PhiM = X(idx,11);
- case 22
- Xmean_bar = [X(idx,:)'; 0; 0; 0; 0; 0];
- case 96
- Xmean_bar = [X(idx,:)'; pars.Clm; pars.Slm; 0; 0; 0; 0; 0];
- end
+ idx = idx_old;
elseif Measures(i).t > told
idx = find(round(t/units.tsf)==Measures(i).t);
- switch n
- case 18
- Xmean_bar = [X(idx,:)'; 0; 0; 0; 0; 0];
- case 20
- Xmean_bar = [X(idx,1:10)'; X(idx,13:end)'; 0; 0; 0; 0; 0];
- pars.PhiM = X(idx,11);
- case 22
- Xmean_bar = [X(idx,:)'; 0; 0; 0; 0; 0];
- case 96
- Xmean_bar = [X(idx,:)'; pars.Clm; pars.Slm; 0; 0; 0; 0; 0];
- end
+ end
+
+ switch n
+ case {18,22}
+ Xmean_bar = [X(idx,:)'; 0; 0; 0; 0; 0];
+ case 20
+ Xmean_bar = [X(idx,1:10)'; X(idx,13:end)'; 0; 0; 0; 0; 0];
+ pars.PhiM = X(idx,11);
+ case {40,96}
+ Xmean_bar = [X(idx,:)'; pars.Clm; pars.Slm; 0; 0; 0; 0; 0];
end
% Useful quantities's initialization
@@ -181,7 +169,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
end
- fprintf('\nProcesso osservazione n. : %d', i);
+ fprintf('\nOsservazione n.%d di %d', i, m);
% Process noise if needed
deltaT = (Measures(i).t-told)*units.tsf;
@@ -199,9 +187,9 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
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 96
+ case {40,96}
Gamma = [eye(q)*deltaT^2/2; eye(q)*deltaT; zeros(n-6,q)];
- Q = Gamma*Q*Gamma' + [zeros(6,n); zeros(n-6,6), 0*...
+ Q = Gamma*Q*Gamma' + [zeros(6,n); zeros(n-6,6), pars.sigmaPh^2*...
diag([deltaT^2/2, deltaT, deltaT^2/2, deltaT, zeros(1,n-10)])];
end
@@ -269,6 +257,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
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))
diff --git a/Utilities/ProjMatrix.m b/Utilities/ProjMatrix.m
index bee4dffb3d1a0e318634957e50ebeb01c6ebdb44..3d956a02261bc612ab36c331445da6ed6f7d4cc8 100644
--- a/Utilities/ProjMatrix.m
+++ b/Utilities/ProjMatrix.m
@@ -1,10 +1,7 @@
-function [M_int, M_ext, PM] = ProjMatrix(t, pars, units)
+function [M_int, M_ext, PM] = ProjMatrix(t, pars, ~)
% Camera intrinsic matrix
K = pars.camera_intrinsic;
-
-
-
% Camera's fixed reference frame has the Z versor along the radial
% direction, and the X and Y on the camera plane.