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