EKFContinuousSystemModel

classdef EKFContinuousSystemModel < BaseContinuousSystemModel
%EKFCONTINUOUSSYSTEMMODEL  Class modeling a continuous time system with EKF predictions
%   sysModel = EKFContinuousSystemModel(f, A, J) creates a class
%   representing the stochastic differential equation
%     dX = f(X,t) dt + A dW
%   with f a function handle, A a matrix, and J the Jacobian of f
%   with respect to x.  This equation is often written as
%     dx/dt = f(x,t) + w    with w = white noise with covariance A * A'
%


%   sysModel = EKFContinuousSystemModel(f, Q, J, 'Name1', Value1, 'Name2', Value2, ...)
%
%   Optional Inputs:
%      'Name1', Value1, 'Name2', Value2, ... is a variable length
%      list of parameter/value pairs.
%
%   properties
%      f - function
%      Q - noise covariance matrix
%      J - jacobian of f with respect to x
%      


    % inherits properties f, A, J, nSteps from GaussianContinousSystemModel
    
    methods
        function sysModel = EKFContinuousSystemModel(f, A, J, varargin)
            if nargin == 0
                superargs = {};
            elseif nargin == 1
                superargs = {f};
            elseif nargin == 2
                error('Must specify Jacobian');
            else
                superargs = {f, A, 'J', J, varargin{:}};
            end
            sysModel = sysModel@BaseContinuousSystemModel(superargs{:});
        end
        
        function [x, P] = predict(this, x, P, dt, curTime)
        % oldMean = old mean
        % oldCov = old covariance
        % dt = time interval

            if nargin == 4
                curTime = 0;
            end
            
            f = this.f;
            A = this.A;
            nSteps = this.nSteps;

            ds = dt/nSteps;
            nNoise = size(A,1);  % dimension of noise w

            for tau = 1:nSteps;
                Jac = eye(size(P,1)) + this.J(x) * ds;
                
                x = x + f(x,curTime)*ds;
                P = Jac * P * Jac';
                
                P = P + ds*(A*A');  % add in error due to process noise covariance
                curTime = curTime + ds;
            end;
        end

    end

end