diff --git a/.DS_Store b/.DS_Store
index 7870c24353ff52011501f690a402807e6feea3a7..a085ba9f2eefe9ea7a47ac443a2bd50311b601f3 100644
Binary files a/.DS_Store and b/.DS_Store differ
diff --git a/Analisys_With_Features.m b/Analisys_With_Features.m
index 8598d6497811a12162e327663e4d770760857cbf..ca462a3660e0eadfeb97f9c3c1d0bdff2f45ce10 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_031_2x2_826891269_828619269.bsp'));
+ 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('Phobos_826891269_828619269.bsp'));
@@ -69,7 +69,7 @@ clc
par.alpha = 1;
par.beta = 2;
par.Nfeatures = 5;
- file_features = 'Principal_Phobos_Ogni5.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
diff --git a/CovarianceAnalysisParameters.m b/CovarianceAnalysisParameters.m
index def5904cc69321f7f6f1f75ec6793a7e971cbb1b..ba9bc4b3e2cb493684e77aec93622cd38faf8d50 100644
--- a/CovarianceAnalysisParameters.m
+++ b/CovarianceAnalysisParameters.m
@@ -50,11 +50,12 @@ cam.SixeY = 24; %[mm]
cam.pixARx = 1;
cam.pixARy = 1;
pars.camera_intrinsic = IntrinsicParameters(cam);
+pars.cam = cam;
pars.FOV = deg2rad(45);
pixels = 2472;
-pars.ObsNoise.camera = pars.FOV/pixels; % [rad/pixel]
-pars.ObsNoise.pixel = .25; % [n. pixel]
+pars.ObsNoise.camera = pars.FOV/pixels; % [rad/pixel]
+pars.ObsNoise.pixel = .25; % [n. pixel]
% Useful to adimensionalize the observables
@@ -72,10 +73,12 @@ pars.R(7,7) = pars.ObsNoise.lidar^2;
pars.R(8,8) = pars.ObsNoise.pixel^2;
% Seconds between observation
-pars.interval_Range = 3600; % s
-pars.interval_Range_rate = 600; % s
+pars.interval_Range = 600e10; % s
+pars.interval_Range_rate = 600e10; % s
pars.interval_lidar = 600; % s
-pars.interval_camera = 3600; % s
+pars.interval_camera = 600; % s
+pars.flag_Limb = 1;
+pars.flag_features = 1;
pars.cutoffLIDAR = 200; % km
diff --git a/Mars_LimbRange.m b/Mars_LimbRange.m
new file mode 100644
index 0000000000000000000000000000000000000000..5c20d67f8ebee058face44565ceb652de80aa9fb
--- /dev/null
+++ b/Mars_LimbRange.m
@@ -0,0 +1,75 @@
+function Limb_Range = Mars_LimbRange(MMX, Phobos, Sun, pars, units)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Y = Mars_LimbRange(MMX, Phobos, Sun, Phi, pars, units)
+%
+% Identify wether Mars is in the FOV and how far is it
+%
+% Input:
+%
+% MMX MMX state vector in the MARSIAU reference frame
+% Phobos Phobos state vector in the MARSIAU reference frame
+% Sun Sun state vector in the MARSIAU reference frame
+% pars parameters structure
+% units units structure
+%
+% Output:
+%
+% Limb_Range Range derived from Mars limb size
+%
+%
+%
+% Author: E.Ciccarelli
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+% Rotation from MARSIAU to Phobos radial reference frame
+
+ i_feat = Phobos(1:3)/norm(Phobos(1:3));
+ v = Phobos(4:6)/norm(Phobos(4:6));
+ k = cross(i_feat, v)/norm(cross(i_feat, v));
+ j = cross(k, i_feat)/norm(cross(k, i_feat));
+
+ R2rot = [i_feat, j, k]';
+
+% MMX-Phobos direction in the rotating reference frame
+ r_sb = MMX(1:3) - Phobos(1:3);
+ r_sb_Ph = R2rot*r_sb;
+ R_sb_Ph = norm(r_sb_Ph);
+ I_sb = r_sb_Ph/R_sb_Ph;
+
+% MMX-Mars direction in the rotating reference frame
+ r_sM = R2rot*MMX(1:3);
+ I_sM = r_sM/norm(r_sM);
+
+% Mars pole position in the Phobos rotating frame
+ Mars_pole = [0; 0; -pars.Mars.gamma] - Phobos(1:3);
+ R_ini = [0, -1, 0; 1, 0, 0; 0, 0, 1];
+ Mars_pole = R_ini*R2rot'*Mars_pole;
+ Mars_centre = zeros(3,1) - Phobos(1:3);
+ Mars_centre = R_ini*R2rot'*Mars_centre;
+
+% Sunlight direction in the rotating reference frame
+ Sun = R2rot*Sun(1:3);
+ I_Sun = Sun/norm(Sun);
+
+
+ [~, ~, PM] = ProjMatrix(-r_sb_Ph, pars, units);
+ MP = PM*[-Mars_pole; 1];
+ MP = MP./repmat(MP(3),3,1);
+ MC = PM*[-Mars_centre; 1];
+ MC = MC./repmat(MC(3),3,1);
+
+ if ((any(MC<0))||(any(MP<0))||((MC(1)>pars.cam.nPixX))||...
+ (MP(1)>pars.cam.nPixX)||(MC(2)>pars.cam.nPixY)||...
+ (MP(2)>pars.cam.nPixY))||(dot(r_sb_Ph, Mars_centre)>0)
+ Limb_Range = [];
+ else
+ Limb_radius = MP - MC;
+ Limb_Range = (sqrt(Limb_radius(1)^2 + Limb_radius(2)^2) + random('Normal',0, pars.ObsNoise.pixel));
+ % Plotfeatures_Pic(MC, MP, pars);
+ end
+
+
+end
\ No newline at end of file
diff --git a/Mars_LimbRange_model.m b/Mars_LimbRange_model.m
new file mode 100644
index 0000000000000000000000000000000000000000..67048418923f8deda884033bff80c859f2cc509b
--- /dev/null
+++ b/Mars_LimbRange_model.m
@@ -0,0 +1,77 @@
+function Limb_Range = Mars_LimbRange_model(MMX, Phobos, Sun, pars, units)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Y = Mars_LimbRange_model(MMX, Phobos, Sun, Phi, pars, units)
+%
+% Identify wether Mars is in the FOV and how far is it
+%
+% Input:
+%
+% MMX MMX state vector in the MARSIAU reference frame
+% Phobos Phobos state vector in the MARSIAU reference frame
+% Sun Sun state vector in the MARSIAU reference frame
+% pars parameters structure
+% units units structure
+%
+% Output:
+%
+% Limb_Range Range derived from Mars limb size
+%
+%
+%
+% Author: E.Ciccarelli
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+% Rotation from MARSIAU to Phobos radial reference frame
+
+ i_feat = Phobos(1:3)/norm(Phobos(1:3));
+ v = Phobos(4:6)/norm(Phobos(4:6));
+ k = cross(i_feat, v)/norm(cross(i_feat, v));
+ j = cross(k, i_feat)/norm(cross(k, i_feat));
+
+ R2rot = [i_feat, j, k]';
+
+% MMX-Phobos direction in the rotating reference frame
+ r_sb = MMX(1:3) - Phobos(1:3);
+ r_sb_Ph = R2rot*r_sb;
+ R_sb_Ph = norm(r_sb_Ph);
+ I_sb = r_sb_Ph/R_sb_Ph;
+
+% MMX-Mars direction in the rotating reference frame
+ r_sM = R2rot*MMX(1:3);
+ I_sM = r_sM/norm(r_sM);
+
+% Mars pole position in the Phobos rotating frame
+ Mars_pole = [0; 0; -pars.Mars.gamma] - Phobos(1:3);
+ R_ini = [0, -1, 0; 1, 0, 0; 0, 0, 1];
+ Mars_pole = R_ini*R2rot'*Mars_pole;
+ Mars_centre = zeros(3,1) - Phobos(1:3);
+ Mars_centre = R_ini*R2rot'*Mars_centre;
+
+% Sunlight direction in the rotating reference frame
+ Sun = R2rot*Sun(1:3);
+ I_Sun = Sun/norm(Sun);
+
+
+ [~, ~, PM] = ProjMatrix(-r_sb_Ph, pars, units);
+ MP = PM*[-Mars_pole; 1];
+ MP = MP./repmat(MP(3),3,1);
+ MC = PM*[-Mars_centre; 1];
+ MC = MC./repmat(MC(3),3,1);
+
+ if ((any(MC<0))||(any(MP<0))||((MC(1)>pars.cam.nPixX))||...
+ (MP(1)>pars.cam.nPixX)||(MC(2)>pars.cam.nPixY)||...
+ (MP(2)>pars.cam.nPixY))||(dot(r_sb_Ph, Mars_centre)>0)
+ Limb_Range = [];
+ else
+ Limb_radius = MP - MC;
+ Limb_Range = sqrt(Limb_radius(1)^2 + Limb_radius(2)^2);
+ % Plotfeatures_Pic(MC, MP, pars);
+
+ end
+
+
+
+end
\ No newline at end of file
diff --git a/Measurement_read.m b/Measurement_read.m
index 3c422c5f0bb46ed4f31d2e81fd10b59d10492159..db9229205dfd52b38c67e53156395ed55159b940 100644
--- a/Measurement_read.m
+++ b/Measurement_read.m
@@ -1,4 +1,5 @@
-function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID, IDX] = Measurement_read(Measures,O,units)
+function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, check_Limb,...
+ Features_ID, IDX] = Measurement_read(Measures,O,units)
IDX = [];
Y_obs = [];
@@ -63,10 +64,21 @@ function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID, IDX] =
check_Lidar = 1;
Lidar = Measures.Lidar;
Y_obs = [Y_obs; Lidar./units.lsf];
- R = [R; O(3,3)];
+ R = [R; O(7,7)];
IDX = [IDX, 7];
end
+ check_Limb = [];
+
+ if ~isempty(Measures.Limb)
+ check_Limb = 1;
+ Limb = Measures.Limb;
+ Y_obs = [Y_obs; Limb];
+ R = [R; O(8,8)];
+ IDX = [IDX, 8];
+ end
+
+
Yobs.meas = Y_obs;
if ~isempty(Measures.Features)
@@ -76,5 +88,7 @@ function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID, IDX] =
Yobs.cam = [];
end
+
+
end
\ No newline at end of file
diff --git a/Observables_model_with_Features.m b/Observables_model_with_Features.m
index 46503fb3e81239bb3b31025aff862c515b6506d8..36328b0e45f4a01ca6402cace81fc3bd849cbcac 100644
--- a/Observables_model_with_Features.m
+++ b/Observables_model_with_Features.m
@@ -1,5 +1,5 @@
function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rate,...
- Lidar_check, Feature_ID, pars, units, O, file_features)
+ Lidar_check, Limb_check, Feature_ID, pars, units, O, file_features)
%==========================================================================
% [G, R] = Observables_model_with_Features(et,Obs,X,St_ID_Range,St_ID_Rate, Feature_ID, par,units)
%
@@ -50,7 +50,8 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat
range_Mad = [];
range_rate_Mad = [];
lidar = [];
-
+ Limb = [];
+
Rm = [];
% Unpacking of the state vector
@@ -161,7 +162,7 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat
lidar = norm(rsb) - R_latlon/units.lsf + bias(3);
- Rm = [Rm; O(3,3)];
+ Rm = [Rm; O(7,7)];
end
@@ -185,18 +186,39 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat
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');
+ fprintf('\nTracking delle features perso!!\n');
return
end
end
-
+
else
Y_pix = [];
end
+ if Limb_check
+% There should be the Mars Limb
+ [Sun, ~] = cspice_spkezr('10', et, 'MARSIAU', 'none', '499');
+
+ R2Rot = [cos(X(9)), sin(X(9)), 0; -sin(X(9)), cos(X(9)), 0; 0, 0, 1];
+ 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];
+
+ 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
+
+ Rm = [Rm; O(8,8)];
+ end
+
+
% Ready to exit
G.meas = [range_Camb; range_Gold; range_Mad; range_rate_Camb; range_rate_Gold; ...
- range_rate_Mad; lidar];
+ range_rate_Mad; lidar; Limb];
G.cam = Y_pix;
G.meas(isnan(G.meas)) = [];
diff --git a/Observations_Generation.m b/Observations_Generation.m
index 7194e88981a753ecfc48e3aef1b55633f8dd1efe..20b7dbe7d02c3d2655d2e09b8e03040c5a104d5b 100644
--- a/Observations_Generation.m
+++ b/Observations_Generation.m
@@ -20,7 +20,7 @@ clc
MMX_InitializeSPICE
cspice_furnsh(which('mar097.bsp'));
cspice_furnsh(which('MARPHOFRZ.txt'));
- cspice_furnsh(which('MMX_QSO_031_2x2_826891269_828619269.bsp'));
+ 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('Phobos_826891269_828619269.bsp'));
@@ -30,7 +30,7 @@ clc
% Time of the analysis
data = '2026-03-16 00:00:00 (UTC)';
data = cspice_str2et(data);
- Ndays = 5;
+ Ndays = 3.5;
t = Ndays*86400;
date_end = data+t;
@@ -40,7 +40,7 @@ clc
% Covariance analysis parameters
pars.et0 = data;
[pars, units] = CovarianceAnalysisParameters(pars, units);
- file_features = 'Principal_Phobos_Ogni5.mat';
+ file_features = 'Principal_Phobos_ogni10.mat';
% Positin of my target bodies at that date
Sun = cspice_spkezr('10', data, 'J2000', 'none', 'MARS');
@@ -51,11 +51,11 @@ clc
%% Creazione lista osservabili
YObs = Observations_with_features(data, date_end, file_features, pars, units);
-% YObs(1).Range = [];
-% YObs(1).Rate = [];
-% YObs(1).Lidar = [];
-% YObs(1) = [];
-% YObs(1).features = [];
+ YObs(1).Range = [];
+ YObs(1).Rate = [];
+ % YObs(1).Lidar = [];
+ % YObs(1) = [];
+ % YObs(1).features = [];
-% save('~/tapyr/Edo/YObs');
+ % save('/Users/ec01259/Documents/pythoncodes/tapyr/YObs');
save('YObs');
\ No newline at end of file
diff --git a/Observations_with_features.m b/Observations_with_features.m
index c351e6e9f48795dcf7a023f0c2f8ddd92bfade11..09da5fce356731f7dde8372991bc7934404eabff 100644
--- a/Observations_with_features.m
+++ b/Observations_with_features.m
@@ -195,32 +195,51 @@ function YObs = Observations_with_features(date_0, date_end, file_features, pars
% Check if Phobos is in light or not
Phobos = cspice_spkezr('-401', i, 'J2000', 'none', 'SUN');
MMX = cspice_spkezr('-34', i, 'J2000', 'none', 'SUN');
+ Mars = cspice_spkezr('499', i, 'J2000', 'none', 'SUN');
R_PS = -Phobos(1:3)/norm(Phobos(1:3));
+ R_MS = -Mars(1:3)/norm(Mars(1:3));
R_MMXP = (MMX(1:3) - Phobos(1:3))/norm(MMX(1:3) - Phobos(1:3));
% Is Mars between Phobos and the Sun?
check3 = cspice_occult('-401','POINT',' ',...
'499','ELLIPSOID','IAU_MARS','none','SUN',i);
- if (dot(R_MMXP,R_PS)>0)&&(check3>=0)&&(rem(round(i-et0),interval_camera)==0)
-
- Sun = cspice_spkezr('10', i, 'MARSIAU', 'none', 'MARS');
- MMX = cspice_spkezr('-34', i, 'MARSIAU', 'none', 'MARS');
- Phobos = cspice_spkezr('-401', i, 'MARSIAU', 'none', 'MARS');
-
+ Sun = cspice_spkezr('10', i, 'MARSIAU', 'none', 'MARS');
+ MMX = cspice_spkezr('-34', i, 'MARSIAU', 'none', 'MARS');
+ Phobos = cspice_spkezr('-401', i, 'MARSIAU', 'none', 'MARS');
+
+
+ if (dot(R_MMXP,R_PS)>0)&&(check3>=0)&&(rem(round(i-et0),interval_camera)==0)&&pars.flag_features
+
% What's the Phobos libration angle at that t?
Phi = Phi_t(t == (i-et0));
% Identification of the features
[~, Y_pix] = visible_features(MMX, Phobos, Sun, Phi,...
file_features, pars, units);
- got_it = 1;
+
+ if ~isempty(Y_pix)
+ got_it = 1;
+ end
else
- Y_LOS = [];
- Y_pix = [];
+ Y_LOS = [];
+ Y_pix = [];
+ Limb = [];
end
-
+
+ if (rem(round(i-et0),interval_camera)==0)&&pars.flag_Limb
+
+% Is there Mars's Limb?
+ Limb = Mars_LimbRange(MMX, Phobos, Sun, pars, units);
+
+ if ~isempty(Limb)
+ got_it = 1;
+ end
+
+ end
+
+
%--------------------------------------------------------------------------
% NEW LINE SETUP
@@ -244,6 +263,7 @@ function YObs = Observations_with_features(date_0, date_end, file_features, pars
Obs.Lidar = Lidar;
Obs.Features= Y_pix;
+ Obs.Limb = Limb;
YObs = [YObs; Obs];
end
end
diff --git a/Residuals_withCamera.m b/Residuals_withCamera.m
index 0080fc5dae54c8dba7c4a319715ae517ecc7b12a..fb575882c21db09e8c48726b90c846ea0ff9932b 100644
--- a/Residuals_withCamera.m
+++ b/Residuals_withCamera.m
@@ -1,5 +1,5 @@
function [err, prefit] = Residuals_withCamera(Y_obs, G, X_bar, X_star, Stat_ID_Range, Stat_ID_Rate, check_Lidar, ...
- Features_ID, O, IDX, et, pars, units, file_features)
+ check_Limb, Features_ID, O, IDX, et, pars, units, file_features)
% Preallocation
@@ -7,11 +7,11 @@ function [err, prefit] = Residuals_withCamera(Y_obs, G, X_bar, X_star, Stat_ID_R
prefit = nan(size(O,1),1);
% Definition of the residuals
- Y_pre = G(et, X_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, ...
+ Y_pre = G(et, X_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, check_Limb, ...
Features_ID, pars, units, O, file_features)';
features = features_list(Y_pre.cam, Features_ID);
Y_pre = [Y_pre.meas; features];
- Y_err = G(et, X_star, Stat_ID_Range, Stat_ID_Rate, check_Lidar, ...
+ Y_err = G(et, X_star, Stat_ID_Range, Stat_ID_Rate, check_Lidar, check_Limb, ...
Features_ID, pars, units, O, file_features)';
features = features_list(Y_err.cam, Features_ID);
Y_err = [Y_err.meas; features];
diff --git a/Results.m b/Results.m
index 08a3097d87e49ddeee0e56272c2a5913dd1aa2b3..f22b8a303b9c4180fc91ada3e759ddc578999708 100644
--- a/Results.m
+++ b/Results.m
@@ -40,65 +40,65 @@ function Results(Est, YObs_Full, pars, units)
% Plot of the 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)
+% 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
diff --git a/UKF_Tool.m b/UKF_Tool.m
index ff3cf64680419d605dd935975a1cba5cc9618cd3..a045229a18b375d8abd017dd1c0755142a9208d3 100644
--- a/UKF_Tool.m
+++ b/UKF_Tool.m
@@ -155,7 +155,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
(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);
@@ -210,7 +210,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
% 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;
@@ -233,7 +233,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
% 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];
@@ -244,7 +244,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
% 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));
@@ -277,7 +277,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
% Residuals
[err(:,i), prefit(:,i)] = Residuals_withCamera(Y_obs, G, Xmean_bar,...
- Xmean_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, ...
+ Xmean_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, check_Limb, ...
Features_ID, O, IDX, et, pars, units, file_features);
diff --git a/UKF_features.m b/UKF_features.m
index c3f21ac498b8d12283cfbc7862ad0b8bc533b800..c605c4cab55e47a8eb7180181e9fbee4da353250 100644
--- a/UKF_features.m
+++ b/UKF_features.m
@@ -68,7 +68,6 @@ 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);
@@ -101,8 +100,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, Features_ID] = Measurement_read(Measures(i),O,units);
-
+ [Y_obs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID, IDX] =...
+ Measurement_read(Measures(i),O,units);
if Measures(i).t == told
@@ -138,6 +137,10 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature
Gamma = [eye(q)*deltaT^2/2; eye(q)*deltaT; zeros(n-6,q)];
Q = Gamma*Q*Gamma' + [zeros(6,n); zeros(n-6,6), pars.sigmaPh^2*...
diag([deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, zeros(1,n-14)])];
+ case 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*...
+ diag([deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, deltaT^2/2, deltaT, zeros(1,n-14)])];
end
P_bar = Q;
@@ -214,9 +217,10 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature
Pold = P;
-% err(IDX,i) = (Y_obs - G(et,obs(:,i),Xold,par,units)').*units.Observations(IDX);
-% prefit(IDX,i) = (Y_obs - G(et,obs(:,i),Xmean_bar,par,units)').*units.Observations(IDX);
-% y_t(IDX,i) = y.*units.Observations(IDX);
+% Residuals
+ [err(:,i), prefit(:,i)] = Residuals_withCamera(Y_obs, G, Xmean_bar,...
+ Xmean_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, ...
+ Features_ID, O, IDX, et, pars, units, file_features);
% Storing the results
diff --git a/YObs.mat b/YObs.mat
index a6fb5c73277e0284d1f3b096dcb6efbd3d06d6a6..0c417514d7b3e3d30c4efd06924bbede9b577607 100644
Binary files a/YObs.mat and b/YObs.mat differ
diff --git a/info_Matlab.mat b/info_Matlab.mat
new file mode 100644
index 0000000000000000000000000000000000000000..726ecd1a25e463d2348f4919043fc4fc03e41178
Binary files /dev/null and b/info_Matlab.mat differ
diff --git a/visible_features.m b/visible_features.m
index 6813aafa44ba609307d0fdb6740fbe2011009e8b..612d44c84b96a768f5f576df7e4b28b57fcf78f4 100644
--- a/visible_features.m
+++ b/visible_features.m
@@ -115,7 +115,7 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
[~, ~, PM] = ProjMatrix(-r_sb_Ph, pars, units);
pix_feat = PM*[-candidate; 1];
pix_feat = pix_feat./repmat(pix_feat(3),3,1);
- Y_pix = [[round(pix_feat(1:2)+ [random('Normal',0, pars.ObsNoise.pixel);...
+ Y_pix = [[(pix_feat(1:2)+ [random('Normal',0, pars.ObsNoise.pixel);...
random('Normal',0, pars.ObsNoise.pixel)]); point_in_light(end,n)], Y_pix];
end
@@ -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