From 66330abfafeeab0957a5731d042d11a01bd0c883 Mon Sep 17 00:00:00 2001
From: "Ciccarelli, Edoardo (PG/R - Comp Sci & Elec Eng)"
<e.ciccarelli@surrey.ac.uk>
Date: Mon, 3 Apr 2023 11:19:34 +0100
Subject: [PATCH] Aggiunto modello con Phobos cartesiano
---
.../MPhMMX_CoupledLibration.m | 3 +-
.../MPhMMX_CoupledLibration_Cartesian.m | 160 +++++++++++
.../MPh_CoupledLibrationCheck.m | 32 +--
MMX_Fcn_CovarianceAnalyses/UKF.m | 7 +-
Model_Check/TestIntegrWithMMXInertia.m | 8 +-
Model_Check/TestIntegrWithPotential.m | 5 +-
Model_Check/TestIntegratorNew.m | 2 +-
.../TestIntegrwithMMXandCartesianPhobos.m | 252 ++++++++++++++++++
PlotFinali.m | 27 ++
9 files changed, 468 insertions(+), 28 deletions(-)
create mode 100644 MMX_Fcn_CovarianceAnalyses/MPhMMX_CoupledLibration_Cartesian.m
create mode 100644 Model_Check/TestIntegrwithMMXandCartesianPhobos.m
diff --git a/MMX_Fcn_CovarianceAnalyses/MPhMMX_CoupledLibration.m b/MMX_Fcn_CovarianceAnalyses/MPhMMX_CoupledLibration.m
index e19bec6..4d7e51c 100644
--- a/MMX_Fcn_CovarianceAnalyses/MPhMMX_CoupledLibration.m
+++ b/MMX_Fcn_CovarianceAnalyses/MPhMMX_CoupledLibration.m
@@ -136,7 +136,8 @@ function dx = MPhMMX_CoupledLibration(~,x,par,~)
dbetady = -rPh(3)*rPh(2)/((1 - rPh(3)^2/RPh^2)^(1/2)*RPh^3);
dbetadz = (1/RPh - rPh(3)^2/RPh^3)/(1 - rPh(3)^2/RPh^2)^(1/2);
- dVdtheta = 3*G/(2*norm(rPh(1:3))^5)*rPh(1:3)'*(M2*(dA1dtheta'*I1*A1 + A1'*I1*dA1dtheta) + M1*(dA2dtheta'*I2*A2 + A2'*I2*dA2dtheta))*rPh(1:3);
+ dVdtheta = 3*G/(2*norm(rPh(1:3))^5)*rPh(1:3)'*(M2*(dA1dtheta'*I1*A1 +...
+ A1'*I1*dA1dtheta) + M1*(dA2dtheta'*I2*A2 + A2'*I2*dA2dtheta))*rPh(1:3);
% Gravitational acceleration in x,y,z
g_M_on_PH = -[dRPhdx, dthetadx, dbetadx; dRPhdy, dthetady, dbetady; dRPhdz, dthetadz, dbetadz]*[dVdRPh; dVdtheta; 0];
diff --git a/MMX_Fcn_CovarianceAnalyses/MPhMMX_CoupledLibration_Cartesian.m b/MMX_Fcn_CovarianceAnalyses/MPhMMX_CoupledLibration_Cartesian.m
new file mode 100644
index 0000000..f67ecab
--- /dev/null
+++ b/MMX_Fcn_CovarianceAnalyses/MPhMMX_CoupledLibration_Cartesian.m
@@ -0,0 +1,160 @@
+function dx = MPhMMX_CoupledLibration_Cartesian(~,x,par,~)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% dx = MPhMMX_CoupledLibration_Cartesian(t,x,pars)
+%
+% 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
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Dimensions
+
+ d = par.d; % Dimension of the problem
+
+%% Useful quantities
+
+% MMX states
+
+ r = x(1:3); % Pos. vector wrt central body
+ v = x(4:6); % Pos. vector wrt central body
+ R = norm(r); % Norm of pos. vector wrt central body
+
+% Phobos states
+
+ Phi = x(13);
+ Phi_dot = x(14);
+ Xsi = x(15);
+ Xsi_dot = x(16);
+
+ r_Ph = x(7:9);
+ v_Ph = x(10:12);
+
+ RPh = norm(r_Ph);
+ h = cross(r_Ph,v_Ph);
+ theta_dot = norm(h)/RPh^2;
+ RPh_dot = dot(r_Ph,v_Ph)/RPh;
+ Ph_0 = par.MARSIAU2perifocal*r_Ph;
+ theta = atan2(Ph_0(2),Ph_0(1));
+
+% Position of MMX wrt Phobos
+ rsb = r - r_Ph;
+
+% Rotation matrix from orbit's to Phobos fixed coordinates frame
+ i = r_Ph/RPh;
+ k = par.perifocal2MARSIAU*[0; 0; 1];
+ j = cross(k,i);
+ NO = [i,j,k];
+ ON = NO';
+
+% Rotation matrix from orbit's to Phobos fixed coordinates frame
+ BO = [cos(Xsi), sin(Xsi), 0; -sin(Xsi), cos(Xsi), 0; 0, 0, 1];
+ OB = BO';
+
+%% Phobos's Motion
+
+% Potential's first order partials
+ dVdRPh = par.ni/RPh^2*(1 + 3/(2*RPh^2)*((par.IMz_bar - par.Is) -...
+ .5*par.IPhx_bar - .5*par.IPhy_bar + par.IPhz_bar + ...
+ 1.5*(par.IPhy_bar - par.IPhx_bar)*cos(2*Xsi)));
+ dVdXsi = 1.5*par.ni/RPh^3 * (par.IPhy_bar - par.IPhx_bar)*sin(2*Xsi);
+
+% Phobos equations of motions
+ Phi_ddot = - dVdXsi/(par.ni*RPh^2) + 2*RPh_dot*theta_dot/RPh;
+ Xsi_ddot = -(1 + par.ni*RPh^2/par.IPhz_bar)*dVdXsi/(par.ni*RPh^2) +...
+ + 2*RPh_dot*theta_dot/RPh;
+
+% Tutto adimensionale
+ G = par.G;
+ M2 = par.MPho;
+ M1 = par.MMars;
+
+ I1x = par.IMx_bar*M1;
+ I1y = par.IMy_bar*M1;
+ I1z = par.IMz_bar*M1;
+
+ I2x = par.IPhx_bar;
+ I2y = par.IPhy_bar;
+ I2z = par.IPhz_bar;
+
+% Need those quantities
+ Theta1 = theta+Phi;
+ Theta2 = theta+Xsi;
+
+ A1 = [cos(Theta1), sin(Theta1), 0; -sin(Theta1), cos(Theta1), 0; 0, 0, 1];
+ dA1dtheta = [-sin(Theta1), cos(Theta1), 0; -cos(Theta1), -sin(Theta1), 0; 0, 0, 0];
+ A2 = [cos(Theta2), sin(Theta2), 0; -sin(Theta2), cos(Theta2), 0; 0, 0, 1];
+ dA2dtheta = [-sin(Theta2), cos(Theta2), 0; -cos(Theta2), -sin(Theta2), 0; 0, 0, 0];
+
+ I1 = diag([I1x, I1y, I1z]);
+ I2 = diag([I2x, I2y, I2z]);
+
+% Chain rule terms
+ dRPhdx = r_Ph(1)/RPh;
+ dRPhdy = r_Ph(2)/RPh;
+ dRPhdz = r_Ph(3)/RPh;
+ dthetadx = -r_Ph(2)/(r_Ph(1)^2 + r_Ph(2)^2);
+ dthetady = r_Ph(1)/(r_Ph(1)^2 + r_Ph(2)^2);
+ dthetadz = 0;
+ dbetadx = -r_Ph(3)*r_Ph(1)/((1 - r_Ph(3)^2/RPh^2)^(1/2)*RPh^3);
+ dbetady = -r_Ph(3)*r_Ph(2)/((1 - r_Ph(3)^2/RPh^2)^(1/2)*RPh^3);
+ dbetadz = (1/RPh - r_Ph(3)^2/RPh^3)/(1 - r_Ph(3)^2/RPh^2)^(1/2);
+
+ dVdtheta = 3*G/(2*norm(r_Ph(1:3))^5)*r_Ph(1:3)'*(M2*(dA1dtheta'*I1*A1 +...
+ A1'*I1*dA1dtheta) + M1*(dA2dtheta'*I2*A2 + A2'*I2*dA2dtheta))*r_Ph(1:3);
+
+
+% Gravitational acceleration in x,y,z
+ g_M_on_PH = -[dRPhdx, dthetadx, dbetadx; dRPhdy, dthetady, dbetady; dRPhdz, dthetadz, dbetadz]*[dVdRPh; dVdtheta; 0];
+ g_Ph_on_M = g_M_on_PH*(-(M2+M1)/(M2*M1))/M1;
+
+%% MMX 2BP with Mars + 3rd body Phobos
+
+% The gravitational effect of Mars on MMX is 2BP+J2
+
+ gM = - par.G*par.MMars*r/R^3;
+ gJ2M = - 1.5*par.G*par.MMars*par.J2*par.RMmean/R^5*...
+ [(1-5*(r(3)/R)^2)*r(1); (1-5*(r(3)/R)^2)*r(2);...
+ (3-5*(r(3)/R)^2)*r(3)];
+
+ gM = gM + gJ2M;
+
+% The gravitational effect of Phobos on MMX includes the Ellipsoid
+% harmonics
+
+ [gb,~,~,~] = SHGM_Full(BO*ON*rsb, par.SH);
+ gPh = NO*OB*gb;
+
+% Combined effect on MMX
+ g_MMX = gM + gPh - g_Ph_on_M;
+
+%% Output for the integrator
+
+ dx = [v; g_MMX; v_Ph; g_M_on_PH; Phi_dot; Phi_ddot; Xsi_dot; Xsi_ddot];
+
+
+end
\ No newline at end of file
diff --git a/MMX_Fcn_CovarianceAnalyses/MPh_CoupledLibrationCheck.m b/MMX_Fcn_CovarianceAnalyses/MPh_CoupledLibrationCheck.m
index 249f2cf..a17275e 100644
--- a/MMX_Fcn_CovarianceAnalyses/MPh_CoupledLibrationCheck.m
+++ b/MMX_Fcn_CovarianceAnalyses/MPh_CoupledLibrationCheck.m
@@ -107,25 +107,25 @@ function dx = MPh_CoupledLibrationCheck(~,x,par,~)
I2_bar = diag([I2x, I2y, I2z])/M2;
% Chain rule terms
- dRPhdx = x(1)/norm(x(1:3));
- dRPhdy = x(2)/norm(x(1:3));
- dRPhdz = x(3)/norm(x(1:3));
- dthetadx = -x(2)/(x(1)^2 + x(2)^2);
- dthetady = x(1)/(x(1)^2 + x(2)^2);
- dthetadz = 0;
- dbetadx = -x(3)*x(1)/((1 - x(3)^2/norm(x(1:3))^2)^(1/2)*norm(x(1:3))^3);
- dbetady = -x(3)*x(2)/((1 - x(3)^2/norm(x(1:3))^2)^(1/2)*norm(x(1:3))^3);
- dbetadz = (1/norm(x(1:3)) - x(3)^2/norm(x(1:3))^3)/(1 - x(3)^2/norm(x(1:3))^2)^(1/2);
-
-% dRPhdx = x(1)/RPh;
-% dRPhdy = x(2)/RPh;
-% dRPhdz = x(3)/RPh;
+% dRPhdx = x(1)/norm(x(1:3));
+% dRPhdy = x(2)/norm(x(1:3));
+% dRPhdz = x(3)/norm(x(1:3));
% dthetadx = -x(2)/(x(1)^2 + x(2)^2);
% dthetady = x(1)/(x(1)^2 + x(2)^2);
% dthetadz = 0;
-% dbetadx = -x(3)*x(1)/((1 - x(3)^2/RPh^2)^(1/2)*RPh^3);
-% dbetady = -x(3)*x(2)/((1 - x(3)^2/RPh^2)^(1/2)*RPh^3);
-% dbetadz = (1/RPh - x(3)^2/RPh^3)/(1 - x(3)^2/RPh^2)^(1/2);
+% dbetadx = -x(3)*x(1)/((1 - x(3)^2/norm(x(1:3))^2)^(1/2)*norm(x(1:3))^3);
+% dbetady = -x(3)*x(2)/((1 - x(3)^2/norm(x(1:3))^2)^(1/2)*norm(x(1:3))^3);
+% dbetadz = (1/norm(x(1:3)) - x(3)^2/norm(x(1:3))^3)/(1 - x(3)^2/norm(x(1:3))^2)^(1/2);
+
+ dRPhdx = x(1)/RPh;
+ dRPhdy = x(2)/RPh;
+ dRPhdz = x(3)/RPh;
+ dthetadx = -x(2)/(x(1)^2 + x(2)^2);
+ dthetady = x(1)/(x(1)^2 + x(2)^2);
+ dthetadz = 0;
+ dbetadx = -x(3)*x(1)/((1 - x(3)^2/RPh^2)^(1/2)*RPh^3);
+ dbetady = -x(3)*x(2)/((1 - x(3)^2/RPh^2)^(1/2)*RPh^3);
+ dbetadz = (1/RPh - x(3)^2/RPh^3)/(1 - x(3)^2/RPh^2)^(1/2);
dVdtheta1 = 3*G/(2*norm(x(1:3))^5)*x(1:3)'*(M2*(dA1dtheta'*I1*A1 + A1'*I1*dA1dtheta) +...
M1*(dA2dtheta'*I2*A2 + A2'*I2*dA2dtheta))*x(1:3);
diff --git a/MMX_Fcn_CovarianceAnalyses/UKF.m b/MMX_Fcn_CovarianceAnalyses/UKF.m
index 8bed401..ebb50eb 100644
--- a/MMX_Fcn_CovarianceAnalyses/UKF.m
+++ b/MMX_Fcn_CovarianceAnalyses/UKF.m
@@ -140,7 +140,6 @@ function [Est] = UKF(Est0,f,G,O,Measures,par,units)
% Apriori quantities
Xmean_bar = sum(W0m'.*Xbar,2);
- Q = zeros(n,n);
deltaT = (tobs(i)-told)*units.tsf;
Q = par.sigma^2*eye(3);
switch n
@@ -204,9 +203,9 @@ function [Est] = UKF(Est0,f,G,O,Measures,par,units)
Pold = P;
- err(IDX,i) = (Y_obs - G(et,obs(:,i),Xold,par,units)').*units.Observations(IDX);
- prefit(IDX,i) = (Y_obs - G(et,obs(:,i),Xmean_bar,par,units)').*units.Observations(IDX);
- y_t(IDX,i) = y.*units.Observations(IDX);
+% err(IDX,i) = (Y_obs - G(et,obs(:,i),Xold,par,units)').*units.Observations(IDX);
+% prefit(IDX,i) = (Y_obs - G(et,obs(:,i),Xmean_bar,par,units)').*units.Observations(IDX);
+% y_t(IDX,i) = y.*units.Observations(IDX);
% Storing the results
diff --git a/Model_Check/TestIntegrWithMMXInertia.m b/Model_Check/TestIntegrWithMMXInertia.m
index eda2d5d..999834f 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_027_2x2_826891269_831211269.bsp'));
+ cspice_furnsh(which('MMX_QSO_049_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('MMX_SwingQSO_027_007_2x2_826891269_831211269.bsp'));
cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
% Model parameters
@@ -194,7 +194,7 @@ clc
%% File for Blender
- file = fopen('MMX_SwingQSOLa_Blender.txt','w+');
+ file = fopen('MMX_QSOLa_Blender.txt','w+');
fprintf(file,('%f, %d, %d\n'),[data, size(QSO_t1,1), 100]);
@@ -212,4 +212,4 @@ clc
end
fclose(file);
- movefile('MMX_SwingQSOLa_Blender.txt','../../computer-vision/MMX/');
\ No newline at end of file
+ movefile('MMX_QSOLa_Blender.txt','../../computer-vision/MMX/');
\ No newline at end of file
diff --git a/Model_Check/TestIntegrWithPotential.m b/Model_Check/TestIntegrWithPotential.m
index fa91792..9a55141 100644
--- a/Model_Check/TestIntegrWithPotential.m
+++ b/Model_Check/TestIntegrWithPotential.m
@@ -9,6 +9,7 @@ clc
set(0,'DefaultAxesFontSize', 16);
% Mac
+ addpath('../');
addpath('../MMX_Fcn_CovarianceAnalyses');
addpath('../../useful_functions/');
addpath(genpath('../../mice'));
@@ -16,7 +17,7 @@ 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(which('Phobos_826891269_828619269.bsp'));
cspice_furnsh('MARPHOFRZ.txt');
% Model parameters
@@ -26,7 +27,7 @@ clc
%% Integration Phobos's new model + MMX
% Initial MMX's state vector
- startdate = '2025-01-10 00:00:00 (UTC)';
+ startdate = '2026-03-16 00:00:00 (UTC)';
data = cspice_str2et(startdate);
[Ph, par] = Phobos_States_NewModel(data,par);
diff --git a/Model_Check/TestIntegratorNew.m b/Model_Check/TestIntegratorNew.m
index 18b99f5..87d4736 100644
--- a/Model_Check/TestIntegratorNew.m
+++ b/Model_Check/TestIntegratorNew.m
@@ -43,7 +43,7 @@ clc
St0 = [Ph; reshape(eye(par.d2),[par.d2^2,1])];
% Integration
- tspan = [0, 2*day]*units.tsf;
+ tspan = [0, 10*day]*units.tsf;
RelTol = 1e-13;
AbsTol = 1e-16;
opt = odeset('RelTol',RelTol,'AbsTol',AbsTol);
diff --git a/Model_Check/TestIntegrwithMMXandCartesianPhobos.m b/Model_Check/TestIntegrwithMMXandCartesianPhobos.m
new file mode 100644
index 0000000..7ff2701
--- /dev/null
+++ b/Model_Check/TestIntegrwithMMXandCartesianPhobos.m
@@ -0,0 +1,252 @@
+clear
+close all
+clc
+
+%% Test model With MMX
+
+ warning('on', 'all'); format longG;
+ set(0,'DefaultTextInterpreter','latex');
+ set(0,'DefaultAxesFontSize', 16);
+
+% Mac
+ addpath('../');
+ addpath('../MMX_Fcn_CovarianceAnalyses');
+ addpath(genpath('../../mice/'));
+ addpath(genpath('../../generic_kernels/'));
+ addpath(genpath('../../useful_functions/'));
+ addpath(genpath('../../dynamical_model/'));
+ addpath(genpath('../MMX_Product/MMX_BSP_Files_GravLib/'));
+ MMX_InitializeSPICE
+ cspice_furnsh(which('mar097.bsp'));
+ cspice_furnsh(which('MMX_QSO_031_2x2_826891269_828619269.bsp'));
+ cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
+
+% Model parameters
+ [par, units]= MMX_DefineNewModelParametersAndUnits;
+ alpha = 13.03; %km
+ beta = 11.4; %km
+ gamma = 9.14; %km
+ Phobos = struct('alpha',alpha/units.lsf,'beta',beta/units.lsf,'gamma',gamma/units.lsf);
+
+
+%% Integration Phobos's new model + MMX
+
+% Initial MMX's state vector
+ startdate = '2026-03-16 00:00:00 (UTC)';
+ data = cspice_str2et(startdate);
+ par.et0 = data;
+
+ [Ph, par] = Phobos_States_NewModel(data,par);
+
+% Initial Phobos's state vector
+ Ph0 = cspice_spkezr('-401', data, 'MARSIAU', 'none', '499')./units.sfVec;
+ Ph0 = [Ph0; Ph(5:end)./[1; units.tsf; 1; units.tsf]];
+
+% Initial MMX's State vector
+ MMX0 = cspice_spkezr('-34', data, 'MARSIAU', 'none', '499')./units.sfVec;
+
+% Initial state vector
+ St0 = [MMX0; Ph0];
+
+% Integration
+ day = 24*3600;
+ tspan = (0:150:10*day)*units.tsf;
+ RelTol = 1e-13;
+ AbsTol = 1e-16;
+ opt = odeset('RelTol',RelTol,'AbsTol',AbsTol);
+ [t,X] = ode113(@(t,X) MPhMMX_CoupledLibration_Cartesian(t,X,par,units),tspan,St0,opt);
+ t = t/units.tsf;
+
+
+%% Check
+
+ err_r_MMX = zeros(3,size(t,1));
+ err_v_MMX = zeros(3,size(t,1));
+ err_r_Ph = zeros(3,size(t,1));
+ err_v_Ph = zeros(3,size(t,1));
+
+ for i = 1:size(t,1)
+
+ time = data+t(i);
+ MMX_t = cspice_spkezr('-34', time, 'MARSIAU', 'none', '499');
+ Phobos_t = cspice_spkezr('-401', time, 'MARSIAU', 'none', '499');
+
+ err_MMX(:,i)= MMX_t-X(i,1:6)'.*units.sfVec;
+ err_Ph(:,i) = Phobos_t-X(i,7:12)'.*units.sfVec;
+
+ err_r_MMX(:,i) = err_MMX(1:3,i);
+ err_v_MMX(:,i) = err_MMX(4:6,i);
+ err_r_Ph(:,i) = err_Ph(1:3,i);
+ err_v_Ph(:,i) = err_Ph(4:6,i);
+
+ end
+
+ figure()
+ subplot(2,3,1)
+ plot(t/3600,err_r_MMX(1,:))
+ grid on
+ hold on
+ subplot(2,3,2)
+ plot(t/3600,err_r_MMX(2,:))
+ grid on
+ hold on
+ subplot(2,3,3)
+ plot(t/3600,err_r_MMX(3,:))
+ grid on
+ hold on
+ subplot(2,3,4)
+ plot(t/3600,err_v_MMX(1,:))
+ grid on
+ hold on
+ subplot(2,3,5)
+ plot(t/3600,err_v_MMX(2,:))
+ grid on
+ hold on
+ subplot(2,3,6)
+ plot(t/3600,err_v_MMX(3,:))
+ grid on
+ hold on
+
+ figure()
+ subplot(2,3,1)
+ plot(t/3600,err_r_Ph(1,:))
+ grid on
+ hold on
+ subplot(2,3,2)
+ plot(t/3600,err_r_Ph(2,:))
+ grid on
+ hold on
+ subplot(2,3,3)
+ plot(t/3600,err_r_Ph(3,:))
+ grid on
+ hold on
+ subplot(2,3,4)
+ plot(t/3600,err_v_Ph(1,:))
+ grid on
+ hold on
+ subplot(2,3,5)
+ plot(t/3600,err_v_Ph(2,:))
+ grid on
+ hold on
+ subplot(2,3,6)
+ plot(t/3600,err_v_Ph(3,:))
+ grid on
+ hold on
+
+
+% Difference wrt SPICE
+
+ r_Ph_t = zeros(1,size(t,1));
+ rdot_Ph_t = zeros(1,size(t,1));
+ theta_t = zeros(1,size(t,1));
+ thetadot_t = zeros(1,size(t,1));
+ Phi1_t = zeros(1,size(t,1));
+
+ Phi1dot_t = zeros(1,size(t,1));
+ Phi2_t = zeros(1,size(t,1));
+ Phi2dot_t = zeros(1,size(t,1));
+
+ for i = 1:size(t,1)
+
+ time = data+t(i);
+ Phobos_t = cspice_spkezr('401', time, 'MARSIAU', 'none', '499');
+ r = Phobos_t(1:3);
+ v = Phobos_t(4:6);
+ h = cross(r,v);
+
+ Ph_0 = par.MARSIAU2perifocal*r;
+ theta_t(i) = atan2(Ph_0(2),Ph_0(1));
+ thetadot_t(i) = norm(h)/norm(r)^2;
+ rdot_Ph_t(i) = dot(r,v)/norm(r);
+ r_Ph_t(i) = norm(Phobos_t(1:3));
+
+ Phobos_t_ORB = cspice_spkezr('401', time, 'MARPHOFRZ', 'none', '499');
+ er = -Phobos_t_ORB(1:3)/norm(Phobos_t_ORB(1:3));
+ eh = cross(Phobos_t_ORB(1:3),Phobos_t_ORB(4:6))/...
+ norm(cross(Phobos_t_ORB(1:3),Phobos_t_ORB(4:6)));
+ ey = cross(eh,er);
+ NO = [er,ey,eh];
+ no = [-er,cross(eh,-er),eh];
+
+ BN = cspice_sxform('MARPHOFRZ','IAU_MARS',time);
+ BO = BN(1:3,1:3)*no;
+ [Phi1_t(i),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; thetadot_t(i)];
+ Phi1dot_t(i)= (sin(ea3)*w_BO(2) + cos(ea3)*w_BO(3))/cos(ea2);
+
+ BN = cspice_sxform('MARPHOFRZ','IAU_PHOBOS',time);
+ R_In2PhPA = par.PB*BN(1:3,1:3);
+ BO = R_In2PhPA*NO;
+ [Phi2_t(i),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; thetadot_t(i)];
+ Phi2dot_t(i)= (sin(ea3)*w_BO(2) + cos(ea3)*w_BO(3))/cos(ea2);
+
+ end
+
+
+% Phi1 Spice and integration
+ figure()
+ subplot(2,4,1)
+ plot(t/3600,mod(X(:,13),2*pi));
+ grid on;
+ hold on;
+ plot(t/3600,mod(Phi1_t,2*pi));
+ ylabel('$\Phi_{M}$ [rad]','Fontsize',26)
+ xlabel('[hours]','Fontsize',26)
+ legend('Integrated','Spice','Fontsize',22)
+ subplot(2,4,5)
+ plot(t/3600,angdiff(X(:,13)',Phi1_t));
+ grid on;
+ ylabel('$\Delta \Phi_{M}$ [rad]','Fontsize',26)
+ xlabel('[hours]','Fontsize',26)
+
+% Phi1_dot Spice and integration
+ subplot(2,4,2)
+ plot(t/3600,X(:,14)*units.tsf);
+ grid on;
+ hold on;
+ plot(t/3600,Phi1dot_t);
+ ylabel('$\dot{\Phi}_{M}$ [rad/s]','Fontsize',26)
+ xlabel('[hours]','Fontsize',26)
+ legend('Integrated','Spice','Fontsize',22)
+ subplot(2,4,6)
+ plot(t/3600,X(:,14)*units.tsf-Phi1dot_t(:));
+ grid on;
+ ylabel('$\Delta \dot{\Phi}_{M}$ [rad/s]','Fontsize',26)
+ xlabel('[hours]','Fontsize',26)
+
+% Phi2 Spice and integration
+ subplot(2,4,3)
+ plot(t/3600,mod(X(:,15),2*pi));
+ grid on;
+ hold on;
+ plot(t/3600,mod(Phi2_t,2*pi));
+ ylabel('$\dot{\Phi}_{Ph}$ [rad]','Fontsize',26)
+ xlabel('[hours]','Fontsize',26)
+ legend('Integrated','Spice','Fontsize',22)
+ subplot(2,4,7)
+ plot(t/3600,angdiff(X(:,15)',Phi2_t));
+ grid on;
+ ylabel('$\Delta \dot{\Phi}_{Ph}$ [rad]','Fontsize',26)
+ xlabel('[hours]','Fontsize',26)
+
+% Phi2_dot Spice and integration
+ subplot(2,4,4)
+ plot(t/3600,X(:,16)*units.tsf);
+ grid on;
+ hold on;
+ plot(t/3600,Phi2dot_t);
+ ylabel('$\dot{\Phi}_{Ph}$ [rad/s]','Fontsize',26)
+ xlabel('[hours]','Fontsize',26)
+ legend('Integrated','Spice','Fontsize',22)
+ subplot(2,4,8)
+ plot(t/3600,X(:,16)*units.tsf-Phi2dot_t(:));
+ grid on;
+ ylabel('$\Delta \dot{\Phi}_{Ph}$ [rad/s]','Fontsize',26)
+ xlabel('[hours]','Fontsize',26)
\ No newline at end of file
diff --git a/PlotFinali.m b/PlotFinali.m
index 92b4816..28595be 100644
--- a/PlotFinali.m
+++ b/PlotFinali.m
@@ -715,3 +715,30 @@ semilogy(t_obs(1:end-1)/3600,180/pi*3.*squeeze(real(sqrt(Est3D.P_t(14,14,1:end-1
semilogy(t_obs(1:end-1)/3600,180/pi*3.*squeeze(real(sqrt(EstSwing.P_t(14,14,1:end-1)))))
xlabel('time [hour]')
ylabel('$\dot{\Phi}_{Ph}$ [deg/s]')
+
+
+
+
+
+% Correlations coefficients
+
+ corr_label = {'$x$','$y$','$z$','$\dot{x}$','$\dot{y}$','$\dot{z}$',...
+ '$R_{Ph}$','$\dot{R}_{Ph}$','$\theta_{Ph}$','$\dot{\theta}_{Ph}$',...
+ '$\Phi_{M}$','$\dot{\Phi}_{M}$','$\Phi_{Ph}$','$\dot{\Phi}_{Ph}$',...
+ '$I_{PhX}$','$I_{PhY}$','$I_{PhZ}$','$\rho$','$\dot{\rho}$','$LIDAR_b$'};
+
+ [~,corr] = readCovMatrix(real(EstSwing.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;
\ No newline at end of file
--
GitLab