%KF_UPDATE  Kalman Filter update step
%
% Syntax:
%   [X,P,K,IM,IS,LH] = KF_UPDATE(X,P,Y,H,R)
%
% In:
%   X - Nx1 mean state estimate after prediction step
%   P - NxN state covariance after prediction step
%   Y - Dx1 measurement vector.
%   H - Measurement matrix.
%   R - Measurement noise covariance.
%
% Out:
%   X  - Updated state mean
%   P  - Updated state covariance
%   K  - Computed Kalman gain
%   IM - Mean of predictive distribution of Y
%   IS - Covariance or predictive mean of Y
%   LH - Predictive probability (likelihood) of measurement.
%   
% Description:
%   Kalman filter measurement update step. Kalman Filter
%   model is
%
%     x[k] = A*x[k-1] + B*u[k-1] + q,  q ~ N(0,Q)
%     y[k] = H*x[k]   + r,             r ~ N(0,R)
%
%   Prediction step of Kalman filter computes predicted
%   mean m-[k] and covariance P-[k] of state:
%
%     p(x[k] | y[1:k-1]) = N(x[k] | m-[k], P-[k])
%
%   See for instance KF_PREDICT how m-[k] and P-[k] are
%   calculated. 
%
%   Update step computes the posterior mean m[k] and
%   covariance P[k]  of state given new measurement:
%
%     p(x[k] | y[1:k]) = N(x[k] | m[k], P[k])
%
%   Innovation distribution is defined as
%
%     p(y[k] | y[1:k-1]) = N(y[k] | IM[k], IS[k])
%   
%   Updated mean x[k] and covarience P[k] are given by
%   the following equations (not the only possible ones):
%
%     v[k] = y[k] - H[k]*m-[k]
%     S[k] = H[k]*P-[k]*H[k]' + R[k]
%     K[k] = P-[k]*H[k]'*[S[k]]^(-1) 
%     m[k] = m-[k] + K[k]*v[k]
%     P[k] = P-[k] - K[k]*S[k]*K[k]'
%
% Example:
%   m = m0;
%   P = P0;
%   M = m0;
%   for i=1:size(Y,2)
%     [m,P] = kf_predict(m,P,A,Q);
%     [m,P] = kf_update(m,P,Y(:,i),H,R);
%     M = [M m];
%   end
%
% See also:
%   KF_PREDICT, EKF_UPDATE

% History:
%   26.02.2007 JH Added the equations for calculating the updated
%                 means and covariances to the description section.
%   12.01.2003 SS Symmetrized covariance update
%   20.11.2002 SS The first official version.
%
% Copyright (C) 2002, 2003 Simo Särkkä
% Copyright (C) 2007 Jouni Hartikainen
%
% $Id: kf_update.m 111 2007-09-04 12:09:23Z ssarkka $
%
% 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 [X,P,K,IM,IS,LH] = kf_update(X,P,y,H,R)

  %
  % Check which arguments are there
  %
  if nargin < 5
    error('Too few arguments');
  end

  %
  % update step
  %
  IM = H*X;
  IS = (R + H*P*H');
  K = P*H'/IS;
  X = X + K * (y-IM);
  P = P - K*IS*K';
  if nargout > 5
    LH = gauss_pdf(y,IM,IS);
  end