|
wolffd@0
|
1 function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin)
|
|
wolffd@0
|
2 % Kalman filter.
|
|
wolffd@0
|
3 % [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...)
|
|
wolffd@0
|
4 %
|
|
wolffd@0
|
5 % INPUTS:
|
|
wolffd@0
|
6 % y(:,t) - the observation at time t
|
|
wolffd@0
|
7 % A - the system matrix
|
|
wolffd@0
|
8 % C - the observation matrix
|
|
wolffd@0
|
9 % Q - the system covariance
|
|
wolffd@0
|
10 % R - the observation covariance
|
|
wolffd@0
|
11 % init_x - the initial state (column) vector
|
|
wolffd@0
|
12 % init_V - the initial state covariance
|
|
wolffd@0
|
13 %
|
|
wolffd@0
|
14 % OPTIONAL INPUTS (string/value pairs [default in brackets])
|
|
wolffd@0
|
15 % 'model' - model(t)=m means use params from model m at time t [ones(1,T) ]
|
|
wolffd@0
|
16 % In this case, all the above matrices take an additional final dimension,
|
|
wolffd@0
|
17 % i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m).
|
|
wolffd@0
|
18 % However, init_x and init_V are independent of model(1).
|
|
wolffd@0
|
19 % 'u' - u(:,t) the control signal at time t [ [] ]
|
|
wolffd@0
|
20 % 'B' - B(:,:,m) the input regression matrix for model m
|
|
wolffd@0
|
21 %
|
|
wolffd@0
|
22 % OUTPUTS (where X is the hidden state being estimated)
|
|
wolffd@0
|
23 % x(:,t) = E[X(:,t) | y(:,1:t)]
|
|
wolffd@0
|
24 % V(:,:,t) = Cov[X(:,t) | y(:,1:t)]
|
|
wolffd@0
|
25 % VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2
|
|
wolffd@0
|
26 % loglik = sum{t=1}^T log P(y(:,t))
|
|
wolffd@0
|
27 %
|
|
wolffd@0
|
28 % If an input signal is specified, we also condition on it:
|
|
wolffd@0
|
29 % e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]
|
|
wolffd@0
|
30 % If a model sequence is specified, we also condition on it:
|
|
wolffd@0
|
31 % e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]
|
|
wolffd@0
|
32
|
|
wolffd@0
|
33 [os T] = size(y);
|
|
wolffd@0
|
34 ss = size(A,1); % size of state space
|
|
wolffd@0
|
35
|
|
wolffd@0
|
36 % set default params
|
|
wolffd@0
|
37 model = ones(1,T);
|
|
wolffd@0
|
38 u = [];
|
|
wolffd@0
|
39 B = [];
|
|
wolffd@0
|
40 ndx = [];
|
|
wolffd@0
|
41
|
|
wolffd@0
|
42 args = varargin;
|
|
wolffd@0
|
43 nargs = length(args);
|
|
wolffd@0
|
44 for i=1:2:nargs
|
|
wolffd@0
|
45 switch args{i}
|
|
wolffd@0
|
46 case 'model', model = args{i+1};
|
|
wolffd@0
|
47 case 'u', u = args{i+1};
|
|
wolffd@0
|
48 case 'B', B = args{i+1};
|
|
wolffd@0
|
49 case 'ndx', ndx = args{i+1};
|
|
wolffd@0
|
50 otherwise, error(['unrecognized argument ' args{i}])
|
|
wolffd@0
|
51 end
|
|
wolffd@0
|
52 end
|
|
wolffd@0
|
53
|
|
wolffd@0
|
54 x = zeros(ss, T);
|
|
wolffd@0
|
55 V = zeros(ss, ss, T);
|
|
wolffd@0
|
56 VV = zeros(ss, ss, T);
|
|
wolffd@0
|
57
|
|
wolffd@0
|
58 loglik = 0;
|
|
wolffd@0
|
59 for t=1:T
|
|
wolffd@0
|
60 m = model(t);
|
|
wolffd@0
|
61 if t==1
|
|
wolffd@0
|
62 %prevx = init_x(:,m);
|
|
wolffd@0
|
63 %prevV = init_V(:,:,m);
|
|
wolffd@0
|
64 prevx = init_x;
|
|
wolffd@0
|
65 prevV = init_V;
|
|
wolffd@0
|
66 initial = 1;
|
|
wolffd@0
|
67 else
|
|
wolffd@0
|
68 prevx = x(:,t-1);
|
|
wolffd@0
|
69 prevV = V(:,:,t-1);
|
|
wolffd@0
|
70 initial = 0;
|
|
wolffd@0
|
71 end
|
|
wolffd@0
|
72 if isempty(u)
|
|
wolffd@0
|
73 [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...
|
|
wolffd@0
|
74 kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial);
|
|
wolffd@0
|
75 else
|
|
wolffd@0
|
76 if isempty(ndx)
|
|
wolffd@0
|
77 [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...
|
|
wolffd@0
|
78 kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ...
|
|
wolffd@0
|
79 'initial', initial, 'u', u(:,t), 'B', B(:,:,m));
|
|
wolffd@0
|
80 else
|
|
wolffd@0
|
81 i = ndx{t};
|
|
wolffd@0
|
82 % copy over all elements; only some will get updated
|
|
wolffd@0
|
83 x(:,t) = prevx;
|
|
wolffd@0
|
84 prevP = inv(prevV);
|
|
wolffd@0
|
85 prevPsmall = prevP(i,i);
|
|
wolffd@0
|
86 prevVsmall = inv(prevPsmall);
|
|
wolffd@0
|
87 [x(i,t), smallV, LL, VV(i,i,t)] = ...
|
|
wolffd@0
|
88 kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ...
|
|
wolffd@0
|
89 'initial', initial, 'u', u(:,t), 'B', B(i,:,m));
|
|
wolffd@0
|
90 smallP = inv(smallV);
|
|
wolffd@0
|
91 prevP(i,i) = smallP;
|
|
wolffd@0
|
92 V(:,:,t) = inv(prevP);
|
|
wolffd@0
|
93 end
|
|
wolffd@0
|
94 end
|
|
wolffd@0
|
95 loglik = loglik + LL;
|
|
wolffd@0
|
96 end
|
|
wolffd@0
|
97
|
|
wolffd@0
|
98
|
|
wolffd@0
|
99
|
|
wolffd@0
|
100
|
|
wolffd@0
|
101
|
