wolffd@0: function [xnew, Vnew, loglik, VVnew] = kalman_update(A, C, Q, R, y, x, V, varargin)
wolffd@0: % KALMAN_UPDATE Do a one step update of the Kalman filter
wolffd@0: % [xnew, Vnew, loglik] = kalman_update(A, C, Q, R, y, x, V, ...)
wolffd@0: %
wolffd@0: % INPUTS:
wolffd@0: % A - the system matrix
wolffd@0: % C - the observation matrix 
wolffd@0: % Q - the system covariance 
wolffd@0: % R - the observation covariance
wolffd@0: % y(:)   - the observation at time t
wolffd@0: % x(:) - E[X | y(:, 1:t-1)] prior mean
wolffd@0: % V(:,:) - Cov[X | y(:, 1:t-1)] prior covariance
wolffd@0: %
wolffd@0: % OPTIONAL INPUTS (string/value pairs [default in brackets])
wolffd@0: % 'initial' - 1 means x and V are taken as initial conditions (so A and Q are ignored) [0]
wolffd@0: % 'u'     - u(:) the control signal at time t [ [] ]
wolffd@0: % 'B'     - the input regression matrix
wolffd@0: %
wolffd@0: % OUTPUTS (where X is the hidden state being estimated)
wolffd@0: %  xnew(:) =   E[ X | y(:, 1:t) ] 
wolffd@0: %  Vnew(:,:) = Var[ X(t) | y(:, 1:t) ]
wolffd@0: %  VVnew(:,:) = Cov[ X(t), X(t-1) | y(:, 1:t) ]
wolffd@0: %  loglik = log P(y(:,t) | y(:,1:t-1)) log-likelihood of innovatio
wolffd@0: 
wolffd@0: % set default params
wolffd@0: u = [];
wolffd@0: B = [];
wolffd@0: initial = 0;
wolffd@0: 
wolffd@0: args = varargin;
wolffd@0: for i=1:2:length(args)
wolffd@0:   switch args{i}
wolffd@0:    case 'u', u = args{i+1};
wolffd@0:    case 'B', B = args{i+1};
wolffd@0:    case 'initial', initial = args{i+1};
wolffd@0:    otherwise, error(['unrecognized argument ' args{i}])
wolffd@0:   end
wolffd@0: end
wolffd@0: 
wolffd@0: %  xpred(:) = E[X_t+1 | y(:, 1:t)]
wolffd@0: %  Vpred(:,:) = Cov[X_t+1 | y(:, 1:t)]
wolffd@0: 
wolffd@0: if initial
wolffd@0:   if isempty(u)
wolffd@0:     xpred = x;
wolffd@0:   else
wolffd@0:     xpred = x + B*u;
wolffd@0:   end
wolffd@0:   Vpred = V;
wolffd@0: else
wolffd@0:   if isempty(u)
wolffd@0:     xpred = A*x;
wolffd@0:   else
wolffd@0:     xpred = A*x + B*u;
wolffd@0:   end
wolffd@0:   Vpred = A*V*A' + Q;
wolffd@0: end
wolffd@0: 
wolffd@0: e = y - C*xpred; % error (innovation)
wolffd@0: n = length(e);
wolffd@0: ss = length(A);
wolffd@0: S = C*Vpred*C' + R;
wolffd@0: Sinv = inv(S);
wolffd@0: ss = length(V);
wolffd@0: loglik = gaussian_prob(e, zeros(1,length(e)), S, 1);
wolffd@0: K = Vpred*C'*Sinv; % Kalman gain matrix
wolffd@0: % If there is no observation vector, set K = zeros(ss).
wolffd@0: xnew = xpred + K*e;
wolffd@0: Vnew = (eye(ss) - K*C)*Vpred;
wolffd@0: VVnew = (eye(ss) - K*C)*A*V;
wolffd@0: