|
wolffd@0
|
1 function [xsmooth, Vsmooth, VVsmooth, loglik] = kalman_smoother(y, A, C, Q, R, init_x, init_V, varargin)
|
|
wolffd@0
|
2 % Kalman/RTS smoother.
|
|
wolffd@0
|
3 % [xsmooth, Vsmooth, VVsmooth, loglik] = kalman_smoother(y, A, C, Q, R, init_x, init_V, ...)
|
|
wolffd@0
|
4 %
|
|
wolffd@0
|
5 % The inputs are the same as for kalman_filter.
|
|
wolffd@0
|
6 % The outputs are almost the same, except we condition on y(:, 1:T) (and u(:, 1:T) if specified),
|
|
wolffd@0
|
7 % instead of on y(:, 1:t).
|
|
wolffd@0
|
8
|
|
wolffd@0
|
9 [os T] = size(y);
|
|
wolffd@0
|
10 ss = length(A);
|
|
wolffd@0
|
11
|
|
wolffd@0
|
12 % set default params
|
|
wolffd@0
|
13 model = ones(1,T);
|
|
wolffd@0
|
14 u = [];
|
|
wolffd@0
|
15 B = [];
|
|
wolffd@0
|
16
|
|
wolffd@0
|
17 args = varargin;
|
|
wolffd@0
|
18 nargs = length(args);
|
|
wolffd@0
|
19 for i=1:2:nargs
|
|
wolffd@0
|
20 switch args{i}
|
|
wolffd@0
|
21 case 'model', model = args{i+1};
|
|
wolffd@0
|
22 case 'u', u = args{i+1};
|
|
wolffd@0
|
23 case 'B', B = args{i+1};
|
|
wolffd@0
|
24 otherwise, error(['unrecognized argument ' args{i}])
|
|
wolffd@0
|
25 end
|
|
wolffd@0
|
26 end
|
|
wolffd@0
|
27
|
|
wolffd@0
|
28 xsmooth = zeros(ss, T);
|
|
wolffd@0
|
29 Vsmooth = zeros(ss, ss, T);
|
|
wolffd@0
|
30 VVsmooth = zeros(ss, ss, T);
|
|
wolffd@0
|
31
|
|
wolffd@0
|
32 % Forward pass
|
|
wolffd@0
|
33 [xfilt, Vfilt, VVfilt, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...
|
|
wolffd@0
|
34 'model', model, 'u', u, 'B', B);
|
|
wolffd@0
|
35
|
|
wolffd@0
|
36 % Backward pass
|
|
wolffd@0
|
37 xsmooth(:,T) = xfilt(:,T);
|
|
wolffd@0
|
38 Vsmooth(:,:,T) = Vfilt(:,:,T);
|
|
wolffd@0
|
39 %VVsmooth(:,:,T) = VVfilt(:,:,T);
|
|
wolffd@0
|
40
|
|
wolffd@0
|
41 for t=T-1:-1:1
|
|
wolffd@0
|
42 m = model(t+1);
|
|
wolffd@0
|
43 if isempty(B)
|
|
wolffd@0
|
44 [xsmooth(:,t), Vsmooth(:,:,t), VVsmooth(:,:,t+1)] = ...
|
|
wolffd@0
|
45 smooth_update(xsmooth(:,t+1), Vsmooth(:,:,t+1), xfilt(:,t), Vfilt(:,:,t), ...
|
|
wolffd@0
|
46 Vfilt(:,:,t+1), VVfilt(:,:,t+1), A(:,:,m), Q(:,:,m), [], []);
|
|
wolffd@0
|
47 else
|
|
wolffd@0
|
48 [xsmooth(:,t), Vsmooth(:,:,t), VVsmooth(:,:,t+1)] = ...
|
|
wolffd@0
|
49 smooth_update(xsmooth(:,t+1), Vsmooth(:,:,t+1), xfilt(:,t), Vfilt(:,:,t), ...
|
|
wolffd@0
|
50 Vfilt(:,:,t+1), VVfilt(:,:,t+1), A(:,:,m), Q(:,:,m), B(:,:,m), u(:,t+1));
|
|
wolffd@0
|
51 end
|
|
wolffd@0
|
52 end
|
|
wolffd@0
|
53
|
|
wolffd@0
|
54 VVsmooth(:,:,1) = zeros(ss,ss);
|
|
wolffd@0
|
55
|
