diff --git a/Analisys_With_Features.m b/Analisys_With_Features.m
index 88570fdc3be010d98c018eaf881f8434d1333df3..fc3c609220c4747b543150cb8e12489f25d61f2c 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_094_2x2_826891269_828619269.bsp'));
+ cspice_furnsh(which('MMX_QSO_031_2x2_826891269_828619269.bsp'));
cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
@@ -44,7 +44,7 @@ clc
% Covariance analysis parameters
[par, units] = CovarianceAnalysisParameters(par, units);
- par.sigma = 0/(units.vsf*units.tsf);
+ par.sigma = 1e-10/(units.vsf*units.tsf);
par.sigmaPh = 0/(units.vsf*units.tsf);
% Initial Phobos's state vector
@@ -68,14 +68,20 @@ clc
par.beta = 2;
file_features = 'Principal_Phobos_Ogni10.mat';
-% YObs(1).Range = [];
-% YObs(1).Rate = [];
-% YObs(1).Lidar = [];
+% 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
[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);
diff --git a/CovarianceAnalysisParameters.m b/CovarianceAnalysisParameters.m
index a5819852d43ada0e1bb3a5d9612bd6d9d0a4cd78..1e2a167f88dc77b095e674728a07c98a24c9042a 100644
--- a/CovarianceAnalysisParameters.m
+++ b/CovarianceAnalysisParameters.m
@@ -50,11 +50,11 @@ cam.SixeY = 24; %[mm]
cam.pixARx = 1;
cam.pixARy = 1;
pars.camera_intrinsic = IntrinsicParameters(cam);
-pars.FOV = deg2rad(45);
-pixels = 2048;
+pars.FOV = deg2rad(45);
+pixels = 2472;
pars.ObsNoise.camera = pars.FOV/pixels; % [rad/pixel]
-pars.ObsNoise.pixel = 1; % [n. pixel]
+pars.ObsNoise.pixel = .25; % [n. pixel]
% Useful to adimensionalize the observables
diff --git a/Observables_model_with_Features.m b/Observables_model_with_Features.m
index 723c8b882dc849628873ec24f860bf13b0262da7..8e2f43ea972199fb892cbb6af7c10b15d2e65159 100644
--- a/Observables_model_with_Features.m
+++ b/Observables_model_with_Features.m
@@ -174,7 +174,7 @@ function [G, Rm] = Observables_model_with_Features(et, X, St_ID_Range, St_ID_Rat
Phobos = [r_Phobos.*units.lsf; v_Phobos.*units.vsf];
Xsi = X(13);
- [~, Y_pix] = visible_features(X_MMX.*units.sfVec, Phobos, Sun, Xsi, file_features, pars, units);
+ [~, Y_pix] = visible_features_model(X_MMX.*units.sfVec, Phobos, Sun, Xsi, file_features, pars, units);
else
Y_pix = [];
end
diff --git a/Observations_Generation.m b/Observations_Generation.m
index 51daed2f99146dab2f2d3b451b41a8a4b5849264..c8ed5e66f3df172842fb86525f5bf683df6cf8d6 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_094_2x2_826891269_828619269.bsp'));
+ cspice_furnsh(which('MMX_QSO_031_2x2_826891269_828619269.bsp'));
cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
%% Initial conditions per la analisi
@@ -49,4 +49,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 = [];
+
+% save('~/tapyr/Edo/YObs');
save('YObs');
\ No newline at end of file
diff --git a/Observations_with_features.m b/Observations_with_features.m
index 9207438495f71310b915695b5bbe8b837a06ef50..ec197559fcea194a8dd6563658ba25199698470e 100644
--- a/Observations_with_features.m
+++ b/Observations_with_features.m
@@ -111,7 +111,7 @@ function YObs = Observations_with_features(date_0, date_end, file_features, pars
got_it = 1;
else
- Lidar = NaN;
+ Lidar = [];
end
diff --git a/UKF_Tool.m b/UKF_Tool.m
index 251a394f128c724ae46b87d56598130be2024908..5649d443f62aaed90610ec47f38744fbe5a85e5f 100644
--- a/UKF_Tool.m
+++ b/UKF_Tool.m
@@ -209,6 +209,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
Y_feat = cell(2*n);
for j = 1:2*n
+
% 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);
@@ -273,7 +274,7 @@ function [Est] = UKF_Tool(Est0, f, fsigma, G, O, Measures, pars, units, file_fea
% Storing the results
- P_t(:,:,i) = abs(real(P)).*units.CovDim;
+ P_t(:,:,i) = abs(P).*units.CovDim;
Pbar_t(:,:,i) = P_bar.*units.CovDim;
X_t(:,i) = Xold.*units.Dim;
x_t(:,i) = K*y.*units.Dim;
diff --git a/UKF_features.m b/UKF_features.m
index beb8c3546b84357d8e4bb00769cce4675d94d88e..c3f21ac498b8d12283cfbc7862ad0b8bc533b800 100644
--- a/UKF_features.m
+++ b/UKF_features.m
@@ -139,7 +139,7 @@ function [Est] = UKF_features(Est0, f, G, O, Measures, pars, units, file_feature
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;
diff --git a/YObs.mat b/YObs.mat
index d47550885dbe87352704d6294e25591d06d0613a..d1e9da904da9dbdf17fdcb11b028565ac9cdf338 100644
Binary files a/YObs.mat and b/YObs.mat differ
diff --git a/visible_features.m b/visible_features.m
index a63cd7ed576002e5a7e08156c272ce543f86b845..2114a2516daf1eb0c4d37eaf5d1a63bafb1c67d4 100644
--- a/visible_features.m
+++ b/visible_features.m
@@ -23,11 +23,11 @@ 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_Clooud = R_ini*Point_Cloud';
- points = zeros(4,size(Point_Clooud,2));
+ Point_Cloud = R_ini*Point_Cloud';
+ points = zeros(4,size(Point_Cloud,2));
for n = 1:size(points,2)
- points(:,n) = [Point_Clooud(:,n); n];
+ points(:,n) = [Point_Cloud(:,n); n];
end
@@ -72,25 +72,12 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
R_sb = norm(r_sb);
r_sb_Ph = RLibration*R2rot*r_sb;
-% Rotation matrix from Phobos to the camera lens
- i_feat = r_sb_Ph/R_sb;
- v = [0; 0; 1];
- j = cross(v, i_feat)/norm(cross(v, i_feat));
- k = cross(i_feat, j)/norm(cross(i_feat, j));
-
- T = [i_feat, j, k]';
-
-% In the CV world si usa questa convenzione, l'asse Z della camera è
-% quello che esce dal piatto
- R_bCam2CV = [0, 0, 1; 0, 1, 0; -1, 0, 0];
- R_CV = R_bCam2CV*T;
+% MMX-Phobos direction in the Phobos-fixed reference frame
+ I = r_sb_Ph/R_sb;
visible = [];
Y_LOS = [];
Y_pix = [];
-
-% MMX-Phobos direction in the Phobos-fixed reference frame
- I = i_feat;
for n = 1:size(point_in_light,2)
candidate = point_in_light(1:3,n);
diff --git a/visible_features_model.m b/visible_features_model.m
new file mode 100644
index 0000000000000000000000000000000000000000..3c744a9e4581874cbaba398642d4aee006650fc8
--- /dev/null
+++ b/visible_features_model.m
@@ -0,0 +1,116 @@
+function [Y_LOS, Y_pix] = visible_features_model(MMX, Phobos, Sun, Phi, file_features, pars, units)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Y = visible_features_model(data, file_features)
+%
+% Identify the visible features and build the observable for analysis
+%
+% Input:
+%
+% data Date of the observation
+% file_features File including the position of the features in the body
+% fixed reference frame
+%
+% Output:
+%
+% Y_LOS Vector of features' Line Of Sight and features' ID
+% Y_pix Vector of features' position in the Pic and features' ID
+%
+% Author: E.Ciccarelli
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% 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
+
+% Rotation from J200 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]';
+
+% Rotation from Phobos radial reference frame to Phobos-fixed reference frame
+
+ RLibration = [cos(Phi), sin(Phi), 0; -sin(Phi), cos(Phi), 0; 0, 0, 1];
+
+% 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));
+
+% Scan on the features to distinguish those in light
+ point_in_light = [];
+
+ for n = 1:size(points,2)
+% If the dot product of the incoming light ray and the feature
+% position vector in the Phobos-fixed ref frame is less than zero,
+% then it is in light
+ if dot(r_PhSun, points(1:3,n))<0
+ point_in_light = [points(:,n), point_in_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 = [];
+
+
+ for n = 1:size(point_in_light,2)
+ candidate = point_in_light(1:3,n);
+
+ r_sf = r_sb_Ph - candidate;
+
+ i_feat = -r_sf/norm(r_sf);
+ v = [0; 0; 1];
+ z_cam = cross(v, I)/norm(cross(v, I));
+ y_cam = cross(z_cam, I)/norm(cross(z_cam, I));
+
+ angle = acos(dot(I,-i_feat));
+
+ if (angle<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);...
+ random('Normal',0, pars.ObsNoise.camera)]*k;
+ Y_LOS = [[LOS_feat; point_in_light(end,n)], Y_LOS];
+
+ [~, ~, PM] = ProjMatrix(-r_sb_Ph, pars, units);
+ pix_feat = PM*[-candidate; 1];
+ 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
+
+
+%% Plot of the Resulting sceene
+
+% PlotGeometryAndLight(points, point_in_light, visible, r_sb_Ph, r_PhSun, pars, units);
+% Plotfeatures_Pic(Y_pix, pars);
+
+end
\ No newline at end of file