UnscentedContinuousSystemModel

classdef UnscentedContinuousSystemModel < BaseContinuousSystemModel
%UNSCENTEDCONTINUOUSSYSTEMMODEL  Class modeling a continuous time system with EKF predictions
%   sysModel = UnscentedContinuousSystemModel(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 = UnscentedContinuousSystemModel(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 describing system evolution
%      A - noise correlation matrix
%      nSteps - number of steps for between simulation intervals
%      W_mean - (unscaled) weight between 0 and 1 assigned to the mean for transform
%      isScaled  - boolean indicating whether to scale unscented transform
%      alpha - scaling factor for unscented transform
%      beta - parameter adjusting for deviation from Gaussian distribution
    
    properties
        % inherits f, A, nSteps from GaussianContinousSystemModel
        W_mean = 0;
        isScaled = true;
        alpha = 1e-3;
        beta = 2;
    end

    methods
        function sysModel = UnscentedContinuousSystemModel(varargin)
            sysModel = sysModel@BaseContinuousSystemModel(varargin{:});
        end
        
        function [x, P] = predict(this, x, P, dt, curTime)
        % oldDist = old distribution
        % T = time interval

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

            ds = dt/nSteps;

            for tau = 1:nSteps;
                [x, P] = this.propagateGaussian(@(x) (x+f(x,curTime)*ds), ...
                                                x, P);
                P = P + ds*(A*A');  % add in error due to process noise covariance
                
                curTime = curTime + ds;
            end;
        end
    
        function [x, P] = propagateGaussian(this, f, x, P)
            
            n = size(x,1);
            [W_s, W_c, sigmaOffsets] = symmetricSigmaPoints(n, this.W_mean, ...
                                         this.isScaled, this.alpha, this.beta);                 
            cholP = sdchol(P);
            [x, P] = unscentedTransform(f, x, cholP, W_s, W_c, sigmaOffsets);

        end
    end
end