%%   MASTER THESIS   %%
%==================== %
%        JOÃO PEIXOTO %
%               60140 %
%  joao.peixoto@ua.pt %
%                2016 %
%==================== %


%% CHAPTER 5 - KALMAN FILTER
%     This script will load the inertial and visual data and merge them
%  (separated data in the same variable), saving the result afterwards. It
%  will also create and add error to both experiments, saving a total of 4
%  merged data values, exp1 and exp2 with no error and the same with error.


%% 00 Clean Workspace and Global Variables
% First, we need to clear the workspace and then define some global
% variables, which will decide on the script behaviour.
for chapter = 0:0
    % Space Cleaning
    clc
    clear all
    close all
    % Configuration Loading
    config1 = CONFIG_DATA_1;
    config2 = CONFIG_DATA_2;
end
%% 01 Data loading, error creation and merging
% In this chapter we will load the ground truth and inertial data as well
% as the visual data calculated in "02 GetImageData" script. We will create
% an error to add to the inertial data and merge all possibilities mention
% above, saving them afterwards
for chapter = 1:1
    % Inertial Data loading
    [cart1, filt1]=function_inertialData(config1,1);
    [cart2, filt2]=function_inertialData(config2,2);
    
    
    % Error Creation
    for i = 1:size(filt1,1)
        filt1E(i,   1) = filt1(i,   1);
        filt1E(i,5: 7) = filt1(i,5: 7) + randn * 10 - 5;
        filt1E(i,8:10) = filt1(i,8:10) + randn * 3 - 1.5;
    end
    for i = 1:size(filt2,1)
        filt2E(i,   1) = filt2(i,   1);
        filt2E(i,5: 7) = filt2(i,5: 7) + randn * 10 - 5;
        filt2E(i,8:10) = filt2(i,8:10) + randn * 3 - 1.5;
    end
    
    
    % For all angle_tol values
    for i = 0:0.50:2.5
        % load data
        folder = '..\02 GetImageData\';
        img1 = load(sprintf('%sexp1_Tol%0.2f.mat', folder, i));
        img2 = load(sprintf('%sexp2_Tol%0.2f.mat', folder, i));
        % merge data
        both1  = function_assignData(img1.ImageSolution,filt1(:,[1 5:10]));
        both2  = function_assignData(img2.ImageSolution,filt2(:,[1 5:10]));
        both1E = function_assignData(img1.ImageSolution,filt1E(:,[1 5:10]));
        both2E = function_assignData(img2.ImageSolution,filt2E(:,[1 5:10]));
        % save result
        save(sprintf('AllData%0.2f.mat',i), 'both1', 'both2', 'both1E', 'both2E')
    end
end