% Demo script for Kalman Filters and Robot Localization
% October 23, 2004 Rudy Negenborn

disp(' ');
disp('This script shows some of the functionality of the ');
disp('Kalman Filters and Robot Localization simulator.');
disp('Feel free to contact me for further info at ');
disp('rudy@negenborn.net.');
disp(' ');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Make sure that the directory of the simulator including all');
disp('subdirectories are added to the path before running this script');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp(' ');
disp('Press any key to start the demo');
pause
disp(' ');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% clear the current workspace
clear all

% load some parameters
startup

% initialize a robot with full state sensor
disp('Parameters for this setting are found in');
disp('diff_drive\diff_drive_problem_fs.m');
diff_drive_init_fs


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate 'real' data for a run with this robot
generate_data


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% feed the actions/measurements of the real data to 
% an extended kalman filter
generate_ekf


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot the real trajectory and measurements
disp('Plotting real trajectory and measurements..')
charts_true_multi(data,0,1,'+')
disp('Press any key...')
pause
disp(' ');

% plot the trajectory as estimated by the EKF
disp('Plotting the trajectory as estimated by the EKF')
charts_estimate_multi(ekf,0)
disp('Press any key...')
pause
disp(' ');

% plot a chart with combined real/estimated info
disp('Plotting combined real/estimated info')
figs = charts_combi_multi(data,ekf,0,200,0)
disp('Press any key...')
pause
disp(' ');

% create an animation of the evolution of the state estimates
disp('Press any key to start an animation of the state estimates evolution')
pause
disp(' ');
charts_combi_animate(data,ekf,0,1,10,1,figs)