UnscentedObserverModel

classdef UnscentedObserverModel < BaseObserverModel
%UNSCENTEDOBSERVERMODEL  Observer model based on Unscented Kalman Filter
%   sysModel = UnscentedObserverModel(h, R, J_x) creates a class
%   representing an observer measuring a state x by the formula
%      z = h(x,t) + v
%   where v is gaussian noise with mean 0 and covariance R and J_x
%   is the Jacobian of h with respect to x.
%
%   sysModel = UnscentedObserverModel(h, R, J_x, J_v) permits nonlinear measurements
%      z = h(x,t,v)
%   where h, R, J_x are as above, and J_v is the Jacobian of h with
%   respect to v.
%
%   sysModel = UnscentedObserverModel(h, R, J_x, 'Name1', Value1, 'Name2', Value2, ...)
%   sysModel = UnscentedObserverModel(h, R, J_x, J_v, 'Name1', Value1, 'Name2', Value2, ...)
%   
%   Optional Inputs:
%      'Name1', Value1, 'Name2', Value2, ... is a variable length
%      list of parameter/value pairs.
%
%   properties
%      h - function
%      R - 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 h, R, isLinear from base
        
        W_mean = 0;
        isScaled = true;  % true if the transform is scaled
        alpha = 1e-3;
        beta = 2;
    end

    methods
        function obsModel = UnscentedObserverModel(varargin)
            obsModel = obsModel@BaseObserverModel(varargin{:});
        end
        
        function [S C innovation] = observerData(this, predMean, predCov, meas, curTime)
            persistent oldn W_s W_c sigmaOffsets;
            
            cholP = sdchol(predCov);
            cholR = this.cholR;
            
            n = length(predMean);
            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
                
            [y, S, C] = unscentedTransform(this.h, predMean, cholP, W_s, W_c, sigmaOffsets, ...
                           this.isLinearNoise, cholR);
            % FIXME FIXME: UT should have consistant interface:
            % either use cholR always or ignore
            % Get singularities right now in linear case...
            if this.isLinearNoise
                S = S + this.R;
            end
            
            innovation = meas - y;
        end

    end
    
end