function [xnew, Pnew, K, loglik] = Simple_Kalman_Update(F, H, Q, R, y, x, P)
% KALMAN_UPDATE Do a one step update of the Kalman filter
% [xnew, Pnew, K, loglik] = Simple_Kalman_Update(F, H, Q, R, y, x, P)
%
% Given
%  x(:) =   E[ X | Y(1:t-1) ] and
%  P(:,:) = Var[ X(t-1) | Y(1:t-1) ],
% compute 
%  xnew(:) =   E[ X | Y(1:t-1) ] and
%  Pnew(:,:) = Var[ X(t) | Y(1:t) ],
% using
%  y(:)   - the observation at time t
%  F(:,:) - the system matrix
%  H(:,:) - the observation matrix
%  Q(:,:) - the system covariance
%  R(:,:) - the observation covariance
%

% a priori
xpred   = F*x;
Ppred   = F*P*F' + Q;

% error (innovation)
e       = y - H*xpred; 

S       = H*Ppred*H' + R;
Sinv    = inv(S);
K       = Ppred*H'*Sinv; % Kalman gain matrix

% log likelihood
loglik  = gaussian_prob(e, zeros(size(e)), S, 0); %0);

% updated
xnew    = xpred + K*e;
Pnew    = (eye(size(F)) - K*H)*Ppred;

%K