diff --git a/.DS_Store b/.DS_Store index 8c602a9effaeb0528f8725e0829ba377515ff5b1..a74eb23937f1a297326f4be205ecc327dd7de2c3 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/Analisys_With_Features.m b/Analisys_With_Features.m index 66cba209c9a7921685437a3e4b1894123fead91e..a7d3bc16fb0a1e9c4fc60597737642e0dcb7ff93 100644 --- a/Analisys_With_Features.m +++ b/Analisys_With_Features.m @@ -22,8 +22,8 @@ clc cspice_furnsh(which('mar097.bsp')); cspice_furnsh(which('MARPHOFRZ.txt')); cspice_furnsh(which('MMX_QSO_049_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('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')); @@ -34,20 +34,20 @@ clc %% Initial conditions for the analysis -% Model parameters - [par, units] = MMX_DefineNewModelParametersAndUnits; +% Model parsameters + [pars, units] = MMX_DefineNewModelParametersAndUnits; % Time of the analysis data = '2026-03-16 00:00:00 (UTC)'; data = cspice_str2et(data); day = 86400; - par.et0 = data; - [Ph,par] = Phobos_States_NewModel(data,par); + pars.et0 = data; + [Ph,pars] = Phobos_States_NewModel(data,pars); -% Covariance analysis parameters - [par, units] = CovarianceAnalysisParameters(par, units); - par.sigma = 1e-10/(units.vsf*units.tsf); - par.sigmaPh = 0/(units.vsf*units.tsf); +% Covariance analysis parsameters + [pars, units] = CovarianceAnalysisParameters(pars, units); + pars.sigma = 1e-10/(units.vsf*units.tsf); + pars.sigmaPh = 0/(units.vsf*units.tsf); % Initial Phobos's state vector Ph0 = Ph./units.sfVec2; @@ -57,42 +57,50 @@ clc MMX0 = MMX0./units.sfVec; % Analysis initial state vector - St0 = [MMX0; Ph0; par.I2; par.bias]; - - Est0.X = St0; - Est0.dx = zeros(size(St0,1),1); - Est0.P0 = par.P0; + % 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; + Est0.dx = St0*1e-6; + pars.flag_FILTERING = 1; + Est0.X = St0+Est0.dx; + Est0.P0 = pars.P0; Est0.t0 = data*units.tsf; %% Analysis - par.alpha = 1; - par.beta = 2; - par.Nfeatures = 5; - file_features = 'Principal_Phobos_Ogni10.mat'; + + file_features = 'Principal_Phobos_ogni10.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. Non solo, se aumenti il numero di -% features gli envelopes si abbassano e diventa osservabile anche la -% rotazione di Marte probabilmente perchè abbastanza risoluta la -% rotazione di Phobos +% comuni tra i vari sigmapoints. - [Est] = UKF_Tool(Est0,@Dynamics_MPHandMMX_Inertia,... - @Cov_Dynamics_Good, @Observables_model_with_Features,... - par.R,YObs,par,units,file_features); - -% Questo è l'UKF puro ma funziona solo scegliendo accuratamente la -% apriori covariance. Se troppo larga diverge!! Also, sembra avere -% problemi con il PN, funziona solo senza (ma forse anche qui va settato -% in un certo modo e non uguale su tutte le componenti del vettore stato). - -% [Est] = UKF_features(Est0, @SigmaPoints_Dynamics_Good,... -% @Observables_model_with_Features,... -% par.R,YObs,par,units,file_features); - - Results(Est, YObs, par, units) + % pars.alpha = 1; + % pars.beta = 2; + % [Est] = UKF_Tool(Est0,@Dynamics_MPHandMMX_Inertia,... + % @Cov_Dynamics_Good, @Observables_model_with_Features,... + % pars.R,YObs,pars,units,file_features); + % [Est] = UKF_Tool(Est0,@Dynamics_MPHandMMX_Inertia,... + % @Sigma_dynamics_NoPhi, @Observables_model_with_Features,... + % pars.R,YObs,pars,units,file_features); + +% 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.beta = 2; + % [Est] = UKF_features(Est0, @SigmaPoints_Dynamics_Good,... + % @Observables_model_with_Features,... + % pars.R,YObs,pars,units,file_features); + [Est] = UKF_features(Est0, @SigmaPoints_Dynamics_Good_NoPhi,... + @Observables_model_with_Features,... + pars.R,YObs,pars,units,file_features); + + Results(Est, pars, units) -% save('./Results/QSOLb_9days_AllObs_alpha8e1.mat', 'Est') + % save('./Results/QSOLc_6days_Auto.mat', 'Est') diff --git a/CovarianceAnalysisParameters.m b/CovarianceAnalysisParameters.m index a55c3880e0920a9b474557dee6095bb16e5234d3..31e7d6f34faf8fc58671fc49c4a98bee32a707d8 100644 --- a/CovarianceAnalysisParameters.m +++ b/CovarianceAnalysisParameters.m @@ -73,10 +73,10 @@ pars.R(7,7) = pars.ObsNoise.lidar^2; pars.R(8,8) = pars.ObsNoise.pixel^2; % Seconds between observation -pars.interval_Range = 600e10; % s +pars.interval_Range = 3600e10; % s pars.interval_Range_rate = 600e10; % s -pars.interval_lidar = 300; % s -pars.interval_camera = 300; % s +pars.interval_lidar = 600; % s +pars.interval_camera = 600; % s pars.flag_Limb = 1; pars.flag_features = 1; @@ -88,8 +88,6 @@ pars.elevation = 80; % deg pars.I2 = [pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar]; % Measurements biases -% bias = [0; 0; 0]... -% ./[units.lsf; units.vsf; units.lsf]; bias = [0; 0; 0; 0; 0]... ./[units.lsf; units.vsf; units.lsf; 1; 1]; @@ -98,22 +96,22 @@ pars.bias = bias; % Unit vector and parameters number useful for later use pars.nCoeff = size(pars.I2,1); pars.nBias = size(bias,1); -% units.Dim = [units.sfVec; units.lsf; units.vsf; 1; units.tsf;... -% 1; units.tsf; 1; units.tsf; ones(pars.nCoeff,1); units.lsf; units.vsf;... -% units.lsf]; units.Dim = [units.sfVec; units.lsf; units.vsf; 1; units.tsf;... - 1; units.tsf; 1; units.tsf; ones(pars.nCoeff,1); units.lsf; units.vsf;... + 1; units.tsf; ones(pars.nCoeff,1); units.lsf; units.vsf;... units.lsf; 1; 1]; +% units.Dim = [units.sfVec; units.lsf; units.vsf; 1; units.tsf;... +% 1; units.tsf; 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(([.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);... -% 1; 1e-3; 1]./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-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);... +% 1; 1e-3; 1; 1e-2; 1e-2]./units.Dim).^2); end \ No newline at end of file diff --git a/Observables_model_with_Features.m b/Observables_model_with_Features.m index 36328b0e45f4a01ca6402cace81fc3bd849cbcac..085cfc21b213024a69072700f0eb69444f4d30f8 100644 --- a/Observables_model_with_Features.m +++ b/Observables_model_with_Features.m @@ -66,7 +66,7 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat case 18 bias = X(16:end); case 20 - bias = X(18:end); + bias = X(16:end); case 22 bias = X(18:end); end @@ -141,7 +141,13 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat NO = [I,j,k]; ON = NO'; - Xsi = X(13); + switch size(X,1) + case 20 + Xsi = X(11); + case 22 + Xsi = X(13); + end + BO = [cos(Xsi), sin(Xsi), 0; -sin(Xsi), cos(Xsi), 0; 0, 0, 1]; rsb = r_MMX-r_Phobos; @@ -175,22 +181,23 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat v_Phobos = pars.perifocal2MARSIAU*(R2Rot'*([X(8); 0; 0]) + ... cross([0; 0; X(10)], [X(7)*cos(X(9)); X(7)*sin(X(9)); 0])); Phobos = [r_Phobos.*units.lsf; v_Phobos.*units.vsf]; - Xsi = X(13); switch size(X,1) case 20 - [~, Y_pix] = visible_features_model(X_MMX.*units.sfVec, Phobos, Sun, Xsi, file_features, pars, units); + Xsi = X(11); case 22 - [~, Y_pix] = visible_features_model(X_MMX.*units.sfVec, Phobos, Sun, Xsi, file_features, pars, units); - - if ~isempty(Y_pix) - Y_pix = Y_pix + repmat([bias(4); bias(5); 0], 1, size(Y_pix,2)); - else - fprintf('\nTracking delle features perso!!\n'); - return - end + Xsi = X(13); end + [~, Y_pix] = visible_features_model(X_MMX.*units.sfVec, Phobos, Sun, Xsi, file_features, pars, units); + + if ~isempty(Y_pix) + Y_pix = Y_pix + repmat([bias(4); bias(5); 0], 1, size(Y_pix,2)); + else + fprintf('\nTracking delle features perso!!\n'); + return + end + else Y_pix = []; end @@ -204,13 +211,8 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat cross([0; 0; X(10)], [X(7)*cos(X(9)); X(7)*sin(X(9)); 0])); Phobos = [r_Phobos.*units.lsf; v_Phobos.*units.vsf]; - switch size(X,1) - case 20 - Limb = Mars_LimbRange_model(X_MMX.*units.sfVec, Phobos, Sun, pars, units)/units.lsf; - case 22 - Limb = Mars_LimbRange_model(X_MMX.*units.sfVec, Phobos, Sun, pars, units); - Limb = Limb + sqrt(bias(4)^2 + bias(5)^2); - end + Limb = Mars_LimbRange_model(X_MMX.*units.sfVec, Phobos, Sun, pars, units); + Limb = Limb + sqrt(bias(4)^2 + bias(5)^2); Rm = [Rm; O(8,8)]; end diff --git a/Observations_Generation.m b/Observations_Generation.m index 6311ab3c285172f1505a2fcf16815fe807850f6e..cc94a20c3596aa8861e28dd9dbb4824b8d4e66e0 100644 --- a/Observations_Generation.m +++ b/Observations_Generation.m @@ -21,8 +21,8 @@ clc cspice_furnsh(which('mar097.bsp')); cspice_furnsh(which('MARPHOFRZ.txt')); cspice_furnsh(which('MMX_QSO_049_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('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')); %% Initial conditions per la analisi diff --git a/Observations_with_features.m b/Observations_with_features.m index 09da5fce356731f7dde8372991bc7934404eabff..9c9096886ef12dc5f8d928bee23a6e941522ed67 100644 --- a/Observations_with_features.m +++ b/Observations_with_features.m @@ -225,7 +225,6 @@ function YObs = Observations_with_features(date_0, date_end, file_features, pars else Y_LOS = []; Y_pix = []; - Limb = []; end if (rem(round(i-et0),interval_camera)==0)&&pars.flag_Limb @@ -237,6 +236,8 @@ function YObs = Observations_with_features(date_0, date_end, file_features, pars got_it = 1; end + else + Limb = []; end diff --git a/Phobos_cartesian_covariance.m b/Phobos_cartesian_covariance.m index 828f72a1ea9f31488809286bc2ea82578dd0aaab..ab7cad11c99aba41f5d609143fcf678892e24d3f 100644 --- a/Phobos_cartesian_covariance.m +++ b/Phobos_cartesian_covariance.m @@ -1,4 +1,35 @@ -function [P_PhXYZ, P_PhXYZ_MMX, P_rel, P_PhVXYZ, P_Vrel] = Phobos_cartesian_covariance(X_t, P_t, pars, ~) +function [P_PhXYZ, P_Xrel, P_PhVXYZ, P_Vrel, X_Phobos] = ... + Phobos_cartesian_covariance(X_t, P_t, pars) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% [P_PhXYZ, P_PhXYZ_MMX, P_rel, P_PhVXYZ, P_Vrel, X_Phobos] = +% Phobos_cartesian_covariance(X_t, P_t, pars, units) +% +% Calculation of the Phobos covariance in cartesian coordinates, as well as +% the MMX-Phobos relative position covariance +% +% Input: +% +% X_t Analysis state vector at different epochs +% P_t Analysis state vector's covariance matrix at different epochs +% pars Structure of parameters +% units Structure of units +% +% +% Output: +% +% P_PhXYZ Phobos position vector covariance in cartesian coordinates +% P_PhXYZ_MMX MMX-Phobos position vector correlation coefficients in cartesian coordinates +% P_rel MMX-Phobos relative position covariance +% P_PhVXYZ Phobos velocity vector covariance in cartesian coordinates +% P_Vrel MMX-Phobos relative velocity covariance +% X_Phobos Phobos state vector in cartesian coordinates +% +% +% Author: E.Ciccarelli +% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Partials delle cartesian coordinates wrt radial distance and true @@ -13,46 +44,48 @@ function [P_PhXYZ, P_PhXYZ_MMX, P_rel, P_PhVXYZ, P_Vrel] = Phobos_cartesian_cova dVdR_dot = zeros(3,size(X_t(9,:),2)); dVdtheta = zeros(3,size(X_t(9,:),2)); dVdtheta_dot = zeros(3,size(X_t(9,:),2)); + X_Phobos = zeros(6,size(X_t(9,:),2)); for i=1:size(X_t(9,:),2) - dVdR(:,i) = pars.perifocal2MARSIAU*cross([0;0;X_t(10,i)], [cos(X_t(9,i));sin(X_t(9,i));0]); + X_Phobos(1:3,i) = pars.perifocal2MARSIAU*[cos(X_t(9,i)); sin(X_t(9,i)); zeros(size(X_t(9,i)))]*X_t(7,i); + X_Phobos(4:6,i) = pars.perifocal2MARSIAU*([cos(X_t(9,i)), sin(X_t(9,i)), 0; -sin(X_t(9,i)), cos(X_t(9,i)), 0; 0, 0, 1]'*[X_t(8,i); 0; 0] +... + cross([0; 0; X_t(10,i)], [cos(X_t(9,i)); sin(X_t(9,i)); 0])*X_t(7,i)); + dVdR(:,i) = pars.perifocal2MARSIAU*cross([0; 0; X_t(10,i)], [cos(X_t(9,i)); sin(X_t(9,i)); 0]); dVdR_dot(:,i) = pars.perifocal2MARSIAU*[cos(X_t(9,i)), sin(X_t(9,i)), 0; -sin(X_t(9,i)), cos(X_t(9,i)), 0; 0, 0, 1]'*([1; 0; 0]); dVdtheta(:,i) = pars.perifocal2MARSIAU*([-sin(X_t(9,i)), cos(X_t(9,i)), 0; -cos(X_t(9,i)), -sin(X_t(9,i)), 0; 0, 0, 1]'*([X_t(8,i); 0; 0]) +... - cross([0; 0; X_t(10,i)], [cos(X_t(9,i)); sin(X_t(9,i)); 0])*X_t(7,i)); - dVdtheta_dot(:,i) = pars.perifocal2MARSIAU*cross([0; 0; 1], [X_t(7,i)*cos(X_t(9,i)); X_t(7,i)*sin(X_t(9,i)); 0]); + cross([0; 0; X_t(10,i)], [-sin(X_t(9,i)); cos(X_t(9,i)); 0])*X_t(7,i)); + dVdtheta_dot(:,i) = pars.perifocal2MARSIAU*cross([0; 0; 1], [cos(X_t(9,i)); sin(X_t(9,i)); 0])*X_t(7,i); end + % Coomposizione della covariance P_PhXYZ - P_PhXYZ = zeros(3,3,size(X_t(9,:),2)); - P_PhVXYZ = zeros(3,3,size(X_t(9,:),2)); + P_PhXYZ = zeros(6,6,size(X_t(9,:),2)); for i=1:size(X_t(9,:),2) - P_PhXYZ(:,:,i) = [dXdR(:,i),dXdtheta(:,i)]*... - [P_t(7,7,i), P_t(7,9,i); P_t(9,7,i), P_t(9,9,i)]*... - [dXdR(:,i),dXdtheta(:,i)]'; - P_PhVXYZ(:,:,i) = [dVdR(:,i), dVdR_dot(:,i), dVdtheta(:,i), dVdtheta_dot(:,i)]*... - P_t(7:10,7:10,i)*[dVdR(:,i), dVdR_dot(:,i), dVdtheta(:,i), dVdtheta_dot(:,i)]'; - + P_PhXYZ(:,:,i) = [dXdR(:,i), dXdtheta(:,i), zeros(3,2); dVdR(:,i), dVdR_dot(:,i), dVdtheta(:,i), dVdtheta_dot(:,i)]*... + P_t(7:10,7:10,i)*[dXdR(:,i), dXdtheta(:,i), zeros(3,2); dVdR(:,i), dVdR_dot(:,i), dVdtheta(:,i), dVdtheta_dot(:,i)]'; end + P_PhVXYZ = P_PhXYZ(4:6,4:6,:); + % Correlation covariance MMX-Phobos - P_PhXYZ_MMX = zeros(3,3,size(X_t(9,:),2)); - P_VPhXYZ_VMMX = zeros(3,3,size(X_t(9,:),2)); + P_PhXYZ_MMX = zeros(6,6,size(X_t(9,:),2)); + P_MMX_PhXYZ = zeros(6,6,size(X_t(9,:),2)); for i=1:size(X_t(9,:),2) - P_PhXYZ_MMX(:,:,i) = [dXdR(:,i),dXdtheta(:,i)]*[P_t(1:3,7,i), P_t(1:3,9,i)]'; - P_VPhXYZ_VMMX(:,:,i) = [dVdR(:,i), dVdR_dot(:,i), dVdtheta(:,i), dVdtheta_dot(:,i)]*P_t(4:6,7:10,i)'; + P_PhXYZ_MMX(:,:,i) = [dXdR(:,i), dXdtheta(:,i), zeros(3,2); dVdR(:,i), dVdR_dot(:,i), dVdtheta(:,i), dVdtheta_dot(:,i)]*P_t(7:10,1:6,i); + P_MMX_PhXYZ(:,:,i) = P_t(1:6,7:10,i)*[dXdR(:,i), dXdtheta(:,i), zeros(3,2); dVdR(:,i), dVdR_dot(:,i), dVdtheta(:,i), dVdtheta_dot(:,i)]'; end % Relative distance covariance between MMX and PHobos - P_rel = P_PhXYZ + P_t(1:3,1:3,:) - 2*P_PhXYZ_MMX; + P_rel = P_PhXYZ + P_t(1:6,1:6,:) - P_PhXYZ_MMX - P_MMX_PhXYZ; + P_Xrel = P_rel(1:3,1:3,:); % Relative velocity covariance between MMX and PHobos - P_Vrel = P_PhVXYZ + P_t(4:6,4:6,:) - 2*P_VPhXYZ_VMMX; - + P_Vrel = P_rel(4:6,4:6,:); end \ No newline at end of file diff --git a/Plot_Nico.m b/Plot_Nico.m index 5d0087c0c77a119225b8c0d032967875511bb0cc..a1e75058002bbdb6e3adb997077f35771126df21 100644 --- a/Plot_Nico.m +++ b/Plot_Nico.m @@ -18,19 +18,19 @@ set(0,'DefaultAxesFontSize', 16); %% Confronto altezze -name10 = 'QSOH_6days_allObs.mat'; +name10 = 'QSOH_6days_Auto.mat'; load(name10); Est10 = Est; -name9 = 'QSOM_6days_allObs.mat'; +name9 = 'QSOM_6days_Auto.mat'; load(name9); Est9 = Est; -name8 = 'QSOLa_6days_allObs.mat'; +name8 = 'QSOLa_6days_Auto.mat'; load(name8); Est8 = Est; -name7 = 'QSOLb_6days_allObs_alpha9e1.mat'; +name7 = 'QSOLb_6days_Auto.mat'; load(name7); Est7 = Est; -name6 = 'QSOLc_6days_allObs_alpha7e1.mat'; +name6 = 'QSOLc_6days_Auto.mat'; load(name6); Est6 = Est; diff --git a/Radial_Along_Cross_Covariance.m b/Radial_Along_Cross_Covariance.m new file mode 100644 index 0000000000000000000000000000000000000000..d0e1ebd789679e39fe643b8d949bebe4d66ffee3 --- /dev/null +++ b/Radial_Along_Cross_Covariance.m @@ -0,0 +1,41 @@ +function P_RAC = Radial_Along_Cross_Covariance(P_XYZ, X, pars, units) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% P_RAC = Radial_Along_Cross_Covariance(P_XYZ, pars, units) +% +% Calculation of the Phobos covariance in cartesian coordinates, as well as +% the MMX-Phobos relative position covariance +% +% Input: +% +% P_XYZ Position vector covariance in cartesian coordinates +% X State vector wrt which we want the covariances +% pars Structure of parameters +% units Structure of units +% +% +% Output: +% +% P_RAC Position vector covariance in radial, along-track and +% cross-track coordinates +% +% +% Author: E.Ciccarelli +% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +% Rotation matrix from the MARSIAU to the Phobos Orbital reference frame + I = X(1:3)/norm(X(1:3)); + v = X(4:6)/norm(X(4:6)); + K = cross(I,v)/norm(cross(I,v)); + J = cross(K,I); + + R_NOPh = [I, J, K]'; + R_tot = R_NOPh; + +% Covariance rotation + P_RAC = R_tot*P_XYZ(1:3,1:3)*R_tot'; + + +end \ No newline at end of file diff --git a/Results.m b/Results.m index 3fb72386e37685361a7fef71c56e2f2bd564a1c4..869b06c93067f4dd3830186dc6be7ccb59007edb 100644 --- a/Results.m +++ b/Results.m @@ -1,4 +1,4 @@ -function Results(Est, YObs_Full, pars, units) +function Results(Est, pars, units) %========================================================================== % NewCov_ResultsPlot(Est, YObs_Full, pars) % @@ -38,68 +38,115 @@ function Results(Est, YObs_Full, pars, units) t_obs = Est.t; -% Plot of the post-fit residuals + +%-------------------------------------------------------------------------- + + +% Plot of the pre and post fit residuals -% figure(1) -% subplot(2,4,1) -% plot(t_obs/3600,Est.pre(1,:)*units.lsf,'x','Color','b') -% grid on; -% hold on; -% plot(t_obs/3600,Est.pre(3,:)*units.lsf,'x','Color','b') -% plot(t_obs/3600,Est.pre(5,:)*units.lsf,'x','Color','b') -% plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(1,:),2)),'.-','Color','b') -% plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(1,:),2)),'.-','Color','b') -% xlabel('$[hour]$') -% ylabel('$[km]$') -% title('Range Pre-fit residual') -% subplot(2,4,2) -% plot(t_obs/3600,Est.pre(2,:)*units.vsf,'x','Color','r') -% grid on; -% hold on; -% plot(t_obs/3600,Est.pre(4,:)*units.vsf,'x','Color','r') -% plot(t_obs/3600,Est.pre(6,:)*units.vsf,'x','Color','r') -% plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(2,:),2)),'.-','Color','r') -% plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(2,:),2)),'.-','Color','r') -% xlabel('$[hour]$') -% ylabel('$[km/s]$') -% title('RangeRate Pre-fit residual') -% subplot(2,4,3) -% plot(t_obs/3600,Est.pre(7,:)*units.lsf,'x','Color','g') -% grid on; -% hold on; -% plot(t_obs/3600,3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.pre(7,:),2)),'.-','Color','g') -% plot(t_obs/3600,-3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.pre(7,:),2)),'.-','Color','g') -% xlabel('$[hour]$') -% ylabel('$[km]$') -% title('Lidar Pre-fit residual') -% subplot(2,4,4) -% plot(t_obs/3600,Est.pre(8,:),'x','Color','m') -% grid on; -% hold on; -% plot(t_obs/3600,3*pars.ObsNoise.pixel*ones(size(Est.pre(8,:),2)),'.-','Color','m') -% plot(t_obs/3600,-3*pars.ObsNoise.pixel*ones(size(Est.pre(8,:),2)),'.-','Color','m') -% xlabel('$[hour]$') -% ylabel('$[N. pixel]$') -% title('Camera Pre-fit residual') -% -% -% subplot(2,4,5) -% histfit([Est.pre(1,:), Est.pre(3,:), Est.pre(5,:)]) -% xlabel('$[km]$') -% title('Range Pre-fit residual') -% subplot(2,4,6) -% histfit([Est.pre(2,:), Est.pre(4,:), Est.pre(6,:)]) -% xlabel('$[km/s]$') -% title('Range Rate Pre-fit residual') -% subplot(2,4,7) -% histfit(Est.pre(7,:)) -% xlabel('$[km]$') -% title('Lidar Pre-fit residual') -% subplot(2,4,8) -% histfit(Est.pre(8,:)) -% xlabel('$[rad]$') -% title('Camera Pre-fit residual') + % figure(1) + % subplot(2,4,1) + % plot(t_obs/3600,Est.pre(1,:)*units.lsf,'x','Color','b') + % grid on; + % hold on; + % plot(t_obs/3600,Est.pre(3,:)*units.lsf,'x','Color','b') + % plot(t_obs/3600,Est.pre(5,:)*units.lsf,'x','Color','b') + % plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(1,:),2)),'.-','Color','b') + % plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(1,:),2)),'.-','Color','b') + % xlabel('$[hour]$') + % ylabel('$[km]$') + % title('Range Pre-fit residual') + % subplot(2,4,2) + % plot(t_obs/3600,Est.pre(2,:)*units.vsf,'x','Color','r') + % grid on; + % hold on; + % plot(t_obs/3600,Est.pre(4,:)*units.vsf,'x','Color','r') + % plot(t_obs/3600,Est.pre(6,:)*units.vsf,'x','Color','r') + % plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(2,:),2)),'.-','Color','r') + % plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(2,:),2)),'.-','Color','r') + % xlabel('$[hour]$') + % ylabel('$[km/s]$') + % title('RangeRate Pre-fit residual') + % subplot(2,4,3) + % plot(t_obs/3600,Est.pre(7,:)*units.lsf,'x','Color','g') + % grid on; + % hold on; + % plot(t_obs/3600,3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.pre(7,:),2)),'.-','Color','g') + % plot(t_obs/3600,-3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.pre(7,:),2)),'.-','Color','g') + % xlabel('$[hour]$') + % ylabel('$[km]$') + % title('Lidar Pre-fit residual') + % subplot(2,4,4) + % plot(t_obs/3600,Est.pre(8,:),'x','Color','m') + % grid on; + % hold on; + % plot(t_obs/3600,3*pars.ObsNoise.pixel*ones(size(Est.pre(8,:),2)),'.-','Color','m') + % plot(t_obs/3600,-3*pars.ObsNoise.pixel*ones(size(Est.pre(8,:),2)),'.-','Color','m') + % xlabel('$[hour]$') + % ylabel('$[N. pixel]$') + % title('Camera Pre-fit residual') + % + % subplot(2,4,5) + % plot(t_obs/3600,Est.err(1,:)*units.lsf,'x','Color','b') + % grid on; + % hold on; + % plot(t_obs/3600,Est.err(3,:)*units.lsf,'x','Color','b') + % plot(t_obs/3600,Est.err(5,:)*units.lsf,'x','Color','b') + % plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.err(1,:),2)),'.-','Color','b') + % plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.err(1,:),2)),'.-','Color','b') + % xlabel('$[hour]$') + % ylabel('$[km]$') + % title('Range post-fit residual') + % subplot(2,4,6) + % plot(t_obs/3600,Est.err(2,:)*units.vsf,'x','Color','r') + % grid on; + % hold on; + % plot(t_obs/3600,Est.err(4,:)*units.vsf,'x','Color','r') + % plot(t_obs/3600,Est.err(6,:)*units.vsf,'x','Color','r') + % plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.err(2,:),2)),'.-','Color','r') + % plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.err(2,:),2)),'.-','Color','r') + % xlabel('$[hour]$') + % ylabel('$[km/s]$') + % title('RangeRate post-fit residual') + % subplot(2,4,7) + % plot(t_obs/3600,Est.err(7,:)*units.lsf,'x','Color','g') + % grid on; + % hold on; + % plot(t_obs/3600,3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.err(7,:),2)),'.-','Color','g') + % plot(t_obs/3600,-3*pars.ObsNoise.lidar*units.lsf*ones(size(Est.err(7,:),2)),'.-','Color','g') + % xlabel('$[hour]$') + % ylabel('$[km]$') + % title('Lidar post-fit residual') + % subplot(2,4,8) + % plot(t_obs/3600,Est.err(8,:),'x','Color','m') + % grid on; + % hold on; + % plot(t_obs/3600,3*pars.ObsNoise.pixel*ones(size(Est.err(8,:),2)),'.-','Color','m') + % plot(t_obs/3600,-3*pars.ObsNoise.pixel*ones(size(Est.err(8,:),2)),'.-','Color','m') + % xlabel('$[hour]$') + % ylabel('$[N. pixel]$') + % title('Camera post-fit residual') + + + % subplot(2,4,5) + % histfit([Est.pre(1,:), Est.pre(3,:), Est.pre(5,:)]) + % xlabel('$[km]$') + % title('Range Pre-fit residual') + % subplot(2,4,6) + % histfit([Est.pre(2,:), Est.pre(4,:), Est.pre(6,:)]) + % xlabel('$[km/s]$') + % title('Range Rate Pre-fit residual') + % subplot(2,4,7) + % histfit(Est.pre(7,:)) + % xlabel('$[km]$') + % title('Lidar Pre-fit residual') + % subplot(2,4,8) + % histfit(Est.pre(8,:)) + % xlabel('$[rad]$') + % title('Camera Pre-fit residual') +%-------------------------------------------------------------------------- + % Square-Root of the MMX covariance error @@ -112,84 +159,101 @@ function Results(Est, YObs_Full, pars, units) sqvz = squeeze(Est.P_t(6,6,1:end-1)); SQRT_V = 3*sqrt(sqvx + sqvy + sqvz); - -% 3sigma Envelopes - figure() - subplot(1,2,1) - semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(1,1,1:end-1)))),'Color','b','LineWidth',1) - hold on; - grid on; - semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(2,2,1:end-1)))),'Color','r','LineWidth',1) - semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(3,3,1:end-1)))),'Color','g','LineWidth',1) - semilogy(t_obs(1:end-1)/3600,SQRT_X,'Color','k','LineWidth',2) - xlabel('time $[hour]$') - ylabel('$[km]$') - title('MMX position vector $3\sigma$ envelopes','Interpreter','latex','FontSize',16) - legend('$3\sigma_{x}$','$3\sigma_{y}$','$3\sigma_{z}$','$3 RMS$','Interpreter','latex','FontSize',14) - subplot(1,2,2) - semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(4,4,1:end-1)))),'Color','b','LineWidth',1) - hold on; - grid on; - semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(5,5,1:end-1)))),'Color','r','LineWidth',1) - semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(6,6,1:end-1)))),'Color','g','LineWidth',1) - semilogy(t_obs(1:end-1)/3600,SQRT_V,'Color','k','LineWidth',2) - xlabel('time $[hour]$') - ylabel('$[km/s]$') - title('MMX velocity vector $3\sigma$ envelopes','Interpreter','latex','FontSize',16) - legend('$3\sigma_{\dot{x}}$','$3\sigma_{\dot{y}}$','$3\sigma_{\dot{z}}$','$3 RMS$','Interpreter','latex','FontSize',14) - - -% Phobos's states uncertainties evolution - -% % 3sigma Envelopes + if pars.flag_FILTERING == 1 + + MMX_real = cspice_spkezr('-34', pars.et0+t_obs, 'MARSIAU', 'none', 'MARS'); + err = abs(MMX_real - Est.X_t(1:6,:)); + + + % 3sigma Envelopes and error + figure() + subplot(2,3,1) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(1,1,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(1,1:end-1),'*','Color','b','LineWidth',1) + xlabel('time $[hour]$','FontSize',26) + ylabel('$[km]$','FontSize',26) + title('$x_{MMX}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,3,2) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(2,2,1:end-1)))),'Color','r','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(2,1:end-1),'*','Color','r','LineWidth',1) + xlabel('time $[hour]$','FontSize',26) + ylabel('$[km]$','FontSize',26) + title('$y_{MMX}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,3,3) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(3,3,1:end-1)))),'Color','g','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(3,1:end-1),'*','Color','g','LineWidth',1) + xlabel('time $[hour]$','FontSize',26) + ylabel('$[km]$','FontSize',26) + title('$z_{MMX}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,3,4) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(4,4,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(4,1:end-1),'*','Color','b','LineWidth',1) + xlabel('time $[hour]$','FontSize',26) + ylabel('$[km/s]$','FontSize',26) + title('$Vx_{MMX}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,3,5) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(5,5,1:end-1)))),'Color','r','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(5,1:end-1),'*','Color','r','LineWidth',1) + xlabel('time $[hour]$','FontSize',26) + ylabel('$[km/s]$','FontSize',26) + title('$Vy_{MMX}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,3,6) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(6,6,1:end-1)))),'Color','g','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(6,1:end-1),'*','Color','g','LineWidth',1) + xlabel('time $[hour]$','FontSize',26) + ylabel('$[km/s]$','FontSize',26) + title('$Vz_{MMX}$ $3\sigma$ envelope and error','FontSize',30) + - figure() - subplot(2,4,1) - semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(7,7,1:end-1)))),'Color','b','LineWidth',1) - grid on - xlabel('time [hour]') - ylabel('$R_{Ph}$ [km]') - subplot(2,4,2) - semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(9,9,1:end-1)))),'Color','r','LineWidth',1) - grid on - xlabel('time [hour]') - ylabel('$\theta_{Ph}$ [rad]') - subplot(2,4,3) - semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(11,11,1:end-1)))),'Color','g','LineWidth',1) - grid on - xlabel('time [hour]') - ylabel('$\Phi_{M}$ [rad]') - subplot(2,4,4) - semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(13,13,1:end-1)))),'Color','m','LineWidth',1) - grid on - xlabel('time [hour]') - ylabel('$\Phi_{Ph}$ [rad]') - - subplot(2,4,5) - semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(8,8,1:end-1)))),'Color','b','LineWidth',1) - grid on - xlabel('time [hour]') - ylabel('$\dot{R}_{Ph}$ [km/s]') - subplot(2,4,6) - semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(10,10,1:end-1)))),'Color','r','LineWidth',1) - grid on - xlabel('time [hour]') - ylabel('$\dot{\theta}_{Ph}$ [rad/s]') - subplot(2,4,7) - semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(12,12,1:end-1)))),'Color','g','LineWidth',1) - grid on - xlabel('time [hour]') - ylabel('$\dot{\Phi}_{M}$ [rad/s]') - subplot(2,4,8) - semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(14,14,1:end-1)))),'Color','m','LineWidth',1) - grid on - xlabel('time [hour]') - ylabel('$\dot{\Phi}_{Ph}$ [rad/s]') + end + + % 3sigma Envelopes + figure() + subplot(1,2,1) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(1,1,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(2,2,1:end-1)))),'Color','r','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(3,3,1:end-1)))),'Color','g','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,SQRT_X,'Color','k','LineWidth',2) + xlabel('time $[hour]$','FontSize',26) + ylabel('$[km]$','FontSize',26) + title('MMX position vector $3\sigma$ envelopes','Interpreter','latex','FontSize',30) + legend('$3\sigma_{x}$','$3\sigma_{y}$','$3\sigma_{z}$','$3 RMS$','Interpreter','latex','FontSize',26) + subplot(1,2,2) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(4,4,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(5,5,1:end-1)))),'Color','r','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(Est.P_t(6,6,1:end-1)))),'Color','g','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,SQRT_V,'Color','k','LineWidth',2) + xlabel('time $[hour]$','FontSize',26) + ylabel('$[km/s]$','FontSize',26) + title('MMX velocity vector $3\sigma$ envelopes','Interpreter','latex','FontSize',30) + legend('$3\sigma_{\dot{x}}$','$3\sigma_{\dot{y}}$','$3\sigma_{\dot{z}}$','$3 RMS$','Interpreter','latex','FontSize',26) + + + +%-------------------------------------------------------------------------- +% Phobos's states uncertainties evolution % Phobos cartesian covaraiance - [P_PhXYZ, ~, P_rel_MMXPh, P_PhVXYZ, P_Vrel_MMXPh] = Phobos_cartesian_covariance(Est.X_t, Est.P_t, pars, units); + [P_PhXYZ, P_rel_MMXPh, P_PhVXYZ, P_Vrel_MMXPh, X_Phobos] =... + Phobos_cartesian_covariance(Est.X_t, Est.P_t, pars); sqx_Ph = squeeze(P_PhXYZ(1,1,1:end-1)); sqy_Ph = squeeze(P_PhXYZ(2,2,1:end-1)); @@ -212,8 +276,288 @@ function Results(Est, YObs_Full, pars, units) SQRT_V_rel = 3*sqrt(sqx_Vrel + sqy_Vrel + sqz_Vrel); + if pars.flag_FILTERING == 1 + + Phobos_real = cspice_spkezr('-401', pars.et0+t_obs, 'MARSIAU', 'none', 'MARS'); + err = abs(Phobos_real - X_Phobos); + + + [Ph, pars] = Phobos_States_NewModel(pars.et0,pars); + Ph = Ph./units.sfVec2; + St0 = [MMX_real(:,1)./units.sfVec; Ph; pars.I2]; + tspan = t_obs*units.tsf; + RelTol = 1e-13; + AbsTol = 1e-16; + opt = odeset('RelTol',RelTol,'AbsTol',AbsTol,'event',@(t,X) landing_Phobos(t,X,pars,units)); + [~,X] = ode113(@(t,X) Dynamics_MPHandMMX_Inertia(t,X,pars,units),tspan,St0,opt); + + + if size(Est.X_t,1) == 20 + + Phi_t = X(:,13:14)'.*[1; units.tsf]; + err_Phi = abs(Phi_t - Est.X_t(11:12,:)); + + % 3sigma Envelopes and error + figure() + subplot(2,4,1) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(1,1:end-1),'*','Color','b','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km]$') + title('$x_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,2) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhXYZ(2,2,1:end-1)))),'Color','r','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(2,1:end-1),'*','Color','r','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km]$') + title('$y_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,3) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhXYZ(3,3,1:end-1)))),'Color','g','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(3,1:end-1),'*','Color','g','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km]$') + title('$z_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,4) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(11,11,1:end-1)))),'Color','m','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err_Phi(1,1:end-1),'*','Color','m','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[rad]$') + title('$\Phi_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,5) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhVXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(4,1:end-1),'*','Color','b','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km/s]$') + title('$Vx_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,6) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhVXYZ(2,2,1:end-1)))),'Color','r','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(5,1:end-1),'*','Color','r','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km/s]$') + title('$Vy_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,7) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhVXYZ(3,3,1:end-1)))),'Color','g','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(6,1:end-1),'*','Color','g','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km/s]$') + title('$Vz_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,8) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(12,12,1:end-1)))),'Color','m','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err_Phi(2,1:end-1),'*','Color','m','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[rad/s]$') + title('$\dot{\Phi}_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + + else + + Phi_t = X(:,13:14)'.*[1; units.tsf]; + err_Phi = abs(Phi_t - Est.X_t(13:14,:)); + + % 3sigma Envelopes and error + figure() + subplot(2,4,1) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(1,1:end-1),'*','Color','b','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km]$') + title('$x_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,2) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhXYZ(2,2,1:end-1)))),'Color','r','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(2,1:end-1),'*','Color','r','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km]$') + title('$y_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,3) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhXYZ(3,3,1:end-1)))),'Color','g','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(3,1:end-1),'*','Color','g','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km]$') + title('$z_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,4) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(13,13,1:end-1)))),'Color','m','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err_Phi(1,1:end-1),'*','Color','m','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[rad]$') + title('$\Phi_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,5) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhVXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(4,1:end-1),'*','Color','b','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km/s]$') + title('$Vx_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,6) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhVXYZ(2,2,1:end-1)))),'Color','r','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(5,1:end-1),'*','Color','r','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km/s]$') + title('$Vy_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,7) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhVXYZ(3,3,1:end-1)))),'Color','g','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err(6,1:end-1),'*','Color','g','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[km/s]$') + title('$Vz_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + subplot(2,4,8) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(14,14,1:end-1)))),'Color','m','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,err_Phi(2,1:end-1),'*','Color','m','LineWidth',1) + xlabel('time $[hour]$') + ylabel('$[rad/s]$') + title('$\dot{\Phi}_{Ph}$ $3\sigma$ envelope and error','FontSize',30) + + end + + else + + if size(Est.X_t,1) == 20 + + % 3sigma Envelopes + figure() + subplot(2,3,1) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(7,7,1:end-1)))),'Color','b','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$R_{Ph}$ [km]') + subplot(2,3,2) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(9,9,1:end-1)))),'Color','r','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\theta_{Ph}$ [rad]') + subplot(2,3,3) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(11,11,1:end-1)))),'Color','m','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\Phi_{Ph}$ [rad]') + + subplot(2,3,4) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(8,8,1:end-1)))),'Color','b','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\dot{R}_{Ph}$ [km/s]') + subplot(2,3,5) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(10,10,1:end-1)))),'Color','r','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\dot{\theta}_{Ph}$ [rad/s]') + subplot(2,3,6) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(12,12,1:end-1)))),'Color','m','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\dot{\Phi}_{Ph}$ [rad/s]') + + else + + + % 3sigma Envelopes + figure() + subplot(2,4,1) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(7,7,1:end-1)))),'Color','b','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$R_{Ph}$ [km]') + subplot(2,4,2) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(9,9,1:end-1)))),'Color','r','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\theta_{Ph}$ [rad]') + subplot(2,4,3) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(11,11,1:end-1)))),'Color','g','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\Phi_{M}$ [rad]') + subplot(2,4,4) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(13,13,1:end-1)))),'Color','m','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\Phi_{Ph}$ [rad]') + + subplot(2,4,5) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(8,8,1:end-1)))),'Color','b','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\dot{R}_{Ph}$ [km/s]') + subplot(2,4,6) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(10,10,1:end-1)))),'Color','r','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\dot{\theta}_{Ph}$ [rad/s]') + subplot(2,4,7) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(12,12,1:end-1)))),'Color','g','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\dot{\Phi}_{M}$ [rad/s]') + subplot(2,4,8) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(14,14,1:end-1)))),'Color','m','LineWidth',1) + grid on + xlabel('time [hour]') + ylabel('$\dot{\Phi}_{Ph}$ [rad/s]') + + end + + end + + + %-------------------------------------------------------------------------- + figure() - subplot(2,2,1) + subplot(1,2,1) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhXYZ(2,2,1:end-1)))),'Color','r','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhXYZ(3,3,1:end-1)))),'Color','g','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,SQRT_X_Ph,'Color','k','LineWidth',2) + xlabel('time $[hour]$','FontSize',26) + ylabel('$[km]$','FontSize',26) + title('Phobos position vector $3\sigma$ envelopes','Interpreter','latex','FontSize',30) + legend('$3\sigma_{x}$','$3\sigma_{y}$','$3\sigma_{z}$','$3 RMS$','Interpreter','latex','FontSize',26) + subplot(1,2,2) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhVXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhVXYZ(2,2,1:end-1)))),'Color','r','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_PhVXYZ(3,3,1:end-1)))),'Color','g','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,SQRT_V_Ph,'Color','k','LineWidth',2) + xlabel('time $[hour]$','FontSize',26) + ylabel('$[km/s]$','FontSize',26) + title('Phobos velocity vector $3\sigma$ envelopes','Interpreter','latex','FontSize',30) + legend('$3\sigma_{\dot{x}}$','$3\sigma_{\dot{y}}$','$3\sigma_{\dot{z}}$','$3 RMS$','Interpreter','latex','FontSize',26) + + + + figure() + subplot(1,3,1) semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1) hold on; grid on; @@ -222,9 +566,9 @@ function Results(Est, YObs_Full, pars, units) semilogy(t_obs(1:end-1)/3600,SQRT_X_Ph,'Color','k','LineWidth',2) xlabel('time $[hour]$') ylabel('$[km]$') - title('Phobos position vector $3\sigma$ envelopes','Interpreter','latex','FontSize',16) + title('Phobos position vector $3\sigma$ envelopes','Interpreter','latex','FontSize',30) legend('$3\sigma_{x}$','$3\sigma_{y}$','$3\sigma_{z}$','$3 RMS$','Interpreter','latex','FontSize',14) - subplot(2,2,2) + subplot(1,3,2) semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_PhVXYZ(1,1,1:end-1)))),'Color','b','LineWidth',1) hold on; grid on; @@ -233,9 +577,9 @@ function Results(Est, YObs_Full, pars, units) semilogy(t_obs(1:end-1)/3600,SQRT_V_Ph,'Color','k','LineWidth',2) xlabel('time $[hour]$') ylabel('$[km]$') - title('Phobos velocity vector $3\sigma$ envelopes','Interpreter','latex','FontSize',16) + title('Phobos velocity vector $3\sigma$ envelopes','Interpreter','latex','FontSize',30) legend('$3\sigma_{\dot{x}}$','$3\sigma_{\dot{y}}$','$3\sigma_{\dot{z}}$','$3 RMS$','Interpreter','latex','FontSize',14) - subplot(2,2,3) + subplot(1,3,3) semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_rel_MMXPh(1,1,1:end-1)))),'Color','b','LineWidth',1) hold on; grid on; @@ -244,30 +588,93 @@ function Results(Est, YObs_Full, pars, units) semilogy(t_obs(1:end-1)/3600,SQRT_X_rel,'Color','k','LineWidth',2) xlabel('time $[hour]$') ylabel('$[km]$') - title('MMX-Phobos relative distance $3\sigma$ envelopes','Interpreter','latex','FontSize',16) + title('MMX-Phobos relative distance $3\sigma$ envelopes','Interpreter','latex','FontSize',30) legend('$3\sigma_{x}$','$3\sigma_{y}$','$3\sigma_{z}$','$3 RMS$','Interpreter','latex','FontSize',14) - subplot(2,2,4) - semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_Vrel_MMXPh(1,1,1:end-1)))),'Color','b','LineWidth',1) + % subplot(2,2,4) + % semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_Vrel_MMXPh(1,1,1:end-1)))),'Color','b','LineWidth',1) + % hold on; + % grid on; + % semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_Vrel_MMXPh(2,2,1:end-1)))),'Color','r','LineWidth',1) + % semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_Vrel_MMXPh(3,3,1:end-1)))),'Color','g','LineWidth',1) + % semilogy(t_obs(1:end-1)/3600,SQRT_V_rel,'Color','k','LineWidth',2) + % xlabel('time $[hour]$') + % ylabel('$[km/s]$') + % title('MMX-Phobos relative velocity $3\sigma$ envelopes','Interpreter','latex','FontSize',30) + % legend('$3\sigma_{\dot{x}}$','$3\sigma_{\dot{y}}$','$3\sigma_{\dot{z}}$','$3 RMS$','Interpreter','latex','FontSize',14) + + + +%-------------------------------------------------------------------------- + + + +% Covariances in the Phobos Radial-Along-Crross track directions + + P_Ph_RAC = zeros(3,3,size(Est.X_t(9,:),2)); + P_MMX_RAC = zeros(3,3,size(Est.X_t(9,:),2)); + P_Rel_RAC = zeros(3,3,size(Est.X_t(9,:),2)); + + for i = 1:size(Est.X_t(1,:),2) + + P_Ph_RAC(:,:,i) = Radial_Along_Cross_Covariance(P_PhXYZ(:,:,i), X_Phobos(:,i), pars, units); + P_MMX_RAC(:,:,i) = Radial_Along_Cross_Covariance(Est.P_t(1:3,1:3,i), Est.X_t(1:6,i), pars, units); + P_Rel_RAC(:,:,i) = Radial_Along_Cross_Covariance(P_rel_MMXPh(:,:,i), Est.X_t(1:6,i)-X_Phobos(:,i), pars, units); + + end + + figure() + subplot(1,3,1) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_Ph_RAC(1,1,1:end-1)))),'Color','b','LineWidth',1) hold on; grid on; - semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_Vrel_MMXPh(2,2,1:end-1)))),'Color','r','LineWidth',1) - semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_Vrel_MMXPh(3,3,1:end-1)))),'Color','g','LineWidth',1) - semilogy(t_obs(1:end-1)/3600,SQRT_V_rel,'Color','k','LineWidth',2) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_Ph_RAC(2,2,1:end-1)))),'Color','r','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_Ph_RAC(3,3,1:end-1)))),'Color','g','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,SQRT_X_Ph,'Color','k','LineWidth',2) xlabel('time $[hour]$') ylabel('$[km]$') - title('MMX-Phobos relative velocity $3\sigma$ envelopes','Interpreter','latex','FontSize',16) - legend('$3\sigma_{x}$','$3\sigma_{y}$','$3\sigma_{z}$','$3 RMS$','Interpreter','latex','FontSize',14) + title('Phobos position $3\sigma$ in RAC directions wrt Phobos position','Interpreter','latex','FontSize',30) + legend('$3\sigma_{radial}$','$3\sigma_{along}$','$3\sigma_{cross}$','$3 RMS$','Interpreter','latex','FontSize',14) + subplot(1,3,2) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_MMX_RAC(1,1,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_MMX_RAC(2,2,1:end-1)))),'Color','r','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_MMX_RAC(3,3,1:end-1)))),'Color','g','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,SQRT_X,'Color','k','LineWidth',2) + xlabel('time $[hour]$') + ylabel('$[km]$') + title('MMX position $3\sigma$ in RAC directions wrt Phobos position','Interpreter','latex','FontSize',30) + legend('$3\sigma_{radial}$','$3\sigma_{along}$','$3\sigma_{cross}$','$3 RMS$','Interpreter','latex','FontSize',14) + subplot(1,3,3) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(P_Rel_RAC(1,1,1:end-1)))),'Color','b','LineWidth',1) + hold on; + grid on; + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_Rel_RAC(2,2,1:end-1)))),'Color','r','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,3*squeeze(real(sqrt(P_Rel_RAC(3,3,1:end-1)))),'Color','g','LineWidth',1) + semilogy(t_obs(1:end-1)/3600,SQRT_X_rel,'Color','k','LineWidth',2) + xlabel('time $[hour]$') + ylabel('$[km]$') + title('MMX position $3\sigma$ in RAC directions wrt Phobos position','Interpreter','latex','FontSize',30) + legend('$3\sigma_{radial}$','$3\sigma_{along}$','$3\sigma_{cross}$','$3 RMS$','Interpreter','latex','FontSize',14) +%-------------------------------------------------------------------------- + % Harmonics 3sigma Envelopes % place0 = size(Est.X,1)-pars.nCoeff-pars.nBias; + +if size(Est.X_t,1) == 20 + corr_label = {'$x$','$y$','$z$','$\dot{x}$','$\dot{y}$','$\dot{z}$',... + '$R_{Ph}$','$\dot{R}_{Ph}$','$\theta_{Ph}$','$\dot{\theta}_{Ph}$',... + '$\Phi_{Ph}$','$\dot{\Phi}_{Ph}$'}; +else 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}$'}; - +end % Moments of inertia % figure() @@ -293,7 +700,10 @@ function Results(Est, YObs_Full, pars, units) % xlabel('time $[hour]$') % legend('$I_{PhZ}$','Interpreter','latex','FontSize',14) - + +%-------------------------------------------------------------------------- + + % Biases place0 = size(Est.X,1)-pars.nBias; @@ -341,34 +751,177 @@ function Results(Est, YObs_Full, pars, units) ylabel('$[pix]$') -% -% % Harmonics Coefficients -% - P_t_I2x = squeeze(Est.P_t(15,15,1:end-1)); - P_t_I2y = squeeze(Est.P_t(16,16,1:end-1)); - P_t_I2z = squeeze(Est.P_t(17,17,1:end-1)); - Cov_I2xI2y = squeeze(Est.P_t(15,16,1:end-1)); - 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); +%-------------------------------------------------------------------------- - figure() - subplot(1,2,1) - 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',14) - subplot(1,2,2) - 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',14) +% Harmonics Coefficients + + +if size(Est.X_t,1) == 20 + + P_t_I2x = squeeze(Est.P_t(13,13,1:end-1)); + P_t_I2y = squeeze(Est.P_t(14,14,1:end-1)); + P_t_I2z = squeeze(Est.P_t(15,15,1:end-1)); + Cov_I2xI2y = squeeze(Est.P_t(13,14,1:end-1)); + 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); + + + 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,:)); + + 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) + 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',14) + subplot(1,2,2) + 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',14) + + + figure() + subplot(1,3,1) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(13,13,1:end-1)))),'LineWidth',1,'Color','b'); + grid on; + hold on; + semilogy(t_obs(1:end-1)/3600,abs(Est.X_t(13,1:end-1)-pars.I2(1)),'*','Color','b'); + xlabel('time $[hour]$') + legend('$I_{PhX}$','Interpreter','latex','FontSize',14) + subplot(1,3,2) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(14,14,1:end-1)))),'LineWidth',1,'Color','r'); + grid on; + hold on; + semilogy(t_obs(1:end-1)/3600,abs(Est.X_t(14,1:end-1)-pars.I2(2)),'*','Color','r'); + xlabel('time $[hour]$') + legend('$I_{PhY}$','Interpreter','latex','FontSize',14) + subplot(1,3,3) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(15,15,1:end-1)))),'LineWidth',1,'Color','g'); + grid on; + hold on; + semilogy(t_obs(1:end-1)/3600,abs(Est.X_t(15,1:end-1)-pars.I2(3)),'*','Color','g'); + xlabel('time $[hour]$') + legend('$I_{PhZ}$','Interpreter','latex','FontSize',14) + + else + + figure() + subplot(1,2,1) + 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',14) + subplot(1,2,2) + 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',14) + + + end + + +else + + P_t_I2x = squeeze(Est.P_t(15,15,1:end-1)); + P_t_I2y = squeeze(Est.P_t(16,16,1:end-1)); + P_t_I2z = squeeze(Est.P_t(17,17,1:end-1)); + Cov_I2xI2y = squeeze(Est.P_t(15,16,1:end-1)); + 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); + + + 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,:)); + + 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) + 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',14) + subplot(1,2,2) + 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',14) + + + figure() + subplot(1,3,1) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(15,15,1:end-1)))),'LineWidth',1,'Color','b'); + grid on; + hold on; + semilogy(t_obs(1:end-1)/3600,abs(Est.X_t(15,1:end-1)-pars.I2(1)),'*','Color','b'); + xlabel('time $[hour]$') + legend('$I_{PhX}$','Interpreter','latex','FontSize',14) + subplot(1,3,2) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(16,16,1:end-1)))),'LineWidth',1,'Color','r'); + grid on; + hold on; + semilogy(t_obs(1:end-1)/3600,abs(Est.X_t(16,1:end-1)-pars.I2(2)),'*','Color','r'); + xlabel('time $[hour]$') + legend('$I_{PhY}$','Interpreter','latex','FontSize',14) + subplot(1,3,3) + semilogy(t_obs(1:end-1)/3600,3.*squeeze(real(sqrt(Est.P_t(17,17,1:end-1)))),'LineWidth',1,'Color','g'); + grid on; + hold on; + semilogy(t_obs(1:end-1)/3600,abs(Est.X_t(17,1:end-1)-pars.I2(3)),'*','Color','g'); + xlabel('time $[hour]$') + legend('$I_{PhZ}$','Interpreter','latex','FontSize',14) + + else + + figure() + subplot(1,2,1) + 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',14) + subplot(1,2,2) + 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',14) + + + end + +end + +%-------------------------------------------------------------------------- + % Correlations coefficients diff --git a/Results/3DQSOLb_6days_AllObs_alpha100.mat b/Results/3DQSOLb_6days_AllObs_alpha100.mat new file mode 100644 index 0000000000000000000000000000000000000000..6bbb9063571d11be8e05aabfa2efaf854f8a9bd7 Binary files /dev/null and b/Results/3DQSOLb_6days_AllObs_alpha100.mat differ diff --git a/Results/QSOH_6days_AllObs_alpha100.mat b/Results/QSOH_6days_AllObs_alpha100.mat new file mode 100644 index 0000000000000000000000000000000000000000..cd7293555c2c04bf44ce662c06a7ada32fde0172 Binary files /dev/null and b/Results/QSOH_6days_AllObs_alpha100.mat differ diff --git a/Results/QSOH_6days_Auto.mat b/Results/QSOH_6days_Auto.mat new file mode 100644 index 0000000000000000000000000000000000000000..575b11e628207d070aa30571d155c61add4a45f7 Binary files /dev/null and b/Results/QSOH_6days_Auto.mat differ diff --git a/Results/QSOLa_6days_AllObs_alpha100.mat b/Results/QSOLa_6days_AllObs_alpha100.mat new file mode 100644 index 0000000000000000000000000000000000000000..18b97777fb994031235b1b26eea7e9b169a89b02 Binary files /dev/null and b/Results/QSOLa_6days_AllObs_alpha100.mat differ diff --git a/Results/QSOLa_6days_Auto.mat b/Results/QSOLa_6days_Auto.mat new file mode 100644 index 0000000000000000000000000000000000000000..e2cd11974cc388e78b44e42305b93a88b9bb96e7 Binary files /dev/null and b/Results/QSOLa_6days_Auto.mat differ diff --git a/Results/QSOLb_6days_AllObs_alpha100.mat b/Results/QSOLb_6days_AllObs_alpha100.mat new file mode 100644 index 0000000000000000000000000000000000000000..f8c5cef4a485923dbe3f59cf5f71ba0d15331e60 Binary files /dev/null and b/Results/QSOLb_6days_AllObs_alpha100.mat differ diff --git a/Results/QSOLb_6days_Auto.mat b/Results/QSOLb_6days_Auto.mat new file mode 100644 index 0000000000000000000000000000000000000000..62b55032ff94d15d7146dc30c06437cbc9a38335 Binary files /dev/null and b/Results/QSOLb_6days_Auto.mat differ diff --git a/Results/QSOLc_6days_AllObs_alpha100.mat b/Results/QSOLc_6days_AllObs_alpha100.mat new file mode 100644 index 0000000000000000000000000000000000000000..831a0abe9fbb97b02e4ceed41b8de95b4873dd13 Binary files /dev/null and b/Results/QSOLc_6days_AllObs_alpha100.mat differ diff --git a/Results/QSOLc_6days_Auto.mat b/Results/QSOLc_6days_Auto.mat new file mode 100644 index 0000000000000000000000000000000000000000..d2aa0219c6833828dc15e9efd04c83461a828a8c Binary files /dev/null and b/Results/QSOLc_6days_Auto.mat differ diff --git a/Results/QSOM_6days_AllObs_alpha100.mat b/Results/QSOM_6days_AllObs_alpha100.mat new file mode 100644 index 0000000000000000000000000000000000000000..dcc351526d82a1561ab0aa2b63e485510ed366c0 Binary files /dev/null and b/Results/QSOM_6days_AllObs_alpha100.mat differ diff --git a/Results/QSOM_6days_Auto.mat b/Results/QSOM_6days_Auto.mat new file mode 100644 index 0000000000000000000000000000000000000000..35a2417136514adcd6f7f85736eb9e0086753ef4 Binary files /dev/null and b/Results/QSOM_6days_Auto.mat differ diff --git a/Results/SwingQSOLb_6days_AllObs_alpha100.mat b/Results/SwingQSOLb_6days_AllObs_alpha100.mat new file mode 100644 index 0000000000000000000000000000000000000000..a0342f9cf309ba22c7bf39679f613022f2c78428 Binary files /dev/null and b/Results/SwingQSOLb_6days_AllObs_alpha100.mat differ diff --git a/SigmaPoints_Dynamics_Good.m b/SigmaPoints_Dynamics_Good.m index 19bdd649a0145f7f49a120335125614bdaddf572..d74fbd5ae29a8777cbbba4f4bc25fcbbb573f8f4 100644 --- a/SigmaPoints_Dynamics_Good.m +++ b/SigmaPoints_Dynamics_Good.m @@ -1,7 +1,7 @@ -function dx = SigmaPoints_Dynamics_Good(~,x,par,~) +function dx = SigmaPoints_Dynamics_Good(~,x,pars,~) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % -% dx = Cov_Dynamics_Good(t,x,pars,units) +% dx = Cov_Dynamics_Good(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 @@ -9,14 +9,14 @@ function dx = SigmaPoints_Dynamics_Good(~,x,par,~) % Input: % .t Elapsed time since epoch (-) % .x State Vector -% .pars Problem Parameters +% .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 Parameters (-) +% .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 @@ -37,9 +37,9 @@ function dx = SigmaPoints_Dynamics_Good(~,x,par,~) %% Dimensions - d = par.d; % Dimension of the problem - nCoeff = par.nCoeff; % N. of gravitational parameters - nBias = par.nBias; + d = pars.d; % Dimension of the problem + nCoeff = pars.nCoeff; % N. of gravitational parsameters + nBias = pars.nBias; size_St = (d+8+nCoeff+nBias); @@ -69,12 +69,12 @@ function dx = SigmaPoints_Dynamics_Good(~,x,par,~) IPhz_bar = x(17+i*size_St); % Position of MMX wrt Phobos - rPh = par.perifocal2MARSIAU*[RPh*cos(theta); RPh*sin(theta); 0]; + rPh = pars.perifocal2MARSIAU*[RPh*cos(theta); RPh*sin(theta); 0]; rsb = r - rPh; % Rotation matrix from orbit's to Phobos fixed coordinates frame I = rPh/RPh; - k = par.MARSIAU2perifocal'*[0; 0; 1]; + k = pars.MARSIAU2perifocal'*[0; 0; 1]; j = cross(k,I); NO = [I,j,k]; ON = NO'; @@ -85,24 +85,24 @@ function dx = SigmaPoints_Dynamics_Good(~,x,par,~) %% Phobos's orbit - % Potential's first order partials - dVdRPh = par.ni/RPh^2*(1 + 3/(2*RPh^2)*((par.IMz_bar - par.Is) +... + % 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*par.ni/RPh^3 * (IPhy_bar - IPhx_bar)*sin(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/par.ni; - theta_ddot = dVdXsi/(par.ni*RPh^2) - 2*RPh_dot*theta_dot/RPh; - Phi_ddot = - dVdXsi/(par.ni*RPh^2) + 2*RPh_dot*theta_dot/RPh; - Xsi_ddot = -(1 + par.ni*RPh^2/IPhz_bar)*dVdXsi/(par.ni*RPh^2) +... + 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 - gM = - par.G*par.MMars*r/R^3; - gJ2M = - 1.5*par.G*par.MMars*par.J2*par.RMmean/R^5*... + gM = - 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)]; @@ -113,21 +113,21 @@ function dx = SigmaPoints_Dynamics_Good(~,x,par,~) C_22_Ph = .25*(IPhy_bar - IPhx_bar); C_20_Ph = -.5*(2*IPhz_bar - IPhx_bar - IPhy_bar); - par.SH.Clm = [1, 0, 0; 0, 0, 0; C_20_Ph, 0, C_22_Ph]./par.SH.Norm(1:3,1:3); + pars.SH.Clm = [1, 0, 0; 0, 0, 0; C_20_Ph, 0, C_22_Ph]./pars.SH.Norm(1:3,1:3); - [gb,~,~,~] = SHGM_Full(BO*ON*rsb,par.SH); + [gb,~,~,~] = SHGM_Full(BO*ON*rsb,pars.SH); gPh = NO*OB*gb; % Trascinamento % Tutto adimensionale - G = par.G; - M2 = par.MPho; - M1 = par.MMars; + G = pars.G; + M2 = pars.MPho; + M1 = pars.MMars; - I1x = par.IMx_bar*M1; - I1y = par.IMy_bar*M1; - I1z = par.IMz_bar*M1; + I1x = pars.IMx_bar*M1; + I1y = pars.IMy_bar*M1; + I1z = pars.IMz_bar*M1; % Need those quantities diff --git a/SigmaPoints_Dynamics_Good_NoPhi.m b/SigmaPoints_Dynamics_Good_NoPhi.m new file mode 100644 index 0000000000000000000000000000000000000000..57f31709e5a817dc1d483cd076de82e7beba4850 --- /dev/null +++ b/SigmaPoints_Dynamics_Good_NoPhi.m @@ -0,0 +1,170 @@ +function dx = SigmaPoints_Dynamics_Good_NoPhi(~,x,pars,~) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% dx = Cov_Dynamics_Good(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 + + +%% Dimensions + + d = pars.d; % Dimension of the problem + nCoeff = pars.nCoeff; % N. of gravitational parsameters + nBias = pars.nBias; + + size_St = (d+6+nCoeff+nBias); + + +%% Useful quantities + + for i = 0:2*size_St + +% Spacecraft state vector + r = x(1+i*size_St:3+i*size_St); % Pos. vector wrt central body + v = x(4+i*size_St:6+i*size_St); % Vel. vector wrt central body + R = norm(r); % Norm of pos. vector wrt central body + +% Phobos states + RPh = x(7+i*size_St); + RPh_dot = x(8+i*size_St); + theta = x(9+i*size_St); + theta_dot = x(10+i*size_St); + Phi = 0; + Xsi = x(11+i*size_St); + Xsi_dot = x(12+i*size_St); + +% Phobos moment of inertia + IPhx_bar = x(13+i*size_St); + IPhy_bar = x(14+i*size_St); + IPhz_bar = x(15+i*size_St); + +% Position of MMX wrt Phobos + rPh = pars.perifocal2MARSIAU*[RPh*cos(theta); RPh*sin(theta); 0]; + rsb = r - rPh; + +% Rotation matrix from orbit's to Phobos fixed coordinates frame + I = rPh/RPh; + k = pars.MARSIAU2perifocal'*[0; 0; 1]; + j = cross(k,I); + NO = [I,j,k]; + ON = NO'; + +% Rotation matrix from orbit's to Phobos fixed coordinates frame + OB = [cos(Xsi), sin(Xsi), 0; -sin(Xsi), cos(Xsi), 0; 0, 0, 1]; + BO = OB'; + + %% 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; + 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 + + gM = - 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 = gM + gJ2M; + +% 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); + + [gb,~,~,~] = SHGM_Full(BO*ON*rsb,pars.SH); + gPh = NO*OB*gb; + +% Trascinamento +% Tutto adimensionale + + G = pars.G; + M2 = pars.MPho; + M1 = pars.MMars; + + I1x = pars.IMx_bar*M1; + I1y = pars.IMy_bar*M1; + I1z = pars.IMz_bar*M1; + + +% Need those quantities + + A1 = [cos(theta+Phi), sin(theta+Phi), 0; -sin(theta+Phi), cos(theta+Phi), 0; 0, 0, 1]; + dA1dtheta = [-sin(theta+Phi), cos(theta+Phi), 0; -cos(theta+Phi), -sin(theta+Phi), 0; 0, 0, 0]; + A2 = [cos(theta+Xsi), sin(theta+Xsi), 0; -sin(theta+Xsi), cos(theta+Xsi), 0; 0, 0, 1]; + dA2dtheta = [-sin(theta+Xsi), cos(theta+Xsi), 0; -cos(theta+Xsi), -sin(theta+Xsi), 0; 0, 0, 0]; + + I1 = diag([I1x, I1y, I1z]); + I2 = diag([IPhx_bar, IPhy_bar, IPhz_bar]); + +% Chain rule terms + dRPhdx = rPh(1)/norm(rPh(1:3)); + dRPhdy = rPh(2)/norm(rPh(1:3)); + dRPhdz = rPh(3)/norm(rPh(1:3)); + dthetadx = -rPh(2)/(rPh(1)^2 + rPh(2)^2); + dthetady = rPh(1)/(rPh(1)^2 + rPh(2)^2); + dthetadz = 0; + dbetadx = -rPh(3)*rPh(1)/(sqrt(rPh(1)^2+rPh(2)^2)*sum(rPh(1:3).^2)); + dbetady = -rPh(3)*rPh(2)/(sqrt(rPh(1)^2+rPh(2)^2)*sum(rPh(1:3).^2)); + dbetadz = sqrt(rPh(1)^2+rPh(2)^2)/sum(rPh(1:3).^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); + +% 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; + +% Combined effect on MMX + g_tot = gM + gPh - g_Ph_on_M; + + +%% Output for the integrator + + dx(1+i*size_St:size_St+i*size_St,1) = [v; g_tot; RPh_dot;... + RPh_ddot; theta_dot; theta_ddot; Xsi_dot; Xsi_ddot;... + zeros(nCoeff,1); zeros(nBias,1)]; + + end + +end \ No newline at end of file diff --git a/Sigma_dynamics_NoPhi.m b/Sigma_dynamics_NoPhi.m new file mode 100644 index 0000000000000000000000000000000000000000..db590f051b9a4feaf4cb5ab87b06c7630db10e7e --- /dev/null +++ b/Sigma_dynamics_NoPhi.m @@ -0,0 +1,170 @@ +function dx = Sigma_dynamics_NoPhi(~,x,pars,~) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% dx = Sigma_dynamics_NoPhi(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 + + +%% Dimensions + + d = pars.d; % Dimension of the problem + nCoeff = pars.nCoeff; % N. of gravitational parsameters + nBias = pars.nBias; + + size_St = (d+6+nCoeff+nBias); + + +%% Useful quantities + + for i = 0:2*size_St-1 + +% Spacecraft state vector + r = x(1+i*size_St:3+i*size_St); % Pos. vector wrt central body + v = x(4+i*size_St:6+i*size_St); % Vel. vector wrt central body + R = norm(r); % Norm of pos. vector wrt central body + +% Phobos states + RPh = x(7+i*size_St); + RPh_dot = x(8+i*size_St); + theta = x(9+i*size_St); + theta_dot = x(10+i*size_St); + Phi = pars.PhiM; + Xsi = x(11+i*size_St); + Xsi_dot = x(12+i*size_St); + +% Phobos moment of inertia + IPhx_bar = x(13+i*size_St); + IPhy_bar = x(14+i*size_St); + IPhz_bar = x(15+i*size_St); + +% Position of MMX wrt Phobos + rPh = pars.perifocal2MARSIAU*RPh*[cos(theta); sin(theta); 0]; + rsb = r - rPh; + +% Rotation matrix from orbit's to Phobos fixed coordinates frame + I = rPh/RPh; + k = pars.MARSIAU2perifocal'*[0; 0; 1]; + j = cross(k,I); + NO = [I,j,k]; + ON = NO'; + +% Rotation matrix from orbit's to Phobos fixed coordinates frame + OB = [cos(Xsi), sin(Xsi), 0; -sin(Xsi), cos(Xsi), 0; 0, 0, 1]; + BO = OB'; + + %% 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; + 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 + + gM = - 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 = gM + gJ2M; + +% 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); + + [gb,~,~,~] = SHGM_Full(BO*ON*rsb,pars.SH); + gPh = NO*OB*gb; + +% Trascinamento +% Tutto adimensionale + + G = pars.G; + M2 = pars.MPho; + M1 = pars.MMars; + + I1x = pars.IMx_bar*M1; + I1y = pars.IMy_bar*M1; + I1z = pars.IMz_bar*M1; + + +% Need those quantities + + A1 = [cos(theta+Phi), sin(theta+Phi), 0; -sin(theta+Phi), cos(theta+Phi), 0; 0, 0, 1]; + dA1dtheta = [-sin(theta+Phi), cos(theta+Phi), 0; -cos(theta+Phi), -sin(theta+Phi), 0; 0, 0, 0]; + A2 = [cos(theta+Xsi), sin(theta+Xsi), 0; -sin(theta+Xsi), cos(theta+Xsi), 0; 0, 0, 1]; + dA2dtheta = [-sin(theta+Xsi), cos(theta+Xsi), 0; -cos(theta+Xsi), -sin(theta+Xsi), 0; 0, 0, 0]; + + I1 = diag([I1x, I1y, I1z]); + I2 = diag([IPhx_bar, IPhy_bar, IPhz_bar]); + +% Chain rule terms + dRPhdx = rPh(1)/norm(rPh(1:3)); + dRPhdy = rPh(2)/norm(rPh(1:3)); + dRPhdz = rPh(3)/norm(rPh(1:3)); + dthetadx = -rPh(2)/(rPh(1)^2 + rPh(2)^2); + dthetady = rPh(1)/(rPh(1)^2 + rPh(2)^2); + dthetadz = 0; + dbetadx = -rPh(3)*rPh(1)/(sqrt(rPh(1)^2+rPh(2)^2)*sum(rPh(1:3).^2)); + dbetady = -rPh(3)*rPh(2)/(sqrt(rPh(1)^2+rPh(2)^2)*sum(rPh(1:3).^2)); + dbetadz = sqrt(rPh(1)^2+rPh(2)^2)/sum(rPh(1:3).^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); + +% 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; + +% Combined effect on MMX + g_tot = gM + gPh - g_Ph_on_M; + + +%% Output for the integrator + + dx(1+i*size_St:size_St+i*size_St,1) = [v; g_tot; RPh_dot;... + RPh_ddot; theta_dot; theta_ddot; Xsi_dot; Xsi_ddot;... + zeros(nCoeff,1); zeros(nBias,1)]; + + end + +end \ No newline at end of file diff --git a/UKF_Tool.m b/UKF_Tool.m index a045229a18b375d8abd017dd1c0755142a9208d3..e1f1dfe7371b88094c2bca5e6028435d9fcc62f2 100644 --- a/UKF_Tool.m +++ b/UKF_Tool.m @@ -90,14 +90,8 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea min_interval = min([interval_Range; interval_Range_rate; interval_lidar; interval_camera]); switch n - case 14 - St0 = [Xold; pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar]; - case 17 - St0 = Xold(1:17); - case 18 - St0 = pars.X0_reference; case 20 - St0 = Xold(1:17); + St0 = pars.X0_reference(1:17); case 22 St0 = Xold(1:17); end @@ -117,31 +111,22 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea if Measures(i).t == told idx = 1; switch n - case 14 - Xmean_bar = X(idx,1:size(Est0.X,1))'; - case 17 - Xmean_bar = X(idx,:)'; - case 18 - Xmean_bar = [X(idx,1:10)'; X(idx,13:end)'; 0; 0; 0]; - pars.PhiM = X(idx,11); case 20 - Xmean_bar = [X(idx,:)'; 0; 0; 0]; + Xmean_bar = [X(idx,1:10)'; X(idx,13:end)'; 0; 0; 0; 0; 0]; + pars.PhiM = X(idx,11); case 22 Xmean_bar = [X(idx,:)'; 0; 0; 0; 0; 0]; end elseif Measures(i).t > told idx = find(round(t/units.tsf)==Measures(i).t); switch n - case 14 - Xmean_bar = X(idx,1:size(Est0.X,1))'; - case 17 - Xmean_bar = X(idx,:)'; - case 18 - Xmean_bar = [X(idx,1:10)'; X(idx,13:end)'; 0; 0; 0]; case 20 - Xmean_bar = [X(idx,:)'; 0; 0; 0]; + Xmean_bar = [X(idx,1:10)'; X(idx,13:end)'; 0; 0; 0; 0; 0]; + pars.PhiM = X(idx,11); + event = @landing_Phobos_UKF_NoPhiM; case 22 Xmean_bar = [X(idx,:)'; 0; 0; 0; 0; 0]; + event = @landing_Phobos_UKF; end end @@ -169,7 +154,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea % Propagation of the sigma points' trajectories St0 = reshape(X0,[n*(2*n),1]); tspan = (told:Measures(i).t)*units.tsf; - opt = odeset('RelTol',1e-13,'AbsTol',1e-16,'event',@(t,X) landing_Phobos_UKF(t,X,pars,units)); + opt = odeset('RelTol',1e-13,'AbsTol',1e-16,'event',@(t,X) event(t,X,pars,units)); [~,X_sigma] = ode113(@(t,X) fsigma(t,X,pars,units),tspan,St0,opt); Xbar = reshape(X_sigma(end,1:n*(2*n))',[n,(2*n)]); @@ -184,7 +169,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea case 20 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)])]; + diag([deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, zeros(1,n-12)])]; case 22 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*... diff --git a/UKF_features.m b/UKF_features.m index c605c4cab55e47a8eb7180181e9fbee4da353250..e861c96e76491e63592e646c1f4446dc677391ca 100644 --- a/UKF_features.m +++ b/UKF_features.m @@ -68,6 +68,7 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature prefit = NaN(size(O,1),m); y_t = NaN(size(O,1),m); X_t = zeros(n,size(Measures,1)); + x_t = zeros(n,size(Measures,1)); P_t = zeros(n,n,m); Pbar_t = zeros(n,n,m); @@ -100,7 +101,7 @@ 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, Features_ID, IDX] =... + [Y_obs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, check_Limb, Features_ID, IDX] =... Measurement_read(Measures(i),O,units); if Measures(i).t == told @@ -126,17 +127,10 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature deltaT = (Measures(i).t-told)*units.tsf; Q = pars.sigma^2*eye(3); switch n - case 6 - Gamma = [eye(q)*deltaT^2/2; eye(q)*deltaT]; - Q = Gamma*Q*Gamma'; - case 14 - 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])]; case 20 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)])]; + diag([deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, zeros(1,n-12)])]; case 22 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*... @@ -161,20 +155,19 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature 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, Features_ID, pars, units, O, file_features); + check_Lidar, check_Limb, Features_ID, pars, units, O, file_features); Y(:,j) = Y_sigma.meas; Y_feat{j} = Y_sigma.cam; if ~isempty(Y_feat{j}) Features_ID = intersect(Features_ID, Y_feat{j}(3,:)); end - end Y_sigma = []; if ~isempty(Y_feat{1}) - for j = 1:2*n+1 + for j = 1:2*n features = features_list(Y_feat{j}, Features_ID); Y_sigma = [Y_sigma, [Y(:,j); features]]; end @@ -184,7 +177,7 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature % Mean predicted measurement [Y_mean,R] = G(et, Xmean_bar, Stat_ID_Range, Stat_ID_Rate,... - check_Lidar, Features_ID, pars, units, O, file_features); + check_Lidar, check_Limb, Features_ID, pars, units, O, file_features); features= features_list(Y_mean.cam, Features_ID); Y_mean = [Y_mean.meas; features]; @@ -192,34 +185,43 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature features= features_list(Y_obs.cam, Features_ID); Y_obs = [Y_obs.meas; features]; + % Innovation covariance and cross covariance - R = [R; O(4,4)*ones(size(features,1),1)]; + R = [R; O(8,8)*ones(size(features,1),1)]; Py = diag(R); Pxy = zeros(n,size(Y_obs,1)); - for j = 1:2*n+1 - Py = Py + W0c(j)*(Y_sigma(:,j) - Y_mean)*(Y_sigma(:,j) - Y_mean)'; - Pxy = Pxy + W0c(j)*(X0(:,j) - Xmean_bar)*(Y_sigma(:,j) - Y_mean)'; - end - -% Kalman gain - K = Pxy/Py; + if size(R,1)>0 -% MEASUREMENT UPDATE - y = (Y_obs - Y_mean); - Xstar = Xmean_bar + K*y; - P = P_bar - K*Py*K'; - -% Next iteration preparation - Xold = Xstar; - told = Measures(i).t; - Pold = P; + for j = 1:2*n + Py = Py + W0c(j)*(Y_sigma(:,j) - Y_mean)*(Y_sigma(:,j) - Y_mean)'; + Pxy = Pxy + W0c(j)*(X0(:,j) - Xmean_bar)*(Y_sigma(:,j) - Y_mean)'; + end + + % Kalman gain + K = Pxy/Py; + % MEASUREMENT UPDATE + y = (Y_obs - Y_mean); + x_est = K*y; + Xstar = Xmean_bar + x_est; + + P = (P_bar - K*Py*K'); + + % Next iteration preparation + Xold = Xstar; + told = Measures(i).t; + Pold = P; + + else + fprintf('\nTracking delle features perso...'); + break + end % Residuals [err(:,i), prefit(:,i)] = Residuals_withCamera(Y_obs, G, Xmean_bar,... - Xmean_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, ... + Xstar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, check_Limb, ... Features_ID, O, IDX, et, pars, units, file_features); @@ -227,6 +229,7 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature P_t(:,:,i) = abs(P).*units.CovDim; Pbar_t(:,:,i) = P_bar.*units.CovDim; X_t(:,i) = Xold.*units.Dim; + x_t(:,i) = x_est.*units.Dim; tobs(i) = Measures(i).t; end @@ -243,6 +246,7 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature Est.pre = prefit; Est.y_t = y_t; Est.X_t = X_t; + Est.x_t = x_t; Est.P_t = P_t; Est.Pbar_t = Pbar_t; diff --git a/YObs.mat b/YObs.mat index e8126f38fbd64912bfcbaf4b51b5000a6ef1fa09..d094df9b9b9f88d797096bf5e30da7927038d85e 100644 Binary files a/YObs.mat and b/YObs.mat differ diff --git a/landing_Phobos_UKF_NoPhiM.m b/landing_Phobos_UKF_NoPhiM.m new file mode 100644 index 0000000000000000000000000000000000000000..dc29f016c35d7e6bbb33117e47f0ede817a71294 --- /dev/null +++ b/landing_Phobos_UKF_NoPhiM.m @@ -0,0 +1,41 @@ +function [value,isterminal,direction] = landing_Phobos_UKF_NoPhiM(~,X,par,units) + + d = par.d; % Dimension of the problem + nCoeff = par.nCoeff; % N. of gravitational parameters + nBias = par.nBias; + + size_St = (d+6+nCoeff+nBias); + + for i = 0:2*size_St-1 + +% Spacecraft state vector + r = X(1+i*size_St:3+i*size_St); % Pos. vector wrt central body + +% Phobos states + RPh = X(7+i*size_St); + theta = X(9+i*size_St); + + Phobos = par.perifocal2MARSIAU*[RPh*cos(theta); RPh*sin(theta); 0]; + + 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 = r - Phobos; + +% Integration is blocked if the spacecraft touch the ellipsoid + d = x(1)^2/alpha^2 + x(2)^2/beta^2 +... + x(3)^2/gamma^2 - 1; + + if d<=0 + fprintf('\nOne of the sigma point fell on Phobos...') + break; + end + + end + + value = d; + isterminal = 1; + direction = 0; + +end \ No newline at end of file diff --git a/visible_features.m b/visible_features.m index 612d44c84b96a768f5f576df7e4b28b57fcf78f4..2119bc4b17607595c608f8c0b1f6b96011a4187c 100644 --- a/visible_features.m +++ b/visible_features.m @@ -124,7 +124,7 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features, %% Plot of the Resulting sceene -% PlotGeometryAndLight(points, point_in_light, visible, r_sb_Ph, r_PhSun, pars, units); -% Plotfeatures_Pic(Y_pix, Y_pix_light, Y_pix_tot, pars); + % PlotGeometryAndLight(points, point_in_light, visible, r_sb_Ph, r_PhSun, pars, units); + % Plotfeatures_Pic(Y_pix, Y_pix_light, Y_pix_tot, pars); end \ No newline at end of file diff --git a/visible_features_model.m b/visible_features_model.m index 41bda2dc625adac0bea7d3081aee3bf46c432916..61e08897de5d62de9da9c57248603f84260848d8 100644 --- a/visible_features_model.m +++ b/visible_features_model.m @@ -105,11 +105,11 @@ function [Y_LOS, Y_pix] = visible_features_model(MMX, Phobos, Sun, Phi, file_fea z_cam = cross(v, I)/norm(cross(v, I)); y_cam = cross(z_cam, I)/norm(cross(z_cam, I)); - % angle_feat = acos(dot(I,-i_feat)); - % angle_sun = acos(dot(I,r_PhSun)); + angle_feat = acos(dot(I,-i_feat)); + angle_sun = acos(dot(I,r_PhSun)); - % if (angle_feat<pars.FOV)&&(angle_sun>pars.FOV)&&(dot(r_sb_Ph, point_in_light(1:3,n))>0) + if (angle_feat<pars.FOV)&&(angle_sun>pars.FOV)&&(dot(r_sb_Ph, point_in_light(1:3,n))>0) k = 1; visible = [candidate, visible]; LOS_feat = [y_cam'; z_cam'] * r_sf/norm(r_sf) + [random('Normal',0, pars.ObsNoise.camera);... @@ -121,7 +121,7 @@ function [Y_LOS, Y_pix] = visible_features_model(MMX, Phobos, Sun, Phi, file_fea pix_feat = pix_feat./repmat(pix_feat(3),3,1); Y_pix = [[pix_feat(1:2); point_in_light(end,n)], Y_pix]; - % end + end end