UnscentedSystemModel

classdef UnscentedSystemModel < BaseDiscreteSystemModel
%UNSCENTEDSYSTEMMODEL Class modeling a discrete time system with UKF predictions
%   sysModel = UKFSystemModel(f, Q) creates a class
%   representing the discrete time system x(t) which evolves via
%      x(t+1) = f(x(t), t) + v
%   where v is gaussian noise with mean 0 and covariance Q
%
%   sysModel = UnscentedSystemModel(f, Q, J_x, 'isLinear', false),
%   models the nonlinear system:
%      x(t+1) = f(x(t), t, v)
%
%   sysModel = UnscentedSystemModel(f, Q, J_x, '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
%      isLinearNoise - true if the system model has additive noise
%      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, Q, isLinear from BaseDiscreteSystemModel
        
        W_mean = 0;
        isScaled = true;  % true if the transform is scaled
        alpha = 1e-3;
        beta = 2;
    end

    methods
        function sysModel = UnscentedSystemModel(varargin)
            sysModel = sysModel@BaseDiscreteSystemModel(varargin{:});
        end
        
        function [x, P] = predict(this, x, P, dt, curTime)

        persistent oldn W_s W_c sigmaOffsets;
            cholP = sdchol(P);
            cholQ = this.cholQ;
            
            n = size(cholP, 1);
            if isempty(oldn) || (oldn ~= n)
                % cache sigma data
                % FIXME: if this.alpha, beta, etc changes we need
                % to recompute.  Which kinda indicates these should
                % be private local variables
                [W_s, W_c, sigmaOffsets] = symmetricSigmaPoints(n, this.W_mean, ...
                                          this.isScaled, this.alpha, this.beta);
                
                 oldn = n;
             end

             [x, P] = unscentedTransform(this.f, x, cholP, W_s, W_c, ...
                                         sigmaOffsets, this.isLinearNoise, ...
                                         cholQ);

            
            
            % FIXME FIXME: UT should have consistant interface:
            % either use cholQ always or ignore
            if this.isLinearNoise
                P = P + this.Q;
            end
        end
        
        
    end
    
end