%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Main script for PAN_ powe aware navigation with sensor fusion 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% change input data load and script to be run to switch from car to bike

%% Load data
disp('Loads data')
%load('GNSSaidedINS_data.mat');
load('cycling_input_data');

%% Load filter settings
disp('Loads settings')
%settings=get_settings();
settings=get_settings_cycling();
settings.P_treshold=2.5;

%% Run the GNSS-aided INS
disp('Runs the GNSS-aided INS')
%out_data=GPSaidedINS(in_data,settings);
out_data=GPSaidedINS_cycling(in_data,settings);

%% Plot the data 
disp('Plot data')
%overall error
out_data.error
%plot_data(in_data,out_data,'True');drawnow
h=zeros(1,2);
figure(2)
plot(in_data.GNSS.pos_ned(2,:),in_data.GNSS.pos_ned(1,:),'b-');
%plot(out_data.x_h(2,:),out_data.x_h(1,:),'r-');
hold on
h(1)=plot(in_data.GNSS.pos_ned(2,:),in_data.GNSS.pos_ned(1,:),'b.');
h(2)=plot(out_data.x_h(2,:),out_data.x_h(1,:),'-');
%h(3)=plot(in_data.GNSS.pos_ned(2,1),in_data.GNSS.pos_ned(1,1),'ks');
title('Trajectory')
ylabel('North [m]')
xlabel('East [m]')
legend(h,'GNSS position estimate','GNSS aided INS trajectory')%,'Start point')
axis equal
grid on

%plot control signal P and control action turn
h=zeros(1,2);
figure(3)
hold on
h(1)=plot(out_data.P_store);
h(2)=plot(out_data.turn);
legend(h,'sum of the trace of P','antenna turned ON')
grid on