EKFObserverModel

classdef EKFObserverModel < BaseObserverModel
%EKFOBSERVERMODEL  Observer model based on Extended Kalman Filter
%   sysModel = EKFObserverModel(h, R, J_x) creates a class
%   representing an observer measuring a state x by the formula
%      z = h(x,t) + w
%   where w is gaussian noise with mean 0 and covariance R and J_x
%   is the Jacobian of h with respect to x.
%
%   sysModel = EKFObserverModel(h, R, J_x, J_w) permits nonlinear measurements
%      z = h(x,t,w)
%   where h, R, J_x are as above, and J_w is the Jacobian of h with
%   respect to w.
%
%   sysModel = EKFObserverModel(h, R, J_x, 'Name1', Value1, 'Name2', Value2, ...)
%   sysModel = EKFObserverModel(h, R, J_x, J_w, '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
%      J_x - Jacobian of h with respect to x
%      J_w - Jacobian of h with respect to w
%      isLinearNoise - true if the system model has additive noise
%      

    methods
        function obsModel = EKFObserverModel(h, R, J_x, varargin)
            if nargin == 0
                superargs = {};
            elseif nargin == 1
                superargs = {h};
            elseif nargin == 2
                error('Must specify Jacobian');
            elseif nargin == 3
                superargs = {h, R, 'J_x', J_x};
            else
                % the fourth argument can be either J_v or start of
                % property/value pairs.
                if isa(varargin{1}, 'function_handle')
                    superargs = [{h, R, 'J_x', J_x, 'J_w', varargin{1}}, ...
                                  varargin{2:end}];
                else
                    superargs = [{f, Q, 'J_x', J_x}, varargin{:}]
                end
            end
            obsModel = obsModel@BaseObserverModel(superargs{:});
        end
        
        function [S C innovation] = observerData(this, predMean, predCov, meas, curTime)
            [y S C] = ekfTransform(this.h, this.J_x, predMean, ...
                                   predCov, this.R, curTime, ...
                                   this.isLinearNoise, this.J_w);

            innovation = meas - y;
        end
    end

end