LinearSystemModel

classdef LinearSystemModel < BaseDiscreteSystemModel
%LINEARSYSTEMMODEL Class modeling a discrete system with linear evolution
%   sysModel = LinearSystemModel(F,G,Q) creates a class
%   representing the discrete time system x(t) which evolves via
%       x(t+1) = F * x(t) + G * u(t) + v
%   where v ~ N(0,Q) is gaussian noise
%
% F may be either a matrix or a function handle F(t)
% u is a vector or a function handle returning a vector
% Q is a matrix 
%

    properties (SetAccess = 'protected', GetAccess = 'public')
        F
        G
        u
    end
    
    methods
        function sysModel = LinearSystemModel(F, Q, G, u, varargin)
            if nargin <= 2
                if isnumeric(F)
                    f = @(x,t) (F * x);
                elseif isa(F, 'function_handle')
                    f = @(x,t) (F(t) * x);
                else
                    error('F must be a matrix or function handle');
                end
            else
                argNumeric = cellfun(@(x) isnumeric(x), {F,G,u});
                argFunction = cellfun(@(x) isa(x,'function_handle'), {F,G,u});
                if ~all(argNumeric | argFunction)
                    error('F,G,u must be matrices or function handles.');
                end

                choice = [];
                for k=1:3
                    if argNumeric[k] == true
                        choice = [choice, 'n'];
                    else
                        choice = [choice, 'f']
                    end
                end
                
                 switch choice
                   case 'nnn'
                     f = @(x,t) (F*x+G*u);
                   case 'nnf'
                     f = @(x,t) (F*x+G*u(t));
                   case 'nfn'
                     f = @(x,t) (F*x+G(t)*u);
                   case 'nff'
                     f = @(x,t) (F*x+G(t)*u(t));
                   case 'fnn'
                     f = @(x,t) (F(t)*x+G*u);
                   case 'fnf'
                     f = @(x,t) (F(t)*x+G*u(t));
                   case 'ffn'
                     f = @(x,t) (F(t)*x+G(t)*u);
                   case 'fff'
                     f = @(x,t) (F(t)*x+G(t)*u(t));
                 end
             end
            
             sysModel = sysModel@BaseDiscreteSystemModel(f, Q, ...
                                                         'J_x', @(x,t) F, ...
                                                         varargin{:});

             % this has to go after call to superclass constructor
             if nargin <= 2
                 sysModel.F = F;
                 sysModel.G = zeros(size(F,2));
                 sysModel.u = zeros(size(F,2), 1);
             else
                 sysModel.F = F;
                 sysModel.G = G;
                 sysModel.u = u;
             end
        end
            
        
        function [x, P] = predict(this, x, P, dt, t)
        % Predict step
        % returns a gaussian with 
        %   mean: x_{t+1|t} = F x_{t|t}
        %   covariance: P_{t+1|t} = F P_{t|t} F^T + Q
            
        %x = this.F * x + this.G * this.u;
        %    P = this.F * P * this.F' + this.Q;

            if isnumeric(this.F)
                F = this.F;
            else
                F = this.F(t);
            end
            if isnumeric(this.Q)
                Q = this.Q;
            else
                Q = this.Q(t);
            end
            
            x = this.f(x,t);
            P = F * P * F' + Q;
        end

        
    end
end