close all;
clear;

%% Load measurements

data = load('volleyball_cameras.mat');

Y = data.Y_measured;    % measurements, n_y x N              [1, ..., N]
X_true = data.X_true;   % true state trajectory, n_x x (N+1) [1, ..., N+1]

%% Parameter

e_l = [1 0 0]';          % line of sight
e_u = [0 0 1]';          % up vector
e_s = cross(e_u, e_l);   % right eye to left eye

d_eyes = 1;    % dist between eyes

P_R = [2  0.5 1.8]';        % position of right eye
P_L = P_R + d_eyes.*e_s;    % position of left eye

n_y = 4;                % two 2-dimensional cameras

n_p = 3;                % 3-dimensional position 
n_v = 3;                % 3-dimensional velocity

n_x = n_p + n_v;        % state dimension

%% Plot 3D trajectories

figure(1); hold on; grid on; axis equal;
plot3(X_true(1, :), X_true(2, :), X_true(3, :), 'kx-');

plot3(P_R(1), P_R(2), P_R(3), 'ro', 'MarkerSize', 5); 
plot3(P_L(1), P_L(2), P_L(3), 'bo', 'MarkerSize', 5);

quiver3(P_R(1), P_R(2), P_R(3), 5*e_l(1), 5*e_l(2), 5*e_l(3), 'r')
quiver3(P_L(1), P_L(2), P_L(3), 5*e_l(1), 5*e_l(2), 5*e_l(3), 'b')

legend('true trajectory', ...
       'right eye', 'left eye', ...
       'line of sight', 'line of sight');
view([-100, 7]);

%% Plot 2D measurements

figure(2); 
s1 = subplot(2,1,2); hold on; grid on; 
title('Right eye');
plot(Y(1, :), Y(2,:), 'rx');
legend('Y measured');

s2= subplot(2,1,1); hold on; grid on;
title('Left eye');
plot(Y(3, :), Y(4,:), 'bx');
legend('Y measured');

linkaxes([s1, s2], 'x')
linkaxes([s1, s2], 'y')



