Skip to content
Snippets Groups Projects
Commit b66673d6 authored by Ciccarelli, Edoardo (PG/R - Comp Sci & Elec Eng)'s avatar Ciccarelli, Edoardo (PG/R - Comp Sci & Elec Eng)
Browse files

Creata function per lista osservabili

parent 94a49527
No related branches found
No related tags found
No related merge requests found
function [pars, units] = CovarianceAnalysisParameters(pars, units)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% [pars, units] = New_MMX_CovarianceAnalysisParameters(pars, units)
%
% This function adds some units and parameters useful to the covariance
% analysis
%
% Inputs:
% pars Structure which contains parameters
% units Structure with units
%
% Outputs:
% pars.ObsNoise Observations noises standard deviation
% pars.ObsNoise.range Standard deviation of the relative distance measure
% pars.ObsNoise.range_rate Standard deviation of the relative velocity measure
% pars.ObsNoise.lidar Standard deviation of the lidar measure
% pars.ObsNoise.camera Standard deviation of the camera measure
% pars.interval Seconds between observations
% pars.Clm Vector with all the Clm coefficients
% pars.Slm Vector with all the Slm coefficients
% pars.nCoeff Number of the harmonics coefficients
% pars.P0 A-priori covariance
% pars.sigma PN diffusion coefficient
%
% units.Dim Vector of normalizing units [sfVec, sfVec, ones(pars.nCoeff,1)]
% units.CovDim Matrix of normalizing units
%
% Author: STAG Team
% Date: Jun 2021
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Parameters
% Observation noise
pars.ObsNoise.range = 5e-3/units.lsf; % [km]
pars.ObsNoise.range_rate= 5e-7/units.vsf; % [km/s]
pars.ObsNoise.lidar = 1e-2/units.lsf; % [km]
cam.f = 15; %[mm]
cam.nPixX = 3296; %[n. pixel]
cam.nPixY = 2472; %[n. pixel]
cam.Res = 100;
cam.SixeX = 36; %[mm]
cam.SixeY = 24; %[mm]
cam.pixARx = 1;
cam.pixARy = 1;
pars.camera_intrinsic = IntrinsicParameters(cam);
pars.FOV = deg2rad(45);
pixels = 2048;
pars.ObsNoise.camera = pars.FOV/pixels; % [rad/pixel]
pars.ObsNoise.pixel = 1; % [n. pixel]
% Useful to adimensionalize the observables
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(1,1) = pars.ObsNoise.range^2;
pars.R(2,2) = pars.ObsNoise.range_rate^2;
pars.R(3,3) = pars.ObsNoise.range^2;
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;
% Seconds between observation
pars.interval_Range = 3600; % s
pars.interval_Range_rate = 600; % s
pars.interval_lidar = 100; % s
pars.interval_camera = 3600; % s
pars.cutoffLIDAR = 200; % km
pars.elevation = 80; % deg
% Gravitational harmonics' parameters
pars.I2 = [pars.IPhx_bar; pars.IPhy_bar; pars.IPhz_bar];
% Measurements biases
bias = [0; 0; 0]...
./[units.lsf; units.vsf; units.lsf];
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.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; 3e-1; 1e-3; 3e-1; 1e-3; 1e-2*ones(pars.nCoeff,1);...
1; 1e-3; 1]./units.Dim).^2);
end
\ No newline at end of file
......@@ -25,32 +25,30 @@ clc
% Time of the analysis
data = '2026-03-16 13:00:00 (UTC)';
data = '2026-03-21 00:00:00 (UTC)';
data = cspice_str2et(data);
Ndays = 1;
t = Ndays*86400;
date_end = data+t;
% Model parameters
[par, units] = MMX_DefineNewModelParametersAndUnits;
[pars, units] = MMX_DefineNewModelParametersAndUnits;
% Covariance analysis parameters
par.et0 = data;
[par, units] = New_MMX_CovarianceAnalysisParameters(par, units);
pars.et0 = data;
[pars, units] = CovarianceAnalysisParameters(pars, units);
cam.f = 15; %[mm]
cam.nPixX = 3296; %[n. pixel]
cam.nPixY = 2472; %[n. pixel]
cam.Res = 100;
cam.SixeX = 36; %[mm]
cam.SixeY = 24; %[mm]
cam.pixARx = 1;
cam.pixARy = 1;
par.camera_intrinsic = IntrinsicParameters(cam);
par.FOV = deg2rad(30);
% Positin of my target bodies at that date
% Positin of my target bodies at that date
Sun = cspice_spkezr('10', data, 'J2000', 'none', 'MARS');
MMX = cspice_spkezr('-34', data, 'J2000', 'none', 'MARS');
Phobos = cspice_spkezr('-401', data, 'J2000', 'none', 'MARS');
Phi = deg2rad(0);
[Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi,...
'Principal_Phobos_Ogni5.mat', par, units);
% [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi,...
% 'Principal_Phobos_Ogni10.mat', par, units);
file_features = 'Principal_Phobos_Ogni2.mat';
YObs = Observations_with_features(data, date_end, file_features, pars, units);
% Plotfeatures_Pic(YObs(685).Features, pars)
function YObs_Full = Observations_with_features(date0, date_end, pars, units)
function YObs = Observations_with_features(date0, date_end, file_features, pars, units)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% dx = Mars_Phobos_Circular_FullState(t,x,pars)
......@@ -35,7 +35,6 @@ function YObs_Full = Observations_with_features(date0, date_end, pars, units)
sigma_range = pars.ObsNoise.range*units.lsf;
sigma_range_rate = pars.ObsNoise.range_rate*units.vsf;
sigma_lidar = pars.ObsNoise.lidar*units.lsf;
sigma_cam = pars.ObsNoise.camera;
%% Observations
......@@ -43,7 +42,7 @@ function YObs_Full = Observations_with_features(date0, date_end, pars, units)
et0 = date0;
et_end = date_end;
YObs_Full = [];
YObs = [];
min_interval = min([interval_Range; interval_Range_rate; interval_lidar; interval_camera]);
......@@ -53,33 +52,6 @@ function YObs_Full = Observations_with_features(date0, date_end, pars, units)
got_it = 0;
%--------------------------------------------------------------------------
% CAMERA OBSERVABLE
%--------------------------------------------------------------------------
% Check if Phobos is in light or not
Phobos = cspice_spkezr('-401', i, 'J2000', 'none', 'SUN');
MMX = cspice_spkezr('-34', i, 'J2000', 'none', 'SUN');
R_PS = -Phobos(1:3)/norm(Phobos(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)
Y = visible_features(i, pars.file_features, pars, units);
got_it = 1;
else
Y = [NaN; NaN];
end
%--------------------------------------------------------------------------
% LIDAR OBSERVABLES WRT PHOBOS SURFACE
%--------------------------------------------------------------------------
......@@ -197,6 +169,37 @@ function YObs_Full = Observations_with_features(date0, date_end, pars, units)
end
%--------------------------------------------------------------------------
% CAMERA OBSERVABLE
%--------------------------------------------------------------------------
% Check if Phobos is in light or not
Phobos = cspice_spkezr('-401', i, 'J2000', 'none', 'SUN');
MMX = cspice_spkezr('-34', i, 'J2000', 'none', 'SUN');
R_PS = -Phobos(1:3)/norm(Phobos(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, 'J2000', 'none', 'MARS');
MMX = cspice_spkezr('-34', i, 'J2000', 'none', 'MARS');
Phobos = cspice_spkezr('-401', i, 'J2000', 'none', 'MARS');
Phi = deg2rad(0);
[~, Y_pix] = visible_features(MMX, Phobos, Sun, Phi,...
file_features, pars, units);
got_it = 1;
else
Y_LOS = [];
Y_pix = [];
end
%--------------------------------------------------------------------------
% NEW LINE SETUP
......@@ -204,70 +207,17 @@ function YObs_Full = Observations_with_features(date0, date_end, pars, units)
if (got_it == 1)
YObs_Full = [YObs_Full, [i-et0; R_Cam; R_dot_Cam; R_Gold; R_dot_Gold; R_Mad; R_dot_Mad; Lidar; Y]];
Obs.t = i-et0;
DSN = [R_Cam; R_dot_Cam; R_Gold; R_dot_Gold; R_Mad; R_dot_Mad];
if sum(~isnan(DSN))>0
Obs.DSN = [R_Cam; R_dot_Cam; R_Gold; R_dot_Gold; R_Mad; R_dot_Mad];
else
Obs.DSN = [];
end
Obs.Lidar = Lidar;
Obs.Features= Y_pix;
YObs = [YObs; Obs];
end
end
% fclose(fileID);
%% Plot of the data
set(0,'DefaultTextInterpreter','latex');
set(0,'DefaultAxesFontSize', 16);
% Observables
DSN_Cam = [];
DSN_GS = [];
DSN_Mad = [];
for i = 1:size(YObs_Full,2)
DSN_Cam = [DSN_Cam, [YObs_Full(1,i); YObs_Full(2:3,i)]];
DSN_GS = [DSN_GS, [YObs_Full(1,i); YObs_Full(4:5,i)]];
DSN_Mad = [DSN_Mad, [YObs_Full(1,i); YObs_Full(6:7,i)]];
end
figure()
subplot(2,1,1)
plot(DSN_Cam(1,:)/3600,DSN_Cam(2,:),'*','Color','b')
grid on; hold on;
plot(DSN_GS(1,:)/3600,DSN_GS(2,:),'*','Color','r')
plot(DSN_Mad(1,:)/3600,DSN_Mad(2,:),'*','Color','g')
ylabel('Range [km]');
xlabel('Time [hour]')
legend('Camberra','GoldStone','Madrid')
subplot(2,1,2)
plot(DSN_Cam(1,:)/3600,DSN_Cam(3,:),'*','Color','b')
grid on; hold on;
plot(DSN_GS(1,:)/3600,DSN_GS(3,:),'*','Color','r')
plot(DSN_Mad(1,:)/3600,DSN_Mad(3,:),'*','Color','g')
ylabel('Range Rate [km/s]');
xlabel('Time [hour]')
legend('Camberra','GoldStone','Madrid')
figure()
plot(YObs_Full(1,:)/3600,YObs_Full(8,:),'*')
grid on; hold on;
ylabel('Lidar [km]');
xlabel('Time [hour]')
figure()
subplot(2,1,1)
plot(YObs_Full(1,:)/3600,YObs_Full(9,:),'*')
grid on; hold on;
ylabel('Cam1 [rad]');
xlabel('Time [hour]')
subplot(2,1,2)
plot(YObs_Full(1,:)/3600,YObs_Full(10,:),'*')
grid on; hold on;
ylabel('Cam2 [rad]');
xlabel('Time [hour]')
end
\ No newline at end of file
function PlotGeometryAndLight(points, point_in_light, visible, r_sb_Ph, r_PhSun, pars, units)
% Rotation matrix from Phobos to the camera lens
i_feat = r_sb_Ph/norm(r_sb_Ph);
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;
figure()
grid on
axis equal
hold on
plot3(points(1,:), points(2,:), points(3,:),'.','Color','b')
plot3(point_in_light(1,:), point_in_light(2,:), point_in_light(3,:), 'x','Color','g')
if ~isempty(visible)
plot3(visible(1,:), visible(2,:), visible(3,:), 'o','Color','r')
for n = 1:size(visible,2)
r_sf = r_sb_Ph - visible(:,n);
LOS = -r_sf/norm(r_sf);
quiver3(r_sb_Ph(1),r_sb_Ph(2),r_sb_Ph(3),LOS(1)*units.lsf,LOS(2)*units.lsf,LOS(3)*units.lsf);
end
end
xlabel('X','FontSize',16,'Interpreter','latex')
ylabel('Y','FontSize',16,'Interpreter','latex')
zlabel('Z','FontSize',16,'Interpreter','latex')
quiver3(-2*r_PhSun(1)*units.lsf,-2*r_PhSun(2)*units.lsf,-2*r_PhSun(3)*units.lsf,...
r_PhSun(1)*units.lsf,r_PhSun(2)*units.lsf,r_PhSun(3)*units.lsf,'LineWidth',1.5,'Color','g')
plot3(-2*r_PhSun(1)*units.lsf,-2*r_PhSun(2)*units.lsf,-2*r_PhSun(3)*units.lsf, '*','Color','g','MarkerSize',20)
quiver3(r_sb_Ph(1),r_sb_Ph(2),r_sb_Ph(3),...
-r_sb_Ph(1)/units.tsf,-r_sb_Ph(2)/units.tsf,-r_sb_Ph(3)/units.tsf,'LineWidth',1.5,'Color','r')
plotCamera('location',r_sb_Ph,'orientation',R_CV,'Color','r')
end
\ No newline at end of file
function Plotfeatures_Pic(Y_pix, pars)
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)])
end
\ No newline at end of file
File added
File added
File added
File added
......@@ -22,7 +22,12 @@ 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];
points = R_ini*Point_Cloud';
Point_Clooud = R_ini*Point_Cloud';
points = zeros(4,size(Point_Clooud,2));
for n = 1:size(points,2)
points(:,n) = [Point_Clooud(:,n); n];
end
%% Identification of the features that are in light
......@@ -50,14 +55,15 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
% 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(:,n))<0
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?
%% Between those features, which ones are inside the FOV?
% Position of MMX wrt Phobos-fixed frame
......@@ -65,8 +71,8 @@ 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 les
i_feat = r_sb_Ph/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));
......@@ -78,10 +84,6 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
R_bCam2CV = [0, 0, 1; 0, 1, 0; -1, 0, 0];
R_CV = R_bCam2CV*T;
% [M_int, M_ext, PM] = ProjMatrix(r_sb_Ph, pars, units);
visible = [];
Y_LOS = [];
Y_pix = [];
......@@ -90,7 +92,7 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
I = i_feat;
for n = 1:size(point_in_light,2)
candidate = point_in_light(:,n);
candidate = point_in_light(1:3,n);
r_sf = r_sb_Ph - candidate;
......@@ -101,52 +103,25 @@ function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features,
angle = acos(dot(I,-i_feat));
if (angle<pars.FOV)&&(dot(r_sb_Ph, point_in_light(:,n))>0)
if (angle<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)];
Y_LOS = [LOS_feat, Y_LOS];
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 = [[ceil(pix_feat(1:2)+ [random('Normal',0, 1); random('Normal',0, 1)]); candidate(end)], Y_pix];
Y_pix = [[ceil(pix_feat(1:2)+ [random('Normal',0, pars.ObsNoise.pixel);...
random('Normal',0, pars.ObsNoise.pixel)]); point_in_light(end,n)], Y_pix];
end
end
figure()
grid on
axis equal
hold on
plot3(points(1,:), points(2,:), points(3,:),'.','Color','b')
plot3(point_in_light(1,:), point_in_light(2,:), point_in_light(3,:), 'x','Color','g')
if ~isempty(visible)
plot3(visible(1,:), visible(2,:), visible(3,:), 'o','Color','r')
for n = 1:size(visible,2)
r_sf = r_sb_Ph - visible(:,n);
LOS = -r_sf/norm(r_sf);
quiver3(r_sb_Ph(1),r_sb_Ph(2),r_sb_Ph(3),LOS(1)*units.lsf,LOS(2)*units.lsf,LOS(3)*units.lsf);
end
end
xlabel('X','FontSize',16,'Interpreter','latex')
ylabel('Y','FontSize',16,'Interpreter','latex')
zlabel('Z','FontSize',16,'Interpreter','latex')
quiver3(-2*r_PhSun(1)*units.lsf,-2*r_PhSun(2)*units.lsf,-2*r_PhSun(3)*units.lsf,...
r_PhSun(1)*units.lsf,r_PhSun(2)*units.lsf,r_PhSun(3)*units.lsf,'LineWidth',1.5,'Color','g')
plot3(-2*r_PhSun(1)*units.lsf,-2*r_PhSun(2)*units.lsf,-2*r_PhSun(3)*units.lsf, '*','Color','g','MarkerSize',20)
quiver3(r_sb_Ph(1),r_sb_Ph(2),r_sb_Ph(3),...
-r_sb_Ph(1)/units.tsf,-r_sb_Ph(2)/units.tsf,-r_sb_Ph(3)/units.tsf,'LineWidth',1.5,'Color','r')
plotCamera('location',r_sb_Ph,'orientation',R_CV,'Color','r')
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)])
%% 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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment