classdef TestKalmanFilter < matlab.unittest.TestCase
    % Copyright 2014 - 2016 The MathWorks, Inc.
    
    methods ( Test )
        
        function SSE_LessThanTolerance( testCase )
            load position.mat;
            [z,y] = callKalmanFilter( position );
            
            tolerance = 0.001; % tolerance of 0.0001 will break
            A = z-1000*y;
            error = sum(sum(A.^2));
            
            testCase.verifyLessThanOrEqual( error, tolerance);
            
            % For debugging
            plot_kalman_filter_trajectory(z,1000*y);
        end
        
        function SampleErrorLessThanTolerance( testCase )
            load position.mat;
            [z,y] = callKalmanFilter( position );
            
            tolerance = 0.01;   % tolerance of 0.001 will break
            A = z-1000*y;

            testCase.verifyEqual(1000*y, z, 'AbsTol', tolerance);
            % For debugging
            plot_kalman_filter_trajectory(z,1000*y);
            
            [value, location] = max(A(:));
            [R,C] = ind2sub(size(A),location);
            disp(['Max value ' num2str(value) ' is located at [' num2str(R) ',' num2str(C) ']']);
        end
    end
end