function [znew, Qnew] = kalman_update( A, C, U, V, x, z, Q, initial)
% KALMAN_UPDATE Do a one step update of the Kalman filter
% [znew, Qnew, loglik] = kalman_update( A, C, U, V, x, z, Q, initial )
%
% Given
%  z(:) =   E[ z | x(1:t-1) ] and
%  Q(:,:) = Var[ z(t-1) | x(1:t-1) ],
% compute 
%  znew(:) =   E[ z | x(1:t-1) ] and
%  Qnew(:,:) = Var[ z(t) | x(1:t) ],
% using
%  x(:)   - the observation at time t
%  A(:,:) - the system matrix
%  C(:,:) - the observation matrix
%  U(:,:) - the system covariance
%  V(:,:) - the observation covariance
%
% If initial=true, z and Q are taken as the initial conditions (and A and C are ignored).
%
% 式番号はテキスト参照

if nargin < 8, initial = 0; end

if initial
  zpred = z;
  Qpred = Q;
else
  zpred = A*z;                        %(62)式
  Qpred = U + A*Q*A';                 %(61b)式
end

Qnew = inv( inv(Qpred)+C'*inv(V)*C ); %(69b)式
K    = Qnew*C'*inv(V);                %(69c)式 --Kalman gain matrix
znew = zpred + K*(x-C*zpred);         %(69a)式