function [state, V, loglike] = kf( y, R, A, C, Q, init_state, Vo)
%
% This is a straight implementation of the Kalman filter that also
% computes the value of the log likelihood function.
%
%        [state, V, loglike] = kf(y, R, A, C, Q, init_state, Vo);
%
%
% INPUT:
%
%        y                 -           matrix containing
%                                      measurement vectors (each
%                                      column contains a
%                                      measurement) 
%
%        R                 -           three dimensional matrix
%                                      containing the sequence of 
%                                      covariance matrices for the
%                                      measurements (R(:,:,t) is
%                                      the matrix at time t)
%                                      
%        A                 -           sequence of state feedback
%                                      matrices (A(:,:,t) is the
%                                      matrix at time t)
%
%        C                 -           sequence of state output
%                                      matrices (C(t,:) is the
%                                      matrix at time t) 
%
%        Q                 -           sequence of 
%                                      covariance matrices for the
%                                      process noise (Q(:,:,t)
%                                      is the matrix at time t)
%
%        init_state        -           initial state vector
%
%        Vo                -           covariance matrix for the
%                                      initial state 
%
% 
% OUTPUT:
%
%        state             -            matrix containing the
%                                       state estimates (each column
%                                       contains a state vector)
%
%        V                 -            three dimensional matrix
%                                       containing the covariance matrix
%                                       sequence for the state estimates
%                                       (V(:,:,t) is the matrix at time
%                                       t)
%
%        loglike           -            The loglikelihood value for the
%                                       sequence of measurements
%                                       contained in y 
%
%
%
N = size(A,1);
M = size(y,1);
T = size(y,2);
%
V = zeros([N,N,T+1]);
state = zeros(N,T+1);
V(:,:,1) = Vo;
state(:,1) = init_state;
%
% initialize some matrices
%
pred_V = zeros(N,N);
S = zeros(M,M);
K = zeros(N,M);
%
% Main filtering loop
%
ll_cons = -(M/2)*log(2*pi);
loglike = 0;
for t = 1:T
  %
  % time update
  %
  pred_state = A*state(:,t);        % state prediction 
  pred_V = A*V(:,:,t)*A' + Q;       % covariance prediction
  pred_y = C*pred_state;            % measurement prediction
  %
  % measurement update
  %
  S = C*pred_V*C' + R;              % measurement prediction covariance
  K = pred_V*C'*pinv(S);            % filter gain
  nu = y(:,t) - pred_y;             % measurement prediction error (innovation)
  state(:,t+1) = pred_state + K*nu; % state estimate
  V(:,:,t+1) = pred_V - K*S*K';     % covariance matrix update
  %
  % compute log likelihood
  %
  loglike = loglike + ll_cons - 0.5*log(det(S)) - 0.5*nu'*inv(S)*nu;
  %
end
%
%
return;
%
% [EOF]
