BLUEFilter

classdef BLUEFilter < handle
%BLUEFILTER  Filter based on Best Linear Unbiased Estimator (BLUE)
%   Performs the filtering step which takes in an observation, and
%   predicts the actual state.
%
%   filt = BLUEFilter(sysModel, obsModel) returns a filter
%
    
    properties (GetAccess = 'public', SetAccess = 'protected')
        sysModel;
        obsModel;
    end
    
    methods
        function filt = BLUEFilter(sysModel, obsModel)
        % sysModel = system model of type BaseSystemModel
        % obsModel = observation model of type
        %             BaseObservationModel
        % dist = gaussian initial distribution
            if nargin > 0
                assert(isa(sysModel, 'BaseSystemModel'));
                assert(isa(obsModel, 'BaseObserverModel'));
                filt.sysModel = sysModel;
                filt.obsModel = obsModel;
            end
        end
        
        function [updatedState, updatedCovariance] = filter(this, mean, cov, newObs, dt, curTime)
        % uses update model:
        % x_{k|k} = x_{k|k-1} + C_{xz} * C_{zz}^{-1} * innovation
        % P_{k|k} = P_{k|k-1} + C_{xz} * C_{zz}^{-1} * C_{zx}
        % 
        % innovation = predicted observation - measured observation
        %   (but possibly in a transformed coordinate system)
            
        % system update
            if nargin < 6
                curTime = 0;  % if curTime not passed, assume it's irrevent
            end
            if nargin < 5
                dt = 1;
            end

            [predMean, predCov] = this.sysModel.predict(mean, cov, dt, curTime);
            
            curTime = curTime + dt;
            [C_zz C_xz innovation] = this.obsModel.observerData(predMean, predCov, ...
                                        newObs, curTime);
            
            K = C_xz / C_zz;  % Kalman gain for BLUE
            
            updatedState = predMean + K * innovation;
            
            % Warning: the minus sign below means numerical error
            % may make covariance not positive definite.
            updatedCovariance = predCov - K * C_xz';
            updatedCovariance = (updatedCovariance + updatedCovariance')/2;
            
        end
    
    end
end