function [z, Q] = kalman_filter( x, A, C, U, V, init_z, init_Q )
% Kalman filter.
% [z, Q] = kalman_filter( x, A, C, U, V, init_z, init_Q )
%
% Inputs:
% x(:,t)   - the observation at time t
% A(:,:) - the system matrix
% C(:,:) - the observation matrix
% U(:,:) - the system covariance
% V(:,:) - the observation covariance
% init_z(:) - the initial state
% init_Q(:,:) - the initial covariance
% Outputs:
% z(:,t) = E[ z_t | t ]
% Q(:,:,t) = Cov[ z_t | t ]

[os T] = size(x);
ss = size(A,1);

z = zeros(ss, T);
Q = zeros(ss, ss, T);

loglik = 0;
for t=1:T
  if t==1
    prevz = init_z(:);
    prevQ = init_Q(:,:);
    initial = 1;
  else
    prevz = z(:,t-1);
    prevQ = Q(:,:,t-1);
    initial = 0;
  end
  [z(:,t), Q(:,:,t)] = ...
      kalman_update( A, C, U, V, x(:,t), prevz, prevQ, initial );
end