|
wolffd@0
|
1 function [xnew, Vnew, loglik, VVnew] = kalman_update(A, C, Q, R, y, x, V, varargin)
|
|
wolffd@0
|
2 % KALMAN_UPDATE Do a one step update of the Kalman filter
|
|
wolffd@0
|
3 % [xnew, Vnew, loglik] = kalman_update(A, C, Q, R, y, x, V, ...)
|
|
wolffd@0
|
4 %
|
|
wolffd@0
|
5 % INPUTS:
|
|
wolffd@0
|
6 % A - the system matrix
|
|
wolffd@0
|
7 % C - the observation matrix
|
|
wolffd@0
|
8 % Q - the system covariance
|
|
wolffd@0
|
9 % R - the observation covariance
|
|
wolffd@0
|
10 % y(:) - the observation at time t
|
|
wolffd@0
|
11 % x(:) - E[X | y(:, 1:t-1)] prior mean
|
|
wolffd@0
|
12 % V(:,:) - Cov[X | y(:, 1:t-1)] prior covariance
|
|
wolffd@0
|
13 %
|
|
wolffd@0
|
14 % OPTIONAL INPUTS (string/value pairs [default in brackets])
|
|
wolffd@0
|
15 % 'initial' - 1 means x and V are taken as initial conditions (so A and Q are ignored) [0]
|
|
wolffd@0
|
16 % 'u' - u(:) the control signal at time t [ [] ]
|
|
wolffd@0
|
17 % 'B' - the input regression matrix
|
|
wolffd@0
|
18 %
|
|
wolffd@0
|
19 % OUTPUTS (where X is the hidden state being estimated)
|
|
wolffd@0
|
20 % xnew(:) = E[ X | y(:, 1:t) ]
|
|
wolffd@0
|
21 % Vnew(:,:) = Var[ X(t) | y(:, 1:t) ]
|
|
wolffd@0
|
22 % VVnew(:,:) = Cov[ X(t), X(t-1) | y(:, 1:t) ]
|
|
wolffd@0
|
23 % loglik = log P(y(:,t) | y(:,1:t-1)) log-likelihood of innovatio
|
|
wolffd@0
|
24
|
|
wolffd@0
|
25 % set default params
|
|
wolffd@0
|
26 u = [];
|
|
wolffd@0
|
27 B = [];
|
|
wolffd@0
|
28 initial = 0;
|
|
wolffd@0
|
29
|
|
wolffd@0
|
30 args = varargin;
|
|
wolffd@0
|
31 for i=1:2:length(args)
|
|
wolffd@0
|
32 switch args{i}
|
|
wolffd@0
|
33 case 'u', u = args{i+1};
|
|
wolffd@0
|
34 case 'B', B = args{i+1};
|
|
wolffd@0
|
35 case 'initial', initial = args{i+1};
|
|
wolffd@0
|
36 otherwise, error(['unrecognized argument ' args{i}])
|
|
wolffd@0
|
37 end
|
|
wolffd@0
|
38 end
|
|
wolffd@0
|
39
|
|
wolffd@0
|
40 % xpred(:) = E[X_t+1 | y(:, 1:t)]
|
|
wolffd@0
|
41 % Vpred(:,:) = Cov[X_t+1 | y(:, 1:t)]
|
|
wolffd@0
|
42
|
|
wolffd@0
|
43 if initial
|
|
wolffd@0
|
44 if isempty(u)
|
|
wolffd@0
|
45 xpred = x;
|
|
wolffd@0
|
46 else
|
|
wolffd@0
|
47 xpred = x + B*u;
|
|
wolffd@0
|
48 end
|
|
wolffd@0
|
49 Vpred = V;
|
|
wolffd@0
|
50 else
|
|
wolffd@0
|
51 if isempty(u)
|
|
wolffd@0
|
52 xpred = A*x;
|
|
wolffd@0
|
53 else
|
|
wolffd@0
|
54 xpred = A*x + B*u;
|
|
wolffd@0
|
55 end
|
|
wolffd@0
|
56 Vpred = A*V*A' + Q;
|
|
wolffd@0
|
57 end
|
|
wolffd@0
|
58
|
|
wolffd@0
|
59 e = y - C*xpred; % error (innovation)
|
|
wolffd@0
|
60 n = length(e);
|
|
wolffd@0
|
61 ss = length(A);
|
|
wolffd@0
|
62 S = C*Vpred*C' + R;
|
|
wolffd@0
|
63 Sinv = inv(S);
|
|
wolffd@0
|
64 ss = length(V);
|
|
wolffd@0
|
65 loglik = gaussian_prob(e, zeros(1,length(e)), S, 1);
|
|
wolffd@0
|
66 K = Vpred*C'*Sinv; % Kalman gain matrix
|
|
wolffd@0
|
67 % If there is no observation vector, set K = zeros(ss).
|
|
wolffd@0
|
68 xnew = xpred + K*e;
|
|
wolffd@0
|
69 Vnew = (eye(ss) - K*C)*Vpred;
|
|
wolffd@0
|
70 VVnew = (eye(ss) - K*C)*A*V;
|
|
wolffd@0
|
71
|
