LinearContinuousSystemModel

classdef LinearContinuousSystemModel < BaseContinuousSystemModel
%LINEARCONTINUOUSSYSTEMMODEL  Class modeling a linear continuous time system
%   sysModel = LinearContinuousSystemModel(F,A) creates a class
%   representing the stochastic differential equation
%     dX = F X dt + A dW
%   with F and A matrices.  This equation is often written as
%     dx/dt = F*x + w    with w = white noise with covariance A' * A
%
% This process has an explicit solution.  It is Gaussian, with expectation
%    E(X_t) = exp(F * t) * X_0
% and covariance
%    Cov(X_t) = exp(F*t) Cov(X_0) exp(F*t)' ...
%               + exp(F*t)* int_0^t exp(-F s) A A' exp(-F' s) ds * exp(F*t)'
    
    properties
        % inherits f, A, nSteps from base
        F;
    end

    methods
        function sysModel = LinearContinuousSystemModel(F, A)
            if nargin == 0
                return;
            elseif nargin == 1
                error('Too few input arguments.');
            else
                assert(isnumeric(F));
                assert(isnumeric(A));
                sysModel.F = F;
                sysModel.A = A;
                sysModel.f = @(x,t) (F*x);
                sysModel.J = @(x,t) (F);
                sysModel.nSteps = 1;  % not used, since we have exact solution
            end
        end

        function newState = simulate(this, oldState, dt, curTime)
        % Simulate time evolution of the system for dt units of time
        % returns a state vector
            switch nargin
              case {0, 1}
                error('Too few arguments');
              case 2
                dt = 1;
                curTime = 0;
              case 3
                curTime = 0;
            end
            
            A = this.A;
            F = this.F;
            nNoise = size(A, 2); % dimension of noise vector
            nState = length(oldState);
            
            Q = A' * A;

            % See Van Loan, "Computing Integrals Involving the
            % Matrix Exponential".
            expF = expm(F * dt);
            M = [F, Q; zeros(nState), -F'];
            Phi = expm(M * dt);
            Phi_12 = Phi(1:nState, nState+1:2*nState);
            Phi_22 = Phi(nState+1:2*nState, nState+1:2*nState);
            C = Phi_22' * Phi_12;
            C = expF * C * expF';
            C = (C + C')/2;
            
            cholC = sdchol(C);

            newState = expF * oldState ...
                + cholC' * randn(size(cholC,1), 1);
            
        end        
        
        function [x, P] = predict(this, oldMean, oldCov, dt, curTime)
        % oldDist = old distribution
        % dt = time interval

            A = this.A;
            F = this.F;
            nNoise = size(A, 2); % dimension of noise vector
            nState = length(oldMean);
            
            Q = A' * A;
            expF = expm(F * dt);
            
            x = expF * oldMean;

            % See Van Loan, "Computing Integrals Involving the
            % Matrix Exponential".
            M = [F, Q; zeros(nState), -F'];
            Phi = expm(M * dt);
            Phi_12 = Phi(1:nState, nState+1:2*nState);
            Phi_22 = Phi(nState+1:2*nState, nState+1:2*nState);

            P = expF * (oldCov + Phi_22' * Phi_12) * expF';
            P = (P + P')/2;
            
        end
    end
    


end