diff --git a/.DS_Store b/.DS_Store
index c327f86ebf8f4282c4497c2e6083bb740f97d86f..7870c24353ff52011501f690a402807e6feea3a7 100644
Binary files a/.DS_Store and b/.DS_Store differ
diff --git a/Analisys_With_Features.m b/Analisys_With_Features.m
index 57feef2e7f3a5030922d6040f33136664315ec52..8598d6497811a12162e327663e4d770760857cbf 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_049_2x2_826891269_828619269.bsp'));
+ cspice_furnsh(which('MMX_QSO_031_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'));
@@ -68,7 +68,8 @@ clc
par.alpha = 1;
par.beta = 2;
- file_features = 'Principal_Phobos_Ogni10.mat';
+ par.Nfeatures = 5;
+ file_features = 'Principal_Phobos_Ogni5.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 83ddae6c19f0de4e67eb4671a31250c3ac7d0a05..def5904cc69321f7f6f1f75ec6793a7e971cbb1b 100644
--- a/CovarianceAnalysisParameters.m
+++ b/CovarianceAnalysisParameters.m
@@ -61,7 +61,7 @@ pars.ObsNoise.pixel = .25; % [n. pixel]
units.Observations = [units.lsf; units.vsf; units.lsf; units.vsf; units.lsf; units.vsf; units.lsf; 1; 1];
% Matrix of the observation noise
-pars.R = zeros(9,9);
+pars.R = zeros(8,8);
pars.R(1,1) = pars.ObsNoise.range^2;
pars.R(2,2) = pars.ObsNoise.range_rate^2;
pars.R(3,3) = pars.ObsNoise.range^2;
@@ -69,11 +69,10 @@ pars.R(4,4) = pars.ObsNoise.range_rate^2;
pars.R(5,5) = pars.ObsNoise.range^2;
pars.R(6,6) = pars.ObsNoise.range_rate^2;
pars.R(7,7) = pars.ObsNoise.lidar^2;
-pars.R(8,8) = pars.ObsNoise.camera^2;
-pars.R(9,9) = pars.ObsNoise.camera^2;
+pars.R(8,8) = pars.ObsNoise.pixel^2;
% Seconds between observation
-pars.interval_Range = 3600e10; % s
+pars.interval_Range = 3600; % s
pars.interval_Range_rate = 600; % s
pars.interval_lidar = 600; % s
pars.interval_camera = 3600; % s
diff --git a/Measurement_read.m b/Measurement_read.m
index ce8ff439f8c9d83ec5170df836e617e65e7d4853..3c422c5f0bb46ed4f31d2e81fd10b59d10492159 100644
--- a/Measurement_read.m
+++ b/Measurement_read.m
@@ -1,5 +1,6 @@
-function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID] = Measurement_read(Measures,O,units)
+function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID, IDX] = Measurement_read(Measures,O,units)
+ IDX = [];
Y_obs = [];
R = [];
Stat_ID_Range = [];
@@ -12,14 +13,17 @@ function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID] = Meas
if ~isnan(Measures.Range(1))
Stat_ID_Range = [Stat_ID_Range; 1];
+ IDX = [IDX, 1];
end
if ~isnan(Measures.Range(2))
Stat_ID_Range = [Stat_ID_Range; 2];
+ IDX = [IDX, 3];
end
if ~isnan(Measures.Range(3))
Stat_ID_Range = [Stat_ID_Range; 3];
+ IDX = [IDX, 5];
end
Ranges = Measures.Range(which);
@@ -34,19 +38,23 @@ function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID] = Meas
if ~isnan(Measures.Rate(1))
Stat_ID_Rate = [Stat_ID_Rate; 1];
+ IDX = [IDX, 2];
end
if ~isnan(Measures.Rate(2))
Stat_ID_Rate = [Stat_ID_Rate; 2];
+ IDX = [IDX, 4];
end
if ~isnan(Measures.Rate(3))
Stat_ID_Rate = [Stat_ID_Rate; 3];
+ IDX = [IDX, 6];
end
Rates = Measures.Rate(which);
Y_obs = [Y_obs; Rates./units.vsf];
R = [R; ones(count,1)*O(2,2)];
+
end
check_Lidar = [];
@@ -56,6 +64,7 @@ function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID] = Meas
Lidar = Measures.Lidar;
Y_obs = [Y_obs; Lidar./units.lsf];
R = [R; O(3,3)];
+ IDX = [IDX, 7];
end
Yobs.meas = Y_obs;
@@ -67,4 +76,5 @@ function [Yobs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID] = Meas
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 a413e1960c69ffdcbf46a967a36ee19f747a782c..46503fb3e81239bb3b31025aff862c515b6506d8 100644
--- a/Observables_model_with_Features.m
+++ b/Observables_model_with_Features.m
@@ -168,11 +168,11 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat
if ~isempty(Feature_ID)
% There should be features
- [Sun, ~] = cspice_spkezr('10',et,'MARSIAU','none','499');
+ [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]));
+ 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);
@@ -195,8 +195,8 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat
end
% Ready to exit
- G.meas = [range_Camb; range_rate_Camb; range_Gold; range_rate_Gold; ...
- range_Mad; range_rate_Mad; lidar];
+ G.meas = [range_Camb; range_Gold; range_Mad; range_rate_Camb; range_rate_Gold; ...
+ range_rate_Mad; lidar];
G.cam = Y_pix;
G.meas(isnan(G.meas)) = [];
diff --git a/Observations_Generation.m b/Observations_Generation.m
index 4a69054d1f8028ef50c7e981f25fdab50fa0a413..7194e88981a753ecfc48e3aef1b55633f8dd1efe 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_049_2x2_826891269_828619269.bsp'));
+ cspice_furnsh(which('MMX_QSO_031_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 = 2;
+ Ndays = 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_Ogni10.mat';
+ file_features = 'Principal_Phobos_Ogni5.mat';
% Positin of my target bodies at that date
Sun = cspice_spkezr('10', data, 'J2000', 'none', 'MARS');
@@ -51,10 +51,10 @@ 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).Range = [];
+% YObs(1).Rate = [];
+% YObs(1).Lidar = [];
+% YObs(1) = [];
% YObs(1).features = [];
% save('~/tapyr/Edo/YObs');
diff --git a/Observations_with_features.m b/Observations_with_features.m
index ec197559fcea194a8dd6563658ba25199698470e..c351e6e9f48795dcf7a023f0c2f8ddd92bfade11 100644
--- a/Observations_with_features.m
+++ b/Observations_with_features.m
@@ -204,9 +204,9 @@ function YObs = Observations_with_features(date_0, date_end, file_features, pars
if (dot(R_MMXP,R_PS)>0)&&(check3>=0)&&(rem(round(i-et0),interval_camera)==0)
- Sun = cspice_spkezr('10', i, 'J2000', 'none', 'MARS');
- MMX = cspice_spkezr('-34', i, 'J2000', 'none', 'MARS');
- Phobos = cspice_spkezr('-401', i, 'J2000', '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');
% What's the Phobos libration angle at that t?
Phi = Phi_t(t == (i-et0));
diff --git a/Plotfeatures_Pic.m b/Plotfeatures_Pic.m
index 1df68378f1ca061ee42648f9791ff3b943c85554..6895aa62db13a27b891f4a598da62f6eb2b00962 100644
--- a/Plotfeatures_Pic.m
+++ b/Plotfeatures_Pic.m
@@ -1,15 +1,87 @@
-function Plotfeatures_Pic(Y_pix, pars)
+function Plotfeatures_Pic(varargin)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Plotfeatures_Pic(Y_pix, Y_pix_light, Y_pix_tot, pars)
+%
+% Plot the interested features in the image plate
+%
+% Input:
+%
+% Y_pix Visible features
+% Y_pix_light Features in light but maybe hidden
+% Y_pix_tot All the body's features
+% pars Structure with problem's parameters
+%
+%
+%
+% Author: E.Ciccarelli
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- figure()
- grid on
- axis equal
- hold on
- plot(Y_pix(1,:), Y_pix(2,:), 'o', 'Color', 'r');
- xlabel('X','FontSize',16,'Interpreter','latex')
- ylabel('Y','FontSize',16,'Interpreter','latex')
- xlim([0, 2*pars.camera_intrinsic(3,1)])
- ylim([0, 2*pars.camera_intrinsic(3,2)])
+ switch size(varargin,2)
+ case 2
+ Y_pix = varargin{1};
+ pars = varargin{2};
+
+ figure(99)
+ grid on
+ axis equal
+ if ~isempty(Y_pix)
+ plot(Y_pix(1,:), Y_pix(2,:), 'o', 'Color', 'r');
+ end
+ hold on
+ xlabel('X','FontSize',16,'Interpreter','latex')
+ ylabel('Y','FontSize',16,'Interpreter','latex')
+ xlim([0, 2*pars.camera_intrinsic(3,1)])
+ ylim([0, 2*pars.camera_intrinsic(3,2)])
+ hold off
+ pause(.5)
+
+ case 3
+ Y_pix = varargin{1};
+ Y_pix_light = varargin{2};
+ pars = varargin{3};
+
+ figure(99)
+ grid on
+ axis equal
+ plot(Y_pix_light(1,:), Y_pix_light(2,:), '*', 'Color', 'g');
+ hold on
+ if ~isempty(Y_pix)
+ plot(Y_pix(1,:), Y_pix(2,:), 'o', 'Color', 'r');
+ end
+ xlabel('X','FontSize',16,'Interpreter','latex')
+ ylabel('Y','FontSize',16,'Interpreter','latex')
+ xlim([0, 2*pars.camera_intrinsic(3,1)])
+ ylim([0, 2*pars.camera_intrinsic(3,2)])
+ hold off
+ pause(.5)
+
+ case 4
+ Y_pix = varargin{1};
+ Y_pix_light = varargin{2};
+ Y_pix_tot = varargin{3};
+ pars = varargin{4};
+
+ figure(99)
+ grid on
+ axis equal
+ plot(Y_pix_light(1,:), Y_pix_light(2,:), '*', 'Color', 'g');
+ hold on
+ if ~isempty(Y_pix)
+ plot(Y_pix(1,:), Y_pix(2,:), 'o', 'Color', 'r');
+ end
+ plot(Y_pix_tot(1,:), Y_pix_tot(2,:), '.', 'Color', 'b');
+ xlabel('X','FontSize',16,'Interpreter','latex')
+ ylabel('Y','FontSize',16,'Interpreter','latex')
+ xlim([0, 2*pars.camera_intrinsic(3,1)])
+ ylim([0, 2*pars.camera_intrinsic(3,2)])
+ hold off
+ pause(.5)
+
+ end
+
end
\ No newline at end of file
diff --git a/Residuals_withCamera.m b/Residuals_withCamera.m
new file mode 100644
index 0000000000000000000000000000000000000000..0080fc5dae54c8dba7c4a319715ae517ecc7b12a
--- /dev/null
+++ b/Residuals_withCamera.m
@@ -0,0 +1,32 @@
+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)
+
+
+% Preallocation
+ err = nan(size(O,1),1);
+ prefit = nan(size(O,1),1);
+
+% Definition of the residuals
+ Y_pre = G(et, X_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, ...
+ 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, ...
+ Features_ID, pars, units, O, file_features)';
+ features = features_list(Y_err.cam, Features_ID);
+ Y_err = [Y_err.meas; features];
+
+ if isempty(Features_ID)
+ err(IDX) = Y_obs - Y_pre;
+ prefit(IDX) = Y_obs - Y_err;
+ else
+ err(IDX) = Y_obs(1:size(IDX,2)) - Y_err(1:size(IDX,2));
+ prefit(IDX) = Y_obs(1:size(IDX,2)) - Y_pre(1:size(IDX,2));
+
+ err(8) = Y_obs(size(IDX,2)+1) - Y_err(size(IDX,2)+1);
+ prefit(8) = Y_obs(size(IDX,2)+1) - Y_pre(size(IDX,2)+1);
+
+ end
+
+
+end
\ No newline at end of file
diff --git a/Results.m b/Results.m
index 417ed05de9ad1ed2f3287fd019f807b290f3275a..08a3097d87e49ddeee0e56272c2a5913dd1aa2b3 100644
--- a/Results.m
+++ b/Results.m
@@ -40,95 +40,66 @@ function Results(Est, YObs_Full, pars, units)
% Plot of the post-fit residuals
-% figure(1)
-% subplot(1,2,1)
-% plot(t_obs/3600,Est.pre(1,:),'x','Color','b')
-% grid on;
-% hold on;
-% plot(t_obs/3600,Est.pre(3,:),'x','Color','b')
-% plot(t_obs/3600,Est.pre(5,:),'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')
-% plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(3,:),2)),'.-','Color','b')
-% plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(3,:),2)),'.-','Color','b')
-% plot(t_obs/3600,3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(5,:),2)),'.-','Color','b')
-% plot(t_obs/3600,-3*pars.ObsNoise.range*units.lsf*ones(size(Est.pre(5,:),2)),'.-','Color','b')
-% xlabel('$[hour]$')
-% ylabel('$[km]$')
-% title('Range Pre-fit residual')
-% figure(2)
-% subplot(1,2,1)
-% plot(t_obs/3600,Est.pre(2,:),'x','Color','r')
-% grid on;
-% hold on;
-% plot(t_obs/3600,Est.pre(4,:),'x','Color','r')
-% plot(t_obs/3600,Est.pre(6,:),'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')
-% plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(4,:),2)),'.-','Color','r')
-% plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(4,:),2)),'.-','Color','r')
-% plot(t_obs/3600,3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(6,:),2)),'.-','Color','r')
-% plot(t_obs/3600,-3*pars.ObsNoise.range_rate*units.vsf*ones(size(Est.pre(6,:),2)),'.-','Color','r')
-% xlabel('$[hour]$')
-% ylabel('$[km/s]$')
-% title('RangeRate Pre-fit residual')
-% figure(3)
-% subplot(1,2,1)
-% plot(t_obs/3600,Est.pre(7,:),'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')
-% figure(4)
-% subplot(1,2,1)
-% plot(t_obs/3600,Est.pre(8,:),'x','Color','m')
-% grid on;
-% hold on;
-% plot(t_obs/3600,3*pars.ObsNoise.camera*ones(size(Est.pre(8,:),2)),'.-','Color','m')
-% plot(t_obs/3600,-3*pars.ObsNoise.camera*ones(size(Est.pre(8,:),2)),'.-','Color','m')
-% xlabel('$[hour]$')
-% ylabel('$[rad]$')
-% title('Camera Pre-fit residual')
-% figure(5)
-% subplot(1,2,1)
-% plot(t_obs/3600,Est.pre(9,:),'x','Color','k')
-% grid on;
-% hold on;
-% plot(t_obs/3600,3*pars.ObsNoise.camera*ones(size(Est.pre(9,:),2)),'.-','Color','k')
-% plot(t_obs/3600,-3*pars.ObsNoise.camera*ones(size(Est.pre(9,:),2)),'.-','Color','k')
-% xlabel('$[hour]$')
-% ylabel('$[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')
-% figure(1)
-% subplot(1,2,2)
-% histfit([Est.pre(1,:), Est.pre(3,:), Est.pre(5,:)])
-% xlabel('$[km]$')
-% title('Range Pre-fit residual')
-% figure(2)
-% subplot(1,2,2)
-% histfit([Est.pre(2,:),Est.pre(4,:), Est.pre(6,:)])
-% xlabel('$[km/s]$')
-% title('Range Rate Pre-fit residual')
-% figure(3)
-% subplot(1,2,2)
-% histfit(Est.pre(7,:))
-% xlabel('$[km]$')
-% title('Lidar Pre-fit residual')
-% figure(4)
-% subplot(1,2,2)
-% histfit(Est.pre(8,:))
-% xlabel('$[rad]$')
-% title('Camera Pre-fit residual')
-% figure(5)
-% subplot(1,2,2)
-% histfit(Est.pre(9,:))
-% xlabel('$[rad]$')
-% 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 1f0f79f2e4de3d43de90b32b93a77687a750bd8f..ff3cf64680419d605dd935975a1cba5cc9618cd3 100644
--- a/UKF_Tool.m
+++ b/UKF_Tool.m
@@ -52,7 +52,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
% The measures must be organized in colums depending on the type of
% measure; each row is reffered to a given measure's time.
- obs = Measures;
+ obs = Measures;
% n,l and m are respectively the number of states, number of different
@@ -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] =...
+ [Y_obs, R, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID, IDX] =...
Measurement_read(Measures(i),O,units);
@@ -181,18 +181,6 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
deltaT = (Measures(i).t-told)*units.tsf;
Q = pars.sigma^2*eye(3);
switch n
- 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 17
- 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 18
- 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, zeros(1,n-12)])];
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*...
@@ -212,8 +200,8 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
% Sigma point redefinition
S_bar = real(sqrtm(P_bar));
- X0 = [(repmat(Xmean_bar,1,n)-gamma.*S_bar),...
- (repmat(Xmean_bar,1,n)+gamma.*S_bar)];
+ X0 = [(repmat(Xmean_bar,1,n) - gamma.*S_bar),...
+ (repmat(Xmean_bar,1,n) + gamma.*S_bar)];
Y = zeros(size(Y_obs.meas,1),2*n);
Y_feat = cell(2*n);
@@ -253,6 +241,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
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)];
@@ -284,9 +273,12 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
break
end
-% err(IDX,i) = (Y_obs - G(et, Xmean_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID, par,units)');
-% prefit(IDX,i) = (Y_obs - G(et, Xmean_bar, Stat_ID_Range, Stat_ID_Rate, check_Lidar, Features_ID, par,units)');
-% y_t(IDX,i) = y;
+
+
+% 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 b49c8bc058e06e419c86592fc29341fb688e8d2f..a6fb5c73277e0284d1f3b096dcb6efbd3d06d6a6 100644
Binary files a/YObs.mat and b/YObs.mat differ
diff --git a/visible_features.m b/visible_features.m
index 0f584e6700e56a6f17f8cb56803d9e669786059a..6813aafa44ba609307d0fdb6740fbe2011009e8b 100644
--- a/visible_features.m
+++ b/visible_features.m
@@ -20,18 +20,7 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% Features defined in the Phobos-fixed reference frame
- load(file_features);
- R_ini = [0, -1, 0; 1, 0, 0; 0, 0, 1];
- Point_Cloud = R_ini*Point_Cloud';
- points = zeros(4,size(Point_Cloud,2));
-
- for n = 1:size(points,2)
- points(:,n) = [Point_Cloud(:,n); n];
- end
-
-
-%% Identification of the features that are in light
+%% Geometry
% Rotation from J200 to Phobos radial reference frame
@@ -49,8 +38,39 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
% Light rays direction in the Phobos-fixed reference frame
r_PhSun = RLibration*R2rot*(Sun(1:3) - Phobos(1:3))/norm(Sun(1:3) - Phobos(1:3));
+
+% Position of MMX wrt Phobos-fixed frame
+
+ r_sb = MMX(1:3) - Phobos(1:3);
+ R_sb = norm(r_sb);
+ r_sb_Ph = RLibration*R2rot*r_sb;
+
+% MMX-Phobos direction in the Phobos-fixed reference frame
+ I = r_sb_Ph/R_sb;
+
+
+% Features defined in the Phobos-fixed reference frame
+ load(file_features);
+ R_ini = [0, -1, 0; 1, 0, 0; 0, 0, 1];
+ Point_Cloud = R_ini*Point_Cloud';
+ points = zeros(4,size(Point_Cloud,2));
+ Y_pix_tot = zeros(2,size(Point_Cloud,2));
+
+ for n = 1:size(points,2)
+ points(:,n) = [Point_Cloud(:,n); n];
+ [~, ~, PM] = ProjMatrix(-r_sb_Ph, pars, units);
+ pix_feat = PM*[-points(1:3,n); 1];
+ pix_feat = pix_feat./repmat(pix_feat(3),3,1);
+ Y_pix_tot = [round(pix_feat(1:2)), Y_pix_tot];
+ end
+
+
+%% Identification of the features that are in light
+
+
% Scan on the features to distinguish those in light
- point_in_light = [];
+ point_in_light = [];
+ Y_pix_light = [];
for n = 1:size(points,2)
% If the dot product of the incoming light ray and the feature
@@ -58,23 +78,16 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
% then it is in light
if dot(r_PhSun, points(1:3,n))<0
point_in_light = [points(:,n), point_in_light];
+ [~, ~, PM] = ProjMatrix(-r_sb_Ph, pars, units);
+ pix_feat = PM*[-points(1:3,n); 1];
+ pix_feat = pix_feat./repmat(pix_feat(3),3,1);
+ Y_pix_light = [round(pix_feat(1:2)), Y_pix_light];
end
end
-
%% Between those features, which ones are inside the FOV?
-
-% Position of MMX wrt Phobos-fixed frame
-
- r_sb = MMX(1:3) - Phobos(1:3);
- R_sb = norm(r_sb);
- r_sb_Ph = RLibration*R2rot*r_sb;
-
-% MMX-Phobos direction in the Phobos-fixed reference frame
- I = r_sb_Ph/R_sb;
-
visible = [];
Y_LOS = [];
Y_pix = [];
@@ -89,9 +102,11 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
z_cam = cross(v, I)/norm(cross(v, I));
y_cam = cross(z_cam, I)/norm(cross(z_cam, I));
- angle = acos(dot(I,-i_feat));
+ angle_feat = acos(dot(I,-i_feat));
+ angle_sun = acos(dot(I,r_PhSun));
+
- if (angle<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)
visible = [candidate, visible];
LOS_feat = [y_cam'; z_cam'] * r_sf/norm(r_sf) + [random('Normal',0, pars.ObsNoise.camera);...
random('Normal',0, pars.ObsNoise.camera)];
@@ -110,6 +125,6 @@ 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, pars);
+% 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 3c744a9e4581874cbaba398642d4aee006650fc8..61e08897de5d62de9da9c57248603f84260848d8 100644
--- a/visible_features_model.m
+++ b/visible_features_model.m
@@ -20,18 +20,7 @@ function [Y_LOS, Y_pix] = visible_features_model(MMX, Phobos, Sun, Phi, file_fea
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% Features defined in the Phobos-fixed reference frame
- load(file_features);
- R_ini = [0, -1, 0; 1, 0, 0; 0, 0, 1];
- Point_Cloud = R_ini*Point_Cloud';
- points = zeros(4,size(Point_Cloud,2));
-
- for n = 1:size(points,2)
- points(:,n) = [Point_Cloud(:,n); n];
- end
-
-
-%% Identification of the features that are in light
+%% Geometry
% Rotation from J200 to Phobos radial reference frame
@@ -49,8 +38,39 @@ function [Y_LOS, Y_pix] = visible_features_model(MMX, Phobos, Sun, Phi, file_fea
% Light rays direction in the Phobos-fixed reference frame
r_PhSun = RLibration*R2rot*(Sun(1:3) - Phobos(1:3))/norm(Sun(1:3) - Phobos(1:3));
+
+% Position of MMX wrt Phobos-fixed frame
+
+ r_sb = MMX(1:3) - Phobos(1:3);
+ R_sb = norm(r_sb);
+ r_sb_Ph = RLibration*R2rot*r_sb;
+
+% MMX-Phobos direction in the Phobos-fixed reference frame
+ I = r_sb_Ph/R_sb;
+
+
+% Features defined in the Phobos-fixed reference frame
+ load(file_features);
+ R_ini = [0, -1, 0; 1, 0, 0; 0, 0, 1];
+ Point_Cloud = R_ini*Point_Cloud';
+ points = zeros(4,size(Point_Cloud,2));
+ Y_pix_tot = zeros(2,size(Point_Cloud,2));
+
+ for n = 1:size(points,2)
+ points(:,n) = [Point_Cloud(:,n); n];
+ [~, ~, PM] = ProjMatrix(-r_sb_Ph, pars, units);
+ pix_feat = PM*[-points(1:3,n); 1];
+ pix_feat = pix_feat./repmat(pix_feat(3),3,1);
+ Y_pix_tot = [round(pix_feat(1:2)), Y_pix_tot];
+ end
+
+
+%% Identification of the features that are in light
+
+
% Scan on the features to distinguish those in light
- point_in_light = [];
+ point_in_light = [];
+ Y_pix_light = [];
for n = 1:size(points,2)
% If the dot product of the incoming light ray and the feature
@@ -58,6 +78,10 @@ function [Y_LOS, Y_pix] = visible_features_model(MMX, Phobos, Sun, Phi, file_fea
% then it is in light
if dot(r_PhSun, points(1:3,n))<0
point_in_light = [points(:,n), point_in_light];
+ [~, ~, PM] = ProjMatrix(-r_sb_Ph, pars, units);
+ pix_feat = PM*[-points(1:3,n); 1];
+ pix_feat = pix_feat./repmat(pix_feat(3),3,1);
+ Y_pix_light = [round(pix_feat(1:2)), Y_pix_light];
end
end
@@ -66,15 +90,6 @@ function [Y_LOS, Y_pix] = visible_features_model(MMX, Phobos, Sun, Phi, file_fea
%% Between those features, which ones are inside the FOV?
-% Position of MMX wrt Phobos-fixed frame
-
- r_sb = MMX(1:3) - Phobos(1:3);
- R_sb = norm(r_sb);
- r_sb_Ph = RLibration*R2rot*r_sb;
-
-% MMX-Phobos direction in the Phobos-fixed reference frame
- I = r_sb_Ph/R_sb;
-
visible = [];
Y_LOS = [];
Y_pix = [];
@@ -90,9 +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 = acos(dot(I,-i_feat));
+ angle_feat = acos(dot(I,-i_feat));
+ angle_sun = acos(dot(I,r_PhSun));
+
- if (angle<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);...
@@ -111,6 +128,6 @@ function [Y_LOS, Y_pix] = visible_features_model(MMX, Phobos, Sun, Phi, file_fea
%% Plot of the Resulting sceene
% PlotGeometryAndLight(points, point_in_light, visible, r_sb_Ph, r_PhSun, pars, units);
-% Plotfeatures_Pic(Y_pix, pars);
+% Plotfeatures_Pic(Y_pix, Y_pix_light, Y_pix_tot, pars);
end
\ No newline at end of file