ekf_update1.m

%EKF_UPDATE1  1st order Extended Kalman Filter update step
%
% Syntax:
%   [M,P,K,MU,S,LH] = EKF_UPDATE1(M,P,Y,H,R,[h,V,param])
%
% In:
%   M  - Nx1 mean state estimate after prediction step
%   P  - NxN state covariance after prediction step
%   Y  - Dx1 measurement vector.
%   H  - Derivative of h() with respect to state as matrix,
%        inline function, function handle or name
%        of function in form H(x,param)
%   R  - Measurement noise covariance.
%   h  - Mean prediction (innovation) as vector,
%        inline function, function handle or name
%        of function in form h(x,param).               (optional, default H(x)*X)
%   V  - Derivative of h() with respect to noise as matrix,
%        inline function, function handle or name
%        of function in form V(x,param).               (optional, default identity)
%   param - Parameters of h                            (optional, default empty)
%
% Out:
%   M  - Updated state mean
%   P  - Updated state covariance
%   K  - Computed Kalman gain
%   MU - Predictive mean of Y
%   S  - Predictive covariance of Y
%   LH - Predictive probability (likelihood) of measurement.
%   
% Description:
%   Extended Kalman Filter measurement update step.
%   EKF model is
%
%     y[k] = h(x[k],r),   r ~ N(0,R)
%
% See also:
%   EKF_PREDICT1, EKF_PREDICT2, EKF_UPDATE2, DER_CHECK,
%   LTI_DISC, KF_UPDATE, KF_PREDICT

% Copyright (C) 2002-2006 Simo Särkkä
%
% $Id: ekf_update1.m 137 2008-02-20 11:46:08Z jmjharti $
%
% This software is distributed under the GNU General Public 
% Licence (version 2 or later); please refer to the file 
% Licence.txt, included with the software, for details.

function [M,P,K,MU,S,LH] = ekf_update1(M,P,y,H,R,h,V,param)

  %
  % Check which arguments are there
  %
  if nargin < 5
    error('Too few arguments');
  end
  if nargin < 6
    h = [];
  end
  if nargin < 7
    V = [];
  end
  if nargin < 8
    param = [];
  end

  %
  % Apply defaults
  %
  if isempty(V)
    V = eye(size(R,1));
  end

  %
  % Evaluate matrices
  %
  if isnumeric(H)
    % nop
  elseif isstr(H) | strcmp(class(H),'function_handle')
    H = feval(H,M,param);
  else
    H = H(M,param);
  end

  if isempty(h)
    MU = H*M;
  elseif isnumeric(h)
    MU = h;
  elseif isstr(h) | strcmp(class(h),'function_handle')
    MU = feval(h,M,param);
  else
    MU = h(M,param);
  end

  if isnumeric(V)
    % nop
  elseif isstr(V) | strcmp(class(V),'function_handle')
    V = feval(V,M,param);
  else
    V = V(M,param);
  end

  
  %
  % update step
  %  
  S = (V*R*V' + H*P*H');
  K = P*H'/S;
  M = M + K * (y-MU);
  P = P - K*S*K';

  if nargout > 5
    LH = gauss_pdf(y,MU,S);
  end