From ba81e3675828e98e16107a6d7d75ab330b48bedb Mon Sep 17 00:00:00 2001
From: "Ciccarelli, Edoardo (PG/R - Comp Sci & Elec Eng)"
<e.ciccarelli@surrey.ac.uk>
Date: Mon, 20 Mar 2023 13:15:43 +0000
Subject: [PATCH] Last update
---
.../New_MMX_CovarianceAnalysisParameters.m | 4 +-
.../RealPhobos_States_NewModel.m | 60 +++++++++++++++++++
Model_Check/TestIntegrWithMMXInertia.m | 8 +--
Model_Check/TestIntegratorNew.m | 1 -
Reference_Definition/BSP_generator.m | 2 +-
Reference_Definition/BSP_generator_3D.m | 2 +-
Reference_Definition/BSP_generator_Swing.m | 12 ++--
Reference_Definition/Constraints.m | 2 +-
Reference_Definition/Constraints_3DQSO.m | 2 +-
Reference_Definition/Constraints_Swing.m | 2 +-
Reference_Definition/CostFunction.m | 2 +-
Reference_Definition/PlotInCond.m | 2 +-
.../QSO3D_ContPoints_Definition.m | 2 +-
.../QSO_ContiPoints_Definition.m | 2 +-
.../SwingQSO_ContiPoints_Definition.m | 2 +-
15 files changed, 82 insertions(+), 23 deletions(-)
create mode 100644 MMX_Fcn_CovarianceAnalyses/RealPhobos_States_NewModel.m
diff --git a/MMX_Fcn_CovarianceAnalyses/New_MMX_CovarianceAnalysisParameters.m b/MMX_Fcn_CovarianceAnalyses/New_MMX_CovarianceAnalysisParameters.m
index 33884d8..7115731 100644
--- a/MMX_Fcn_CovarianceAnalyses/New_MMX_CovarianceAnalysisParameters.m
+++ b/MMX_Fcn_CovarianceAnalyses/New_MMX_CovarianceAnalysisParameters.m
@@ -38,9 +38,9 @@ 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]
-FOV = 45*pi/180;
+pars.FOV = deg2rad(45);
pixels = 2048;
-pars.ObsNoise.camera = FOV/pixels; % [rad/pixel]
+pars.ObsNoise.camera = pars.FOV/pixels; % [rad/pixel]
% Useful to adimensionalize the observables
units.Observations = [units.lsf; units.vsf; units.lsf; units.vsf; units.lsf; units.vsf; units.lsf; 1; 1];
diff --git a/MMX_Fcn_CovarianceAnalyses/RealPhobos_States_NewModel.m b/MMX_Fcn_CovarianceAnalyses/RealPhobos_States_NewModel.m
new file mode 100644
index 0000000..f750110
--- /dev/null
+++ b/MMX_Fcn_CovarianceAnalyses/RealPhobos_States_NewModel.m
@@ -0,0 +1,60 @@
+function [Ph, par] = RealPhobos_States_NewModel(data,par)
+
+ Phobos_0 = cspice_spkezr('401', data, 'MARSIAU', 'none', '499');
+
+ r = Phobos_0(1:3);
+ v = Phobos_0(4:6);
+ h = cross(r,v);
+ theta_dot = norm(h)/norm(r)^2;
+ vr = dot(r,v)/norm(r);
+
+% For the true anomaly I need to move to the orbit's plane
+ eh = h/norm(h);
+ ey = cross(v,h)/par.mu - r/norm(r);
+ ey = ey/norm(ey);
+ Y = cross(eh,ey)/norm(cross(eh,ey));
+ par.MARSIAU2perifocal = [ey,Y,eh]';
+ par.perifocal2MARSIAU = par.MARSIAU2perifocal';
+
+ Ph_0 = par.MARSIAU2perifocal*Phobos_0(1:3);
+ theta = atan2(Ph_0(2),Ph_0(1));
+
+ Ph_0_ORB = cspice_spkezr('401', data, 'MARPHOFRZ', 'none', '499');
+ er = -Ph_0_ORB(1:3)/norm(Ph_0_ORB(1:3));
+ eh = cross(Ph_0_ORB(1:3),Ph_0_ORB(4:6))/...
+ norm(cross(Ph_0_ORB(1:3),Ph_0_ORB(4:6)));
+ ey = cross(eh,er);
+ NO = [er,ey,eh];
+ no = [-er,cross(eh,-er),eh];
+ par.no = no;
+
+% Mars's libration angle
+ BN = cspice_sxform('MARPHOFRZ','IAU_MARS',data);
+ BO = BN(1:3,1:3)*no;
+ [Phi1,ea2,ea3] = cspice_m2eul(BO,3,2,1);
+
+ Wtilde = -BN(4:6, 1:3)*BN(1:3, 1:3)';
+ w_BN = [-Wtilde(2,3); Wtilde(1,3); -Wtilde(1,2)];
+ w_BO = w_BN - BO*[0; 0; theta_dot];
+ Phi1_dot = (sin(ea3)*w_BO(2) + cos(ea3)*w_BO(3))/cos(ea2);
+
+
+% Phobos's libration angle
+ par.PB = [0.999240107370417 0.0378550984643587 0.00928436010779999
+ -0.0379006415569816 0.99927008291848 0.00477940934253335
+ -0.00909665828350325 -0.00512766070913186 0.999945477465509];
+
+ BN = cspice_sxform('MARPHOFRZ','IAU_PHOBOS',data);
+ R_In2PhPA = par.PB*BN(1:3,1:3);
+ BO = R_In2PhPA*NO;
+ [Phi2,ea2,ea3] = cspice_m2eul(BO,3,2,1);
+
+ Wtilde = -BN(4:6, 1:3)*BN(1:3, 1:3)';
+ w_BN = [-Wtilde(2,3); Wtilde(1,3); -Wtilde(1,2)];
+ w_BO = w_BN - BO*[0; 0; theta_dot];
+ Phi2_dot = (sin(ea3)*w_BO(2) + cos(ea3)*w_BO(3))/cos(ea2);
+
+% New state model
+ Ph = [norm(r); vr; theta; theta_dot; Phi1; Phi1_dot; Phi2; Phi2_dot];
+
+end
\ No newline at end of file
diff --git a/Model_Check/TestIntegrWithMMXInertia.m b/Model_Check/TestIntegrWithMMXInertia.m
index d055970..eda2d5d 100644
--- a/Model_Check/TestIntegrWithMMXInertia.m
+++ b/Model_Check/TestIntegrWithMMXInertia.m
@@ -19,9 +19,9 @@ clc
MMX_InitializeSPICE
cspice_furnsh('MARPHOFRZ.txt');
cspice_furnsh(which('mar097.bsp'));
-% cspice_furnsh(which('MMX_QSO_094_2x2_826891269_831211269.bsp'));
-% cspice_furnsh(which('MMX_3DQSO_094_009_2x2_826891269_828619269.bsp'));
- cspice_furnsh(which('MMX_SwingQSO_049_000_2x2_826891269_828619269.bsp'));
+% cspice_furnsh(which('MMX_QSO_027_2x2_826891269_831211269.bsp'));
+% cspice_furnsh(which('MMX_3DQSO_027_009_2x2_826891269_828619269.bsp'));
+ cspice_furnsh(which('MMX_SwingQSO_027_007_2x2_826891269_831211269.bsp'));
cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
% Model parameters
@@ -55,7 +55,7 @@ clc
% Integration
day = 24*3600;
- tspan = (0:100:.5*day)*units.tsf;
+ tspan = (0:100:5*day)*units.tsf;
RelTol = 1e-13;
AbsTol = 1e-16;
opt = odeset('RelTol',RelTol,'AbsTol',AbsTol,'event', @(t,X) landing_Phobos(t,X,par,units));
diff --git a/Model_Check/TestIntegratorNew.m b/Model_Check/TestIntegratorNew.m
index 593553e..18b99f5 100644
--- a/Model_Check/TestIntegratorNew.m
+++ b/Model_Check/TestIntegratorNew.m
@@ -17,7 +17,6 @@ clc
addpath(genpath('../MMX_Product/MMX_FullEphemeris_BSP_Files/'));
MMX_InitializeSPICE
cspice_furnsh(which('mar097.bsp'));
- cspice_furnsh(which('QSO_22000m_2026_03_15.bsp'));
cspice_furnsh('MARPHOFRZ.txt');
% Windows
diff --git a/Reference_Definition/BSP_generator.m b/Reference_Definition/BSP_generator.m
index ad23f81..ce74f4a 100644
--- a/Reference_Definition/BSP_generator.m
+++ b/Reference_Definition/BSP_generator.m
@@ -32,7 +32,7 @@ clc
startdate = '2026-03-16 00:00:00 (UTC)';
data = cspice_str2et(startdate);
par.et0 = data;
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
% QSO-H
% load("QSO-H_20rev.mat")
diff --git a/Reference_Definition/BSP_generator_3D.m b/Reference_Definition/BSP_generator_3D.m
index 033cc3b..acffbf4 100644
--- a/Reference_Definition/BSP_generator_3D.m
+++ b/Reference_Definition/BSP_generator_3D.m
@@ -31,7 +31,7 @@ clc
startdate = '2026-03-16 00:00:00 (UTC)';
data = cspice_str2et(startdate);
par.et0 = data;
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
% % 3DQSO-Lb
% load("3dQSO-Lb.mat")
diff --git a/Reference_Definition/BSP_generator_Swing.m b/Reference_Definition/BSP_generator_Swing.m
index fb8c7e0..fa78339 100644
--- a/Reference_Definition/BSP_generator_Swing.m
+++ b/Reference_Definition/BSP_generator_Swing.m
@@ -32,17 +32,17 @@ clc
startdate = '2026-03-16 00:00:00 (UTC)';
data = cspice_str2et(startdate);
par.et0 = data;
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
% % SwingQSO-Lb
-% load("SwingQSO-Lb.mat")
+ load("SwingQSO-Lb.mat")
% % If MMX comes from the optimization
-% X_MMX = X_good(1:6,1);
+ X_MMX = X_good(1:6,1);
% If MMX comes from a .bsp file
- cspice_furnsh(which('MMX_SwingQSO_031_011_2x2_826891269_828619269.bsp'));
- X_MMX = cspice_spkezr('-34', data, 'MARSIAU', 'none', '499')./units.sfVec;
+% cspice_furnsh(which('MMX_SwingQSO_031_011_2x2_826891269_828619269.bsp'));
+% X_MMX = cspice_spkezr('-34', data, 'MARSIAU', 'none', '499')./units.sfVec;
% Initial Phobos's state vector
Ph0 = Ph./units.sfVec2;
@@ -95,7 +95,7 @@ clc
system(command);
end
- MMX_GenerateBSPFiles_GravLib(t, Xmmx, XPh, 'SwingQSO', [31, 11], period_Def, Model, micePATH)
+ MMX_GenerateBSPFiles_GravLib(t, Xmmx, XPh, 'SwingQSO', [27, 7], period_Def, Model, micePATH)
%% Test risultati
diff --git a/Reference_Definition/Constraints.m b/Reference_Definition/Constraints.m
index e8c5303..1ee35f9 100644
--- a/Reference_Definition/Constraints.m
+++ b/Reference_Definition/Constraints.m
@@ -3,7 +3,7 @@ function [c, ceq] = Constraints(X,Geom,data,X_patch)
[par, units] = MMX_DefineNewModelParametersAndUnits;
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
Ph = Ph./units.sfVec2;
par.Geom = Geom;
diff --git a/Reference_Definition/Constraints_3DQSO.m b/Reference_Definition/Constraints_3DQSO.m
index 0709f4f..6928d29 100644
--- a/Reference_Definition/Constraints_3DQSO.m
+++ b/Reference_Definition/Constraints_3DQSO.m
@@ -2,7 +2,7 @@ function [c, ceq] = Constraints_3DQSO(X,Geom,data,X_patch_PhRot,X_patch_MAIAU)
[par, units] = MMX_DefineNewModelParametersAndUnits;
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
Ph = Ph./units.sfVec2;
par.Geom = Geom;
diff --git a/Reference_Definition/Constraints_Swing.m b/Reference_Definition/Constraints_Swing.m
index 61bb799..bea1629 100644
--- a/Reference_Definition/Constraints_Swing.m
+++ b/Reference_Definition/Constraints_Swing.m
@@ -1,7 +1,7 @@
function [c, ceq] = Constraints_Swing(X,Geom,data,X_patch)
[par, units] = MMX_DefineNewModelParametersAndUnits;
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
Ph = Ph./units.sfVec2;
par.Geom = Geom;
diff --git a/Reference_Definition/CostFunction.m b/Reference_Definition/CostFunction.m
index ff51359..e52ec2f 100644
--- a/Reference_Definition/CostFunction.m
+++ b/Reference_Definition/CostFunction.m
@@ -2,7 +2,7 @@ function DeltaV = CostFunction(X,Geom,data)
[par, units] = MMX_DefineNewModelParametersAndUnits;
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
Ph = Ph./units.sfVec2;
par.Geom = Geom;
diff --git a/Reference_Definition/PlotInCond.m b/Reference_Definition/PlotInCond.m
index 006c740..a694054 100644
--- a/Reference_Definition/PlotInCond.m
+++ b/Reference_Definition/PlotInCond.m
@@ -3,7 +3,7 @@ function stop = PlotInCond(x,~,~,Geom,data)
stop = false;
[par, units] = MMX_DefineNewModelParametersAndUnits;
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
Ph = Ph./units.sfVec2;
par.Geom = Geom;
diff --git a/Reference_Definition/QSO3D_ContPoints_Definition.m b/Reference_Definition/QSO3D_ContPoints_Definition.m
index f891f0c..8300835 100644
--- a/Reference_Definition/QSO3D_ContPoints_Definition.m
+++ b/Reference_Definition/QSO3D_ContPoints_Definition.m
@@ -105,7 +105,7 @@ clc
% Initial state
data = cspice_str2et('2026-03-16, 00:00:00 (UTC)');
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
Ph = Ph./units.sfVec2;
day = 24*3600;
diff --git a/Reference_Definition/QSO_ContiPoints_Definition.m b/Reference_Definition/QSO_ContiPoints_Definition.m
index e381e5a..c57a35c 100644
--- a/Reference_Definition/QSO_ContiPoints_Definition.m
+++ b/Reference_Definition/QSO_ContiPoints_Definition.m
@@ -138,7 +138,7 @@ clc
% Initial state
data = cspice_str2et('2026-03-16, 00:00:00 (UTC)');
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
Ph = Ph./units.sfVec2;
day = 24*3600;
diff --git a/Reference_Definition/SwingQSO_ContiPoints_Definition.m b/Reference_Definition/SwingQSO_ContiPoints_Definition.m
index 5d18156..16fd8b0 100644
--- a/Reference_Definition/SwingQSO_ContiPoints_Definition.m
+++ b/Reference_Definition/SwingQSO_ContiPoints_Definition.m
@@ -103,7 +103,7 @@ clc
% Initial state
data = cspice_str2et('2026-03-16, 00:00:00 (UTC)');
- [Ph, par] = Phobos_States_NewModel(data,par);
+ [Ph, par] = RealPhobos_States_NewModel(data,par);
Ph = Ph./units.sfVec2;
day = 24*3600;
--
GitLab