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