output = load('test.txt');

% 01 myfile << "," << time_elapsed;		// time elapsed 
% 02 myfile << "\n"<< x_measured; 		// measured by artoolkit
% 03 myfile << "," << y_measured; 		// measured by artoolkit
% 04 myfile << "," << x_pos;		 		// posteriori estimation by kalman filter
% 05 myfile << "," << y_pos;				// posteriori estimation by kalman filter
% 06 myfile << "," << x_prediction;		// priori estimation by kalman filter
% 07 myfile << "," << y_prediction;		// priori estimation by kalman filter
% 08 myfile << "," << x_velocity;		// posteriori estimation by kalman filter
% 09 myfile << "," << y_velocity;		// posteriori estimation by kalman filter
% 10 myfile << "," << heading_measured;	// measured by artoolkit
% 11 myfile << "," << heading_state;		// posteriori estimation by kalman filter
% 12 myfile << "," << heading_prediction;// priori estimation by kalman filter
% 13 myfile << "," << xbee_message.speed;// speed sent (by the agent module) to the robot
% 14 myfile << "," << xbee_message.direction; //dir sent (by the agent module) to the robot
% 15 myfile << "," << velocity; 			//velocity module ( sqrt(x²+y²))

% 960,100
% 1060,140
% 1110,240
% 1060,340
% 960,380
% 260,380
% 160,340
% 110,240
% 160,140
% 260,100

%y correction (opencv grows from top to bottom)
image_heigth = 480;
% y = 480 - y;

%x_goto_points = [960 1060 1110 1060 960 260 160 110 160 260 960];
%y_goto_points = [100 140 240 340 380 380 340 240 140 100 100];

time = output(:,1);

x_measured = output(:,2);
y_measured = image_heigth - output(:,3);
x_state = output(:,4);
y_state = image_heigth - output(:,5);
x_prediction = output(:,6);
y_prediction = image_heigth - output(:,7);
x_speed = output(:,8);
y_speed = output(:,9);

%heading_measured = output(:,10);

speed_sent = output(:,13);

velocity = output(:,15);
%time = (1:length(output))*1/30;

H = figure('name','position');
hold on, grid on, axis equal;
%set(H,'pos',[800   200   500   400]);

plot(x_measured,y_measured,'bo-');
plot(x_state,y_state,'ro-');
plot(x_prediction,y_prediction,'y*');
%plot(x_goto_points,y_goto_points,'kp-');
legend('measured','corrected state','prediction')

I = figure('name','independent position');
hold on, grid on;
%set(I,'pos',[800   200   500   400]);
plot(time,x_state,'mo-')
plot(time,y_state,'ko-')
plot(time,x_measured)
plot(time,y_measured,'r')
%plot(x_prediction,'*')
%plot(y_prediction,'r*')

J = figure('name','speed');
hold on, grid on;
%set(J,'pos',[800   200   500   400]);
speed = sqrt(x_speed.^2+y_speed.^2);
plot(time,x_speed,'ro-');
plot(time,y_speed,'ko-');
% plot(time,speed,'bd');
plot(time,velocity,'bd-');
set(gca,'YLim',[-500 500]);
plot(time,speed_sent,'g*');

%%manual calculation of speed, for comparison purposes
x_measured_speed = diff(x_measured).*30;
y_measured_speed = diff(y_measured).*30;
K = figure('name','measured speed');
hold on, grid on;
%set(K,'pos',[800   200   500   400]);
measured_speed = sqrt(x_measured_speed.^2+y_measured_speed.^2);
plot(time(1:end-1),x_measured_speed,'ro');
plot(time(1:end-1),y_measured_speed,'ko');
plot(time(1:end-1),measured_speed,'bd');
set(gca,'YLim',[-500 500]);
