diff --git a/New Model/Analisys.m b/New Model/Analisys.m
index baa9d88fce8f656f3711e76e60053f84f583e212..eeb1cc406fb77eaeab23d2e0196242f3335482ab 100644
--- a/New Model/Analisys.m
+++ b/New Model/Analisys.m
@@ -60,27 +60,20 @@ clc
% Analysis initial state vector
St0 = [MMX0; Ph0(1:2); Ph0(7:8); pars.I2; pars.bias];
- % St0 = [MMX0; Ph0; pars.I2; pars.bias];
Est0.dx = zeros(size(St0,1),1);
pars.flag_FILTERING = 0;
- % Est0.dx = St0.*randn(size(St0))*1e-6;
- % pars.flag_FILTERING = 1;
Est0.X = St0 + Est0.dx;
Est0.P0 = pars.P0;
Est0.t0 = data*units.tsf;
%% Analysis
-
+% Features catalogues
file_features = 'Principal_Phobos_ogni2.mat';
file_features_Mars = 'Mars_4facets.mat';
-% Sembra non osservabile con features e LIDAR only. Come mi aspetterei...
-% Se prendi solo il lidar, gli envelopes si comportano come mi aspetterei
-% ma con l'aggiunta delle features crascha perchè non trova features
-% comuni tra i vari sigmapoints.
-
+% Analysis
pars.alpha = 1;
pars.beta = 2;
[Est] = UKF_Tool(Est0,@Dynamics_MPHandMMX_Relative_ridotta,...
@@ -88,8 +81,7 @@ clc
pars.R,YObs,pars,units,file_features,file_features_Mars);
-% Questo è l'UKF puro, funziona giocando con l'alpha! Ma non tutti gli
-% stati sono visibilissimi. Vedo il libration ma non la sua velocita'
+% Questo è l'UKF puro
% pars.alpha = 1e0;
% pars.beta = 2;
@@ -97,12 +89,7 @@ clc
% @Observables_model_with_Features,...
% pars.R,YObs,pars,units,file_features,file_features_Mars);
- % 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,file_features_Mars);
-
+
Results_relative(Est, pars, units)
% save('./Results/QSOLc_6days_Auto.mat', 'Est')
diff --git a/New Model/Analisys_harmonics.m b/New Model/Analisys_harmonics.m
new file mode 100644
index 0000000000000000000000000000000000000000..1b14fdb3956545e11c315c34463d1978879076ea
--- /dev/null
+++ b/New Model/Analisys_harmonics.m
@@ -0,0 +1,87 @@
+clear
+close all
+clc
+
+%% Setup
+
+ restoredefaultpath
+ addpath('../');
+ addpath('./MMX_BSP_Files_Relative/');
+ 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/'));
+ cspice_kclear
+ MMX_InitializeSPICE
+ cspice_furnsh('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('Phobos_826891269_828619269.bsp'));
+
+
+%% Load observations and path for syntethic pictures
+
+ load("YObs_rel.mat");
+
+
+%% Initial conditions for the analysis
+
+% Model parsameters
+ [pars, units] = MMX_DefineNewModelParametersAndUnits_Harm;
+
+% Time of the analysis
+ data = '2026-03-16 00:00:00 (UTC)';
+ data = cspice_str2et(data);
+ day = 86400;
+ pars.et0 = data;
+ [Ph,pars] = Phobos_States_NewModel(data,pars);
+
+% 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;
+ K = Iz*Ph0(4) + pars.IPhz_bar*Ph0(8);
+ pars.K = K;
+
+% Initial MMX's State vector
+ MMX0 = cspice_spkezr('-35', data, 'IAU_Phobos', 'none', '-401');
+ MMX0 = [pars.PB, zeros(3,3); zeros(3,3), pars.PB]*MMX0./units.sfVec;
+
+% Analysis initial state vector
+ St0 = [MMX0; Ph0(1:2); Ph0(7:8); pars.Clm; pars.Slm; pars.bias];
+
+ Est0.dx = zeros(size(St0,1),1);
+ pars.flag_FILTERING = 0;
+ Est0.X = St0 + Est0.dx;
+ Est0.P0 = pars.P0;
+ Est0.t0 = data*units.tsf;
+
+%% Analysis
+
+% Features catalogues
+ file_features = 'Principal_Phobos_ogni2.mat';
+ file_features_Mars = 'Mars_4facets.mat';
+
+% Analysis
+
+ pars.alpha = 1;
+ pars.beta = 2;
+ [Est] = UKF_Tool(Est0,@Dynamics_MPHandMMX_Relative_harmonics,...
+ @Cov_Dynamics_Rel_harmonics, @Observables_model_relative,...
+ pars.R,YObs,pars,units,file_features,file_features_Mars);
+
+
+ Results_relative(Est, pars, units)
+
+ % save('./Results/QSOLc_6days_Auto.mat', 'Est')
+
diff --git a/New Model/CovarianceAnalysisParameters_harmonics.m b/New Model/CovarianceAnalysisParameters_harmonics.m
new file mode 100644
index 0000000000000000000000000000000000000000..62b752597b04c85bf8d67722492a8af94b4a6745
--- /dev/null
+++ b/New Model/CovarianceAnalysisParameters_harmonics.m
@@ -0,0 +1,124 @@
+function [pars, units] = CovarianceAnalysisParameters_harmonics(pars, units)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% [pars, units] = CovarianceAnalysisParameters_harmonics(pars, units)
+%
+% This function adds some units and parameters useful to the covariance
+% analysis
+%
+% Inputs:
+% pars Structure which contains parameters
+% units Structure with units
+%
+% Outputs:
+% pars.ObsNoise Observations noises standard deviation
+% pars.ObsNoise.range Standard deviation of the relative distance measure
+% 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
+% pars.nCoeff Number of the harmonics coefficients
+% pars.P0 A-priori covariance
+% pars.sigma PN diffusion coefficient
+%
+% units.Dim Vector of normalizing units [sfVec, sfVec, ones(pars.nCoeff,1)]
+% units.CovDim Matrix of normalizing units
+%
+% Author: STAG Team
+% Date: Jun 2021
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Parameters
+
+% Observation noise
+pars.ObsNoise.range = 5e-3/units.lsf; % [km]
+pars.ObsNoise.range_rate= 5e-7/units.vsf; % [km/s]
+pars.ObsNoise.lidar = 1e-2/units.lsf; % [km]
+
+cam.f = 15; %[mm]
+cam.nPixX = 3296; %[n. pixel]
+cam.nPixY = 2472; %[n. pixel]
+cam.Res = 100;
+cam.SixeX = 36; %[mm]
+cam.SixeY = 24; %[mm]
+cam.pixARx = 1;
+cam.pixARy = 1;
+pars.camera_intrinsic = IntrinsicParameters(cam);
+pars.cam = cam;
+pars.FOV = deg2rad(45);
+pixels = 2472;
+
+pars.ObsNoise.camera = pars.FOV/pixels; % [rad/pixel]
+pars.ObsNoise.pixel = .2; % [n. pixel]
+
+
+% Useful to adimensionalize the observables
+units.Observations = [units.lsf; units.vsf; units.lsf; units.vsf; units.lsf; units.vsf; units.lsf; 1; 1];
+
+% Matrix of the observation noise
+pars.R = zeros(8,8);
+pars.R(1,1) = pars.ObsNoise.range^2;
+pars.R(2,2) = pars.ObsNoise.range_rate^2;
+pars.R(3,3) = pars.ObsNoise.range^2;
+pars.R(4,4) = pars.ObsNoise.range_rate^2;
+pars.R(5,5) = pars.ObsNoise.range^2;
+pars.R(6,6) = pars.ObsNoise.range_rate^2;
+pars.R(7,7) = pars.ObsNoise.lidar^2;
+pars.R(8,8) = pars.ObsNoise.pixel^2;
+
+% Seconds between observation
+pars.interval_Range = 3600e10; % s
+pars.interval_Range_rate = 600e10; % s
+pars.interval_lidar = 600; % s
+pars.interval_camera = 600; % s
+pars.flag_Limb = 1;
+pars.flag_features = 1;
+pars.flag_features_Mars = 0;
+pars.flag_Deimos = 0;
+
+
+pars.cutoffLIDAR = 200; % km
+pars.elevation = 80; % deg
+
+% Gravitational harmonics' parameters
+Clm = [];
+Slm = [];
+
+for i=1:size(pars.SH.Clm,1)
+ Clm = [Clm; pars.SH.Clm(i,1:i)'];
+end
+for i=2:size(pars.SH.Clm,1)
+ Slm = [Slm; pars.SH.Clm(i,2:i)'];
+end
+
+pars.Clm = Clm;
+pars.Slm = Slm;
+
+% Measurements biases
+bias = [0; 0; 0; 0; 0]...
+ ./[units.lsf; units.vsf; units.lsf; 1; 1];
+
+pars.bias = bias;
+
+% Unit vector and parameters number useful for later use
+pars.nCoeff = size(pars.Clm,1) + size(pars.Slm,1);
+pars.nBias = size(bias,1);
+units.Dim = [units.sfVec; units.lsf; units.vsf; 1; units.tsf;...
+ ones(pars.nCoeff,1); units.lsf; units.vsf;...
+ units.lsf; 1; 1];
+units.CovDim = (repmat(units.Dim, 1, length(units.Dim)).*...
+ repmat(units.Dim', length(units.Dim), 1));
+
+
+% 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);...
+ 1; 1e-3; 1; 1e-2; 1e-2]./units.Dim).^2);
+
+end
\ No newline at end of file
diff --git a/New Model/Initial_cond_perBSP/3DQSO-Lb_5rev_4x4.mat b/New Model/Initial_cond_perBSP/3DQSO-Lb_5rev_4x4.mat
new file mode 100644
index 0000000000000000000000000000000000000000..114d04be15b4dec3e7ee3f65cdd0216f1d98b20a
Binary files /dev/null and b/New Model/Initial_cond_perBSP/3DQSO-Lb_5rev_4x4.mat differ
diff --git a/New Model/Initial_cond_perBSP/3DQSO-Lc_5rev_4x4.mat b/New Model/Initial_cond_perBSP/3DQSO-Lc_5rev_4x4.mat
new file mode 100644
index 0000000000000000000000000000000000000000..fcb4f2fd36afc61361ea7c2240aab10be2f71c9d
Binary files /dev/null and b/New Model/Initial_cond_perBSP/3DQSO-Lc_5rev_4x4.mat differ
diff --git a/New Model/Initial_cond_perBSP/QSO-Lb_5rev_4x4.mat b/New Model/Initial_cond_perBSP/QSO-Lb_5rev_4x4.mat
new file mode 100644
index 0000000000000000000000000000000000000000..ad38a0da77240fbf952cc62a0538c066551ef176
Binary files /dev/null and b/New Model/Initial_cond_perBSP/QSO-Lb_5rev_4x4.mat differ
diff --git a/New Model/Initial_cond_perBSP/QSO-Lb_5rev_8x8.mat b/New Model/Initial_cond_perBSP/QSO-Lb_5rev_8x8.mat
index 0923e6116902e04753252b0b04fe3a8329d3c8a5..fd5d204c599c5ce1907f34317f7776ef314f42fd 100644
Binary files a/New Model/Initial_cond_perBSP/QSO-Lb_5rev_8x8.mat and b/New Model/Initial_cond_perBSP/QSO-Lb_5rev_8x8.mat differ
diff --git a/New Model/Initial_cond_perBSP/QSO-Lc_5rev_4x4.mat b/New Model/Initial_cond_perBSP/QSO-Lc_5rev_4x4.mat
new file mode 100644
index 0000000000000000000000000000000000000000..2b8fd7832563422eaee0c215c61f725fb73ae877
Binary files /dev/null and b/New Model/Initial_cond_perBSP/QSO-Lc_5rev_4x4.mat differ
diff --git a/New Model/Initial_cond_perBSP/QSO-Lc_5rev_8x8.mat b/New Model/Initial_cond_perBSP/QSO-Lc_5rev_8x8.mat
index 7a1d6ce989eb0b7d832245ac67e1b5c22a515e67..8172a10e1ef1f70a64f8d4740595e949cea397a5 100644
Binary files a/New Model/Initial_cond_perBSP/QSO-Lc_5rev_8x8.mat and b/New Model/Initial_cond_perBSP/QSO-Lc_5rev_8x8.mat differ
diff --git a/New Model/Initial_cond_perBSP/SwingQSO-Lb_5rev_4x4_11.mat b/New Model/Initial_cond_perBSP/SwingQSO-Lb_5rev_4x4_11.mat
new file mode 100644
index 0000000000000000000000000000000000000000..37e734c6c35b41701bc198f23926bbd2720a2894
Binary files /dev/null and b/New Model/Initial_cond_perBSP/SwingQSO-Lb_5rev_4x4_11.mat differ
diff --git a/New Model/Initial_cond_perBSP/SwingQSO-Lb_5rev_4x4_13.mat b/New Model/Initial_cond_perBSP/SwingQSO-Lb_5rev_4x4_13.mat
new file mode 100644
index 0000000000000000000000000000000000000000..02d03e2cb1814dbe736a6ee0f6b4b5cd027cbfe4
Binary files /dev/null and b/New Model/Initial_cond_perBSP/SwingQSO-Lb_5rev_4x4_13.mat differ
diff --git a/New Model/Initial_cond_perBSP/SwingQSO-Lc_5rev_4x4_11.mat b/New Model/Initial_cond_perBSP/SwingQSO-Lc_5rev_4x4_11.mat
new file mode 100644
index 0000000000000000000000000000000000000000..622072cbba3c739f6db6fa75114850980ee1bc45
Binary files /dev/null and b/New Model/Initial_cond_perBSP/SwingQSO-Lc_5rev_4x4_11.mat differ
diff --git a/New Model/Initial_cond_perBSP/SwingQSO-Lc_5rev_4x4_13.mat b/New Model/Initial_cond_perBSP/SwingQSO-Lc_5rev_4x4_13.mat
new file mode 100644
index 0000000000000000000000000000000000000000..763a3a1751dec3a3b808b66957d1f49d050a5fe5
Binary files /dev/null and b/New Model/Initial_cond_perBSP/SwingQSO-Lc_5rev_4x4_13.mat differ
diff --git a/New Model/MMX_SwingQSOM_Blender.txt b/New Model/MMX_SwingQSOM_Blender.txt
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/New Model/Observations_Generation_relative.m b/New Model/Observations_Generation_relative.m
index 725c54a556609bde21fec09fb6693157c73ae19d..4af71cfaadeaff17d45043749e0e2b0ce5e146fb 100644
--- a/New Model/Observations_Generation_relative.m
+++ b/New Model/Observations_Generation_relative.m
@@ -25,7 +25,7 @@ clc
MMX_InitializeSPICE
cspice_furnsh('MARPHOFRZ.txt');
cspice_furnsh(which('mar097.bsp'));
- cspice_furnsh(which('MMX_QSO_027_2x2_826891269_829483269_relative_right.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('Phobos_826891269_828619269.bsp'));
@@ -36,7 +36,7 @@ clc
% Time of the analysis
data = '2026-03-16 00:00:00 (UTC)';
data = cspice_str2et(data);
- Ndays = 3;
+ Ndays = 1;
t = Ndays*86400;
date_end = data+t;
diff --git a/New Model/QSO-Lb_10rev_8x8.mat b/New Model/QSO-Lb_10rev_8x8.mat
deleted file mode 100644
index 88a218ab1a27b079b7e4cd718d628d8ff683e749..0000000000000000000000000000000000000000
Binary files a/New Model/QSO-Lb_10rev_8x8.mat and /dev/null differ
diff --git a/New Model/BSP_generator.m b/New Model/Utilities/BSP_generator.m
similarity index 95%
rename from New Model/BSP_generator.m
rename to New Model/Utilities/BSP_generator.m
index abd3a8a4d5409975c723c25ada277ea1ea0c12ab..a1c1289cb49b237c5c138566a831f0a3ec1e68a5 100644
--- a/New Model/BSP_generator.m
+++ b/New Model/Utilities/BSP_generator.m
@@ -47,7 +47,7 @@ clc
% QSO-Lb
- load("QSO-Lb_10rev_8x8.mat")
+ load("QSO-Lb_5rev_8x8.mat")
% If MMX comes from the optimization
X_MMX = X_good(1:6,1);
@@ -64,7 +64,7 @@ clc
pars.K = K;
% Initial state vector
- St0 = [X_MMX; Ph0(1:2); Ph0(7:8); pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar];
+ St0 = [X_MMX; Ph0(1:2); Ph0(7:8)];
% Integration
day = 86400;
@@ -93,7 +93,7 @@ clc
system(command);
end
- MMX_GenerateBSPFiles_Relative(t, Xmmx, 'QSO', [32, 0], period_Def, Model, micePATH)
+ MMX_GenerateBSPFiles_Relative(t, Xmmx, 'QSO', [31, 0], period_Def, Model, micePATH)
%% Test risultati
diff --git a/New Model/Constraints.m b/New Model/Utilities/Constraints.m
similarity index 91%
rename from New Model/Constraints.m
rename to New Model/Utilities/Constraints.m
index 35d5b6c6df59bfe06a1f4ea6ac5047357bcf3825..84b792aa8aff9feb9ac352ccca48a51857121439 100644
--- a/New Model/Constraints.m
+++ b/New Model/Utilities/Constraints.m
@@ -29,11 +29,11 @@ function [c, ceq] = Constraints(X,pars,units,X_patch)
for i = 2:size(X,2)
- St1meno = [X(1:6,i-1); X_Ph0(1:end,i-1); pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar];
+ St1meno = [X(1:6,i-1); X_Ph0(1:end,i-1)];
tspan1meno = [0, X(end-1,i-1)];
[~,X1meno] = ode113(@(t,X) dynamics(t,X,pars,units),tspan1meno,St1meno,opt);
- St1plus = [X(1:6,i); X_Ph0(1:end,i); pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar];
+ St1plus = [X(1:6,i); X_Ph0(1:end,i)];
tspan1plus = [0, X(end-2,i)];
[~,X1plus] = ode113(@(t,X) -dynamics(t,X,pars,units),tspan1plus,St1plus,opt);
@@ -70,7 +70,7 @@ function [c, ceq] = Constraints(X,pars,units,X_patch)
end
- c = [];
+ c = Perp;
ceq = [DeltaX; times];
end
\ No newline at end of file
diff --git a/New Model/CostFunction.m b/New Model/Utilities/CostFunction.m
similarity index 85%
rename from New Model/CostFunction.m
rename to New Model/Utilities/CostFunction.m
index 8f873c20474dc90d738d39ed7c35d8e16cbd3399..6755c003692044062c8659e15b4a24498df8aaee 100644
--- a/New Model/CostFunction.m
+++ b/New Model/Utilities/CostFunction.m
@@ -22,11 +22,11 @@ function DeltaV = CostFunction(X,pars,units)
for i = 2:size(X,2)
- St1meno = [X(1:6,i-1); X_Ph0(1:end,i-1); pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar];
+ St1meno = [X(1:6,i-1); X_Ph0(1:end,i-1)];
tspan1meno = [0, X(end-1,i-1)];
[~,X1meno] = ode113(@(t,X) dynamics(t,X,pars,units),tspan1meno,St1meno,opt);
- St1plus = [X(1:6,i); X_Ph0(1:end,i); pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar];
+ St1plus = [X(1:6,i); X_Ph0(1:end,i)];
tspan1plus = [0, X(end-2,i)];
[~,X1plus] = ode113(@(t,X) -dynamics(t,X,pars,units),tspan1plus,St1plus,opt);
diff --git a/New Model/Cov_Dynamics_Rel.m b/New Model/Utilities/Cov_Dynamics_Rel.m
similarity index 99%
rename from New Model/Cov_Dynamics_Rel.m
rename to New Model/Utilities/Cov_Dynamics_Rel.m
index 4a5d1e0e9f08a665e27b726d69e8f57726a3e039..bd268404fbe770139731c6d53266a9adb72c0d01 100644
--- a/New Model/Cov_Dynamics_Rel.m
+++ b/New Model/Utilities/Cov_Dynamics_Rel.m
@@ -101,7 +101,7 @@ function dx = Cov_Dynamics_Rel(~,x,pars,~)
C_22_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);
+ [gPh,~,~,~] = SHGM(rsb, pars.SH);
% Combined effect on MMX
theta_dot = (pars.K - IPhz_bar*Xsi_dot)/Iz;
diff --git a/New Model/Utilities/Cov_Dynamics_Rel_harmonics.m b/New Model/Utilities/Cov_Dynamics_Rel_harmonics.m
new file mode 100644
index 0000000000000000000000000000000000000000..5baafbd9a1c6590c2a0cc12426ad7b1ff856a032
--- /dev/null
+++ b/New Model/Utilities/Cov_Dynamics_Rel_harmonics.m
@@ -0,0 +1,119 @@
+function dx = Cov_Dynamics_Rel_harmonics(~,x,pars,~)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% dx = Cov_Dynamics_Rel(t,x,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
+%
+% Input:
+% .t Elapsed time since epoch (-)
+% .x State Vector
+% .pars Problem Parameters
+% .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 Parameters (-)
+% .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
+
+
+%% Dimensions
+
+ d = pars.d; % Dimension of the problem
+ nCoeff = pars.nCoeff; % N. of gravitational parameters
+ nBias = pars.nBias;
+
+ size_St = (d+4+nCoeff+nBias);
+
+
+%% Useful quantities
+
+ for i = 0:2*size_St-1
+
+% Spacecraft state vector
+ rsb = x(1+i*size_St:3+i*size_St); % Pos. vector wrt central body
+ vsb = x(4+i*size_St:6+i*size_St); % Vel. vector wrt central body
+
+% Phobos states
+ RPh = x(7+i*size_St);
+ RPh_dot = x(8+i*size_St);
+ Xsi = x(9+i*size_St);
+ Xsi_dot = x(10+i*size_St);
+
+% Phobos moment of inertia
+ IPhx_bar = pars.IPhx_bar;
+ IPhy_bar = pars.IPhy_bar;
+ IPhz_bar = pars.IPhz_bar;
+
+% Position of Mars wrt Phobos
+ rPh = RPh*[cos(-Xsi); sin(-Xsi); 0];
+
+ %% 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
+
+ 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)];
+
+ gT = pars.G*pars.MMars*rPh/RPh^3;
+
+ gM = gM1 + gJ2M - gT;
+
+% 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);
+
+% Combined effect on MMX
+ theta_dot = (pars.K - IPhz_bar*Xsi_dot)/Iz;
+ theta_ddot = dVdXsi/(pars.ni*RPh^2) - 2*RPh_dot*theta_dot/RPh;
+ W = [0; 0; theta_dot + Xsi_dot];
+ W_dot = [0; 0; theta_ddot + Xsi_ddot];
+
+ g_tot = gPh + gM - cross(W_dot,rsb) - 2 * cross(W,vsb) - cross(W,cross(W,rsb));
+
+
+%% Output for the integrator
+
+ dx(1+i*size_St:size_St+i*size_St,1) = [vsb; g_tot; RPh_dot;...
+ RPh_ddot; Xsi_dot; Xsi_ddot; zeros(nCoeff,1); zeros(nBias,1)];
+
+ end
+
+end
\ No newline at end of file
diff --git a/New Model/Deimos_Pic_relative.m b/New Model/Utilities/Deimos_Pic_relative.m
similarity index 100%
rename from New Model/Deimos_Pic_relative.m
rename to New Model/Utilities/Deimos_Pic_relative.m
diff --git a/New Model/Deimos_Pic_relative_model.m b/New Model/Utilities/Deimos_Pic_relative_model.m
similarity index 100%
rename from New Model/Deimos_Pic_relative_model.m
rename to New Model/Utilities/Deimos_Pic_relative_model.m
diff --git a/New Model/Dynamics_MPHandMMX_Relative.m b/New Model/Utilities/Dynamics_MPHandMMX_Relative.m
similarity index 100%
rename from New Model/Dynamics_MPHandMMX_Relative.m
rename to New Model/Utilities/Dynamics_MPHandMMX_Relative.m
diff --git a/New Model/Dynamics_MPHandMMX_Relative_harmonics.m b/New Model/Utilities/Dynamics_MPHandMMX_Relative_harmonics.m
similarity index 93%
rename from New Model/Dynamics_MPHandMMX_Relative_harmonics.m
rename to New Model/Utilities/Dynamics_MPHandMMX_Relative_harmonics.m
index 2d37521b7d0dca5314c16f94832e0cf335a5080e..ded782e50c2f29279c236d0c63455514fe8dfaaa 100644
--- a/New Model/Dynamics_MPHandMMX_Relative_harmonics.m
+++ b/New Model/Utilities/Dynamics_MPHandMMX_Relative_harmonics.m
@@ -48,9 +48,9 @@ function dx = Dynamics_MPHandMMX_Relative_harmonics(~,x,pars,~)
Xsi = x(9);
Xsi_dot = x(10);
- IPhx_bar = x(11);
- IPhy_bar = x(12);
- IPhz_bar = x(13);
+ IPhx_bar = pars.IPhx_bar;
+ IPhy_bar = pars.IPhy_bar;
+ IPhz_bar = pars.IPhz_bar;
% Position of Mars wrt Phobos
rPh = RPh*[cos(-Xsi); sin(-Xsi); 0];
@@ -103,7 +103,8 @@ function dx = Dynamics_MPHandMMX_Relative_harmonics(~,x,pars,~)
%% Output for the integrator
- dx = [vsb; g_tot; RPh_dot; RPh_ddot; Xsi_dot; Xsi_ddot; 0; 0; 0];
+ % dx = [vsb; g_tot; RPh_dot; RPh_ddot; Xsi_dot; Xsi_ddot; 0; 0; 0];
+ dx = [vsb; g_tot; RPh_dot; RPh_ddot; Xsi_dot; Xsi_ddot];
end
\ No newline at end of file
diff --git a/New Model/Dynamics_MPHandMMX_Relative_ridotta.m b/New Model/Utilities/Dynamics_MPHandMMX_Relative_ridotta.m
similarity index 100%
rename from New Model/Dynamics_MPHandMMX_Relative_ridotta.m
rename to New Model/Utilities/Dynamics_MPHandMMX_Relative_ridotta.m
diff --git a/New Model/Dynamics_MPHandMMX_Relative_tests.m b/New Model/Utilities/Dynamics_MPHandMMX_Relative_tests.m
similarity index 100%
rename from New Model/Dynamics_MPHandMMX_Relative_tests.m
rename to New Model/Utilities/Dynamics_MPHandMMX_Relative_tests.m
diff --git a/New Model/MARPHOFRZ.txt b/New Model/Utilities/MARPHOFRZ.txt
similarity index 100%
rename from New Model/MARPHOFRZ.txt
rename to New Model/Utilities/MARPHOFRZ.txt
diff --git a/New Model/MMX_GenerateBSPFiles_Relative.m b/New Model/Utilities/MMX_GenerateBSPFiles_Relative.m
similarity index 100%
rename from New Model/MMX_GenerateBSPFiles_Relative.m
rename to New Model/Utilities/MMX_GenerateBSPFiles_Relative.m
diff --git a/New Model/MPH_Relative.m b/New Model/Utilities/MPH_Relative.m
similarity index 100%
rename from New Model/MPH_Relative.m
rename to New Model/Utilities/MPH_Relative.m
diff --git a/New Model/Mars_LimbRange_relative.m b/New Model/Utilities/Mars_LimbRange_relative.m
similarity index 100%
rename from New Model/Mars_LimbRange_relative.m
rename to New Model/Utilities/Mars_LimbRange_relative.m
diff --git a/New Model/Mars_LimbRange_relative_model.m b/New Model/Utilities/Mars_LimbRange_relative_model.m
similarity index 100%
rename from New Model/Mars_LimbRange_relative_model.m
rename to New Model/Utilities/Mars_LimbRange_relative_model.m
diff --git a/New Model/Observables_model_relative.m b/New Model/Utilities/Observables_model_relative.m
similarity index 99%
rename from New Model/Observables_model_relative.m
rename to New Model/Utilities/Observables_model_relative.m
index a30f9e5f818c768155ef15207fc0e4b5b04de6a6..1dafddd9e1764e0dbb8dfb3973316cd88f1c1ebe 100644
--- a/New Model/Observables_model_relative.m
+++ b/New Model/Utilities/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(14:end);
+ bias = X(d+4+pars.nCoeff:end);
Xsi = X(9);
% Definition of Mars position WRT Earth J2000
diff --git a/New Model/Observables_relative.m b/New Model/Utilities/Observables_relative.m
similarity index 100%
rename from New Model/Observables_relative.m
rename to New Model/Utilities/Observables_relative.m
diff --git a/New Model/Plot_InitialCondition.m b/New Model/Utilities/Plot_InitialCondition.m
similarity index 70%
rename from New Model/Plot_InitialCondition.m
rename to New Model/Utilities/Plot_InitialCondition.m
index a13349d85f3bc562fdc4c9c4c76cf566a649379f..3e53f0a624e54d6174698cf372fc5d3194df7822 100644
--- a/New Model/Plot_InitialCondition.m
+++ b/New Model/Utilities/Plot_InitialCondition.m
@@ -21,17 +21,13 @@ function stop = Plot_InitialCondition(x,~,~,pars,units)
for j = 1:size(x,2)
- X_0opt = [x(1:6,j); X_Ph0(1:4,j); pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar];
+ X_0opt = [x(1:6,j); X_Ph0(1:4,j)];
tspanp = [0,x(8,j)];
tspanm = [0,x(7,j)];
opt = odeset('RelTol',RelTol,'AbsTol',AbsTol,'event',@(t,X) landing_Phobos(t,X,pars,units));
[~,Xp] = ode113(@(t,X) dynamics(t,X,pars,units),tspanp,X_0opt,opt);
[~,Xm] = ode113(@(t,X) -dynamics(t,X,pars,units),tspanm,X_0opt,opt);
- XPhp = Xp(:,7:10);
- XPhm = Xm(:,7:10);
-
- X_PhobosMARSIAU = zeros(3,size(Xp,1)+size(Xm,1));
X_QSO = zeros(3,size(Xp,1)+size(Xm,1));
% Braccio in avanti dal patch point
@@ -59,19 +55,7 @@ function stop = Plot_InitialCondition(x,~,~,pars,units)
end
-% figure(11)
-% plot3(Xp(:,1)*units.lsf,Xp(:,2)*units.lsf,Xp(:,3)*units.lsf);
-% hold on;
-% grid on;
-% axis equal
-% plot3(Xm(:,1)*units.lsf,Xm(:,2)*units.lsf,Xm(:,3)*units.lsf);
-% plot3(x(1,j)*units.lsf,x(2,j)*units.lsf,x(3,j)*units.lsf,'r*');
-% % plot3(X_PhobosMARSIAU(1,:),X_PhobosMARSIAU(2,:),X_PhobosMARSIAU(3,:))
-% planetPlot('Mars',[0;0;0],1);
-% xlabel('x [km]');
-% ylabel('y [km]');
-% zlabel('z [km]');
-% legend('MMX','Patch points','Phobos');
+
figure(10)
plot3(X_QSO(1,:)*units.lsf,X_QSO(2,:)*units.lsf,X_QSO(3,:)*units.lsf);
hold on;
diff --git a/New Model/QSOs_Definition.m b/New Model/Utilities/QSOs_Definition.m
similarity index 89%
rename from New Model/QSOs_Definition.m
rename to New Model/Utilities/QSOs_Definition.m
index 7c7a880599ea68e91bfea0405766156f8c375b99..c56b67c441fa059792c70332066697b3241be833 100644
--- a/New Model/QSOs_Definition.m
+++ b/New Model/Utilities/QSOs_Definition.m
@@ -47,25 +47,25 @@ clc
%% Da runnare una sola volta
-% r0 = [100;0;0]/L; % km
-% V0 = [0;-2*r0(1);0]; % km/s
-%
-% par.ds = 1e-3; % user-defined step-length parameter
-% par.dsMax = 1e-1; % maximum value for step-length parameter...see line 94
-% par.Iter = 15; % Max. no. of Newton's Method iterations
-% par.Nmax = 170; % No. of desired orbits
-% par.Opt = 5; % Optimal no. of Newton's Iteration - Useful for adaptive step-length...see line 94
-% par.Tol = 1e-10; % Newton's method tolerance
-%
-% X0 = [r0;V0];
-% T0 = 2*pi;
-% dX0_tilde_ds = [-1;0;0;0;2;0]/sqrt(5);
-% dT_tilde_ds = 0;
-% dlam_tilde_ds = 0;
-%
-% [Xpo,Tpo,Lampo,p,q,Lam,bif] = Predictor_Corrector(X0,T0,@HillProb_Sphere_Nd,@landing_Nd,dX0_tilde_ds,dT_tilde_ds,dlam_tilde_ds,Phobos,par);
-% save('Xpo','Xpo')
-% save('Tpo','Tpo')
+ % r0 = [100;0;0]/L; % km
+ % V0 = [0;-2*r0(1);0]; % km/s
+ %
+ % par.ds = 1e-3; % user-defined step-length parameter
+ % par.dsMax = 1e-1; % maximum value for step-length parameter...see line 94
+ % par.Iter = 15; % Max. no. of Newton's Method iterations
+ % par.Nmax = 170; % No. of desired orbits
+ % par.Opt = 5; % Optimal no. of Newton's Iteration - Useful for adaptive step-length...see line 94
+ % par.Tol = 1e-10; % Newton's method tolerance
+ %
+ % X0 = [r0;V0];
+ % T0 = 2*pi;
+ % dX0_tilde_ds = [-1;0;0;0;2;0]/sqrt(5);
+ % dT_tilde_ds = 0;
+ % dlam_tilde_ds = 0;
+ %
+ % [Xpo,Tpo,Lampo,p,q,Lam,bif] = Predictor_Corrector(X0,T0,@HillProb_Sphere_Nd,@landing_Nd,dX0_tilde_ds,dT_tilde_ds,dlam_tilde_ds,Phobos,par);
+ % save('Xpo','Xpo')
+ % save('Tpo','Tpo')
%% QSOs Patch points
@@ -73,7 +73,7 @@ clc
% Portforlio di orbite
load("Xpo.mat")
load("Tpo.mat")
- n_rev = 10;
+ n_rev = 5;
% QSO-H
% idx = 1;
@@ -85,10 +85,10 @@ clc
% idx = 79;
% % QSO-Lb
- idx = 86;
+ % idx = 86;
% QSO-Lc
- % idx = 90;
+ idx = 90;
% Definition patch points
Phi0 = eye(6);
@@ -156,9 +156,9 @@ clc
% % QSO-La
% % inp.Amplitudes = [49, 0];
% % QSO-Lb
-% inp.Amplitudes = [31, 0];
+% % inp.Amplitudes = [31, 0];
% % QSO-Lb
-% % inp.Amplitudes = [27, 0];
+% inp.Amplitudes = [27, 0];
%
%
% % Propagazione e caricamento dei dati della orbita di interesse
@@ -210,9 +210,9 @@ clc
% % QSO-La
% % inp.Amplitudes = [49, 0];
% % QSO-Lb
-% inp.Amplitudes = [31, 0];
+% % inp.Amplitudes = [31, 0];
% % QSO-Lc
-% % inp.Amplitudes = [27, 0];
+% inp.Amplitudes = [27, 0];
%
%
% % Propagazione e caricamento dei dati della orbita di interesse
@@ -221,7 +221,7 @@ clc
% load(filename, 'Xqp', 'Wqp', 'Model');
%
% % Integrazione e identificazione dei patch points
-% idx = 13;
+% idx = 11;
% Ic = Xqp(1:6,idx);
% Model.pars.T = Wqp(1,idx)*units.tsf;
% N_rev = 2.5;
@@ -315,7 +315,7 @@ clc
X_good = fmincon(@(x) CostFunction(x,pars,units),X0,[],[],[],[],[],[], ...
@(x) Constraints(x,pars,units,X_patch),options);
- save('QSO-Lb_10rev_8x8.mat','X_good')
+ save('QSO-Lc_5rev_8x8.mat','X_good')
%% Test della nuova initial condition found
diff --git a/New Model/landing_Phobos_relative.m b/New Model/Utilities/landing_Phobos_relative.m
similarity index 100%
rename from New Model/landing_Phobos_relative.m
rename to New Model/Utilities/landing_Phobos_relative.m
diff --git a/New Model/landing_Phobos_relative_UKF.m b/New Model/Utilities/landing_Phobos_relative_UKF.m
similarity index 100%
rename from New Model/landing_Phobos_relative_UKF.m
rename to New Model/Utilities/landing_Phobos_relative_UKF.m
diff --git a/New Model/Utilities/landing_Phobos_relative_ridotto.m b/New Model/Utilities/landing_Phobos_relative_ridotto.m
new file mode 100644
index 0000000000000000000000000000000000000000..ce7d59de0c0fc67fb871c94f6dad6d5ae8a60fd4
--- /dev/null
+++ b/New Model/Utilities/landing_Phobos_relative_ridotto.m
@@ -0,0 +1,26 @@
+function [value,isterminal,direction] = landing_Phobos_relative_ridotto(~,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(9)); sin(-X(9)); 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/New Model/visible_features_Mars_relative.m b/New Model/Utilities/visible_features_Mars_relative.m
similarity index 100%
rename from New Model/visible_features_Mars_relative.m
rename to New Model/Utilities/visible_features_Mars_relative.m
diff --git a/New Model/visible_features_model_relative.m b/New Model/Utilities/visible_features_model_relative.m
similarity index 100%
rename from New Model/visible_features_model_relative.m
rename to New Model/Utilities/visible_features_model_relative.m
diff --git a/New Model/visible_features_relative.m b/New Model/Utilities/visible_features_relative.m
similarity index 100%
rename from New Model/visible_features_relative.m
rename to New Model/Utilities/visible_features_relative.m
diff --git a/New Model/YObs_rel.mat b/New Model/YObs_rel.mat
index 0196083a5cd45fd09e6b09deb0cdd3776d0ea83a..ad91c2825099b7059cce4c12966f30b993e14902 100644
Binary files a/New Model/YObs_rel.mat and b/New Model/YObs_rel.mat differ
diff --git a/New Model/matlab.mat b/New Model/matlab.mat
deleted file mode 100644
index ca091c56e23cf8ae30b92a4c637b8ff0e9dd83dc..0000000000000000000000000000000000000000
Binary files a/New Model/matlab.mat and /dev/null differ
diff --git a/UKF_Tool.m b/UKF_Tool.m
index 9c007f9f58610dda18c312aeaff891fbfdc60795..031e16f0e5e4fd7fbe7104e778ef01b8f136e516 100644
--- a/UKF_Tool.m
+++ b/UKF_Tool.m
@@ -47,7 +47,8 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
% Apriori initial guess, deviation and covariance matrix.
Xold = Est0.X;
Pold = Est0.P0;
-
+ idx_old = 1;
+
% Column of the measures time.
told = Measures(1).t;
@@ -93,7 +94,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
switch n
case 18
St0 = Xold(1:13);
- event = @landing_Phobos_relative;
+ event = @landing_Phobos_relative_ridotto;
event_UKF = @landing_Phobos_relative_UKF;
case 20
St0 = pars.X0_reference(1:17);
@@ -103,6 +104,10 @@ 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;
+ event_UKF = @landing_Phobos_relative_UKF;
end
tspan = (0:min_interval:Measures(end).t)*units.tsf;
@@ -118,7 +123,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
% Look for the observation to process
if Measures(i).t == told
- idx = 1;
+ idx = idx_old;
switch n
case 18
Xmean_bar = [X(idx,:)'; 0; 0; 0; 0; 0];
@@ -127,6 +132,8 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
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
elseif Measures(i).t > told
idx = find(round(t/units.tsf)==Measures(i).t);
@@ -138,6 +145,8 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
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
@@ -190,6 +199,10 @@ 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
+ 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*...
+ diag([deltaT^2/2, deltaT, deltaT^2/2, deltaT, zeros(1,n-10)])];
end
% Covariance matrix's time update
@@ -281,14 +294,15 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units,...
K = Pxy/Py;
% MEASUREMENT UPDATE
- y = (Y_obs - Y_mean);
- Xstar = Xmean_bar + K*y;
- P = (P_bar - K*Py*K');
+ y = (Y_obs - Y_mean);
+ Xstar = Xmean_bar + K*y;
+ P = (P_bar - K*Py*K');
% Next iteration preparation
- Xold = Xmean_bar;
- told = Measures(i).t;
- Pold = P;
+ Xold = Xmean_bar;
+ told = Measures(i).t;
+ Pold = P;
+ idx_old = idx;
else
fprintf('\nTracking delle features perso...');
diff --git a/Deimos_Pic.m b/Utilities/Deimos_Pic.m
similarity index 100%
rename from Deimos_Pic.m
rename to Utilities/Deimos_Pic.m
diff --git a/Deimos_Pic_model.m b/Utilities/Deimos_Pic_model.m
similarity index 100%
rename from Deimos_Pic_model.m
rename to Utilities/Deimos_Pic_model.m
diff --git a/Mars_LimbRange.m b/Utilities/Mars_LimbRange.m
similarity index 100%
rename from Mars_LimbRange.m
rename to Utilities/Mars_LimbRange.m
diff --git a/Mars_LimbRange_model.m b/Utilities/Mars_LimbRange_model.m
similarity index 100%
rename from Mars_LimbRange_model.m
rename to Utilities/Mars_LimbRange_model.m
diff --git a/Measurement_read.m b/Utilities/Measurement_read.m
similarity index 100%
rename from Measurement_read.m
rename to Utilities/Measurement_read.m
diff --git a/Observables_model_with_Features.m b/Utilities/Observables_model_with_Features.m
similarity index 100%
rename from Observables_model_with_Features.m
rename to Utilities/Observables_model_with_Features.m
diff --git a/Observations_with_features.m b/Utilities/Observations_with_features.m
similarity index 100%
rename from Observations_with_features.m
rename to Utilities/Observations_with_features.m
diff --git a/Phobos_cartesian_covariance.m b/Utilities/Phobos_cartesian_covariance.m
similarity index 100%
rename from Phobos_cartesian_covariance.m
rename to Utilities/Phobos_cartesian_covariance.m
diff --git a/PlotComparison_Nico.m b/Utilities/PlotComparison_Nico.m
similarity index 100%
rename from PlotComparison_Nico.m
rename to Utilities/PlotComparison_Nico.m
diff --git a/PlotGeometryAndLight.m b/Utilities/PlotGeometryAndLight.m
similarity index 100%
rename from PlotGeometryAndLight.m
rename to Utilities/PlotGeometryAndLight.m
diff --git a/PlotGeometryAndLight_Mars.m b/Utilities/PlotGeometryAndLight_Mars.m
similarity index 100%
rename from PlotGeometryAndLight_Mars.m
rename to Utilities/PlotGeometryAndLight_Mars.m
diff --git a/Plotfeatures_Pic.m b/Utilities/Plotfeatures_Pic.m
similarity index 100%
rename from Plotfeatures_Pic.m
rename to Utilities/Plotfeatures_Pic.m
diff --git a/ProjMatrix.m b/Utilities/ProjMatrix.m
similarity index 100%
rename from ProjMatrix.m
rename to Utilities/ProjMatrix.m
diff --git a/Radial_Along_Cross_Covariance.m b/Utilities/Radial_Along_Cross_Covariance.m
similarity index 100%
rename from Radial_Along_Cross_Covariance.m
rename to Utilities/Radial_Along_Cross_Covariance.m
diff --git a/Residuals_withCamera.m b/Utilities/Residuals_withCamera.m
similarity index 100%
rename from Residuals_withCamera.m
rename to Utilities/Residuals_withCamera.m
diff --git a/SigmaPoints_Dynamics_Good.m b/Utilities/SigmaPoints_Dynamics_Good.m
similarity index 100%
rename from SigmaPoints_Dynamics_Good.m
rename to Utilities/SigmaPoints_Dynamics_Good.m
diff --git a/SigmaPoints_Dynamics_Good_NoPhi.m b/Utilities/SigmaPoints_Dynamics_Good_NoPhi.m
similarity index 100%
rename from SigmaPoints_Dynamics_Good_NoPhi.m
rename to Utilities/SigmaPoints_Dynamics_Good_NoPhi.m
diff --git a/Sigma_dynamics_NoPhi.m b/Utilities/Sigma_dynamics_NoPhi.m
similarity index 100%
rename from Sigma_dynamics_NoPhi.m
rename to Utilities/Sigma_dynamics_NoPhi.m
diff --git a/features_list.m b/Utilities/features_list.m
similarity index 100%
rename from features_list.m
rename to Utilities/features_list.m
diff --git a/landing_Phobos_UKF_NoPhiM.m b/Utilities/landing_Phobos_UKF_NoPhiM.m
similarity index 100%
rename from landing_Phobos_UKF_NoPhiM.m
rename to Utilities/landing_Phobos_UKF_NoPhiM.m
diff --git a/visible_features.m b/Utilities/visible_features.m
similarity index 100%
rename from visible_features.m
rename to Utilities/visible_features.m
diff --git a/visible_features_Mars.m b/Utilities/visible_features_Mars.m
similarity index 100%
rename from visible_features_Mars.m
rename to Utilities/visible_features_Mars.m
diff --git a/visible_features_Mars_model.m b/Utilities/visible_features_Mars_model.m
similarity index 100%
rename from visible_features_Mars_model.m
rename to Utilities/visible_features_Mars_model.m
diff --git a/visible_features_model.m b/Utilities/visible_features_model.m
similarity index 100%
rename from visible_features_model.m
rename to Utilities/visible_features_model.m