-
Notifications
You must be signed in to change notification settings - Fork 2
/
ex.m
151 lines (138 loc) · 4.62 KB
/
ex.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
function varargout = ex (varargin)
% Example usage of KFGS.
%
% ex('ex_grad', Ns, No, Nt, Nstack)
% Compute the log-likelihood and gradient for a random problem having state
% vector size Ns, observation vector size No, time steps Nt, and at most
% Nstack saved intermediate steps.
[varargout{1:nargout}] = feval(varargin{:});
end
function ex_grad (Ns, No, Nt, Nstack)
% Set up a random problem. p holds F, H, Q, R, etc.
p = RandProb(Ns, No, Nt);
% 1 or 2. Just test different types of parameterizations.
p.grad_test = 2;
% Run the basic Kalman filter for timing comparison.
t = tic();
ll1 = KalmanFilter(p.F, p.Q, p.H, p.Rc, p.x0, p.Pp0c, p.Y);
etkf = toc(t);
% Now run kf_loglik_grad to get both the log likelihood and its gradient
% for the nominal set of parameters.
t = tic();
r = kf_rcd('init', 'm');
[ll2 ga] = kf_loglik_grad_cp(...
r, @(varargin)kl_fn(p, varargin{:}), Ns, Nt, ...
Nstack*(Ns+1)*Ns*8);
eta = toc(t);
% Compare.
fprintf(1, 'et = %1.2fs time compared with KF = %1.2fx\n', eta, eta/etkf);
end
function ex_smooth (Ns, No, Nt, Nstack)
% Set up a random problem.
p = RandProb(Ns, No, Nt);
% Run the basic Kalman filter for timing comparison.
t = tic();
ll1 = KalmanFilter(p.F, p.Q, p.H, p.Rc, p.x0, p.Pp0c, p.Y);
etkf = toc(t);
t = tic();
% Run the KF/smoother.
clear global gs;
r = kf_rcd('init', 'm');
ll2 = kf_loglik_smooth_cp(...
r, @(varargin)kl_fn(p, varargin{:}), ...
@kl_ofn, Ns, Nt, Nstack*(Ns+1)*Ns*8);
eta = toc(t);
% Compare.
fprintf(1, 'et = %1.2fs time compared with KF = %1.2fx\n', eta, eta/etkf);
% In practice, we would access gs data here if desired.
% ...
% Then clear it.
clear global gs;
end
function p = RandProb (m, n, nt)
p.F = randn(m);
p.H = randn(n, m);
p.Pp0 = randn(m); p.Pp0 = p.Pp0'*p.Pp0; p.Pp0c = chol(p.Pp0);
p.x0 = randn(m, 1);
p.Y = randn(n, nt);
p.Q = randn(m); p.Q = p.Q'*p.Q;
p.R = randn(n); p.R = p.R'*p.R; p.Rc = chol(p.R);
end
function loglik = KalmanFilter (F, Q, H, Rc, x0, Pp0c, Y)
nt = size(Y, 2);
ns = size(F, 1);
loglik = 0;
for (it = 1:nt)
if (it > 1)
[xp Ppc] = kf_qrsc_predict(F, Q, xf, Pfc);
else
xp = x0;
Ppc = Pp0c;
end
[xf Pfc z Sc] = kf_qrsc_update(H, Rc, Y(:, it), xp, Ppc);
q = z' / Sc;
loglik = loglik - sum(log(diag(Sc))) - 0.5*q*q';
end
end
function varargout = kl_fn (p, key, tidx, varargin)
switch (key)
case 'i'
% Initial conditions.
varargout(1:2) = {p.x0 p.Pp0c};
case 'f'
% State-space transition matrix.
varargout{1} = p.F;
case 'fq'
% State-space transition matrix and process covariance.
varargout(1:2) = {p.F p.Q};
case 'h'
% Observation matrix.
varargout{1} = p.H;
case 'hry'
% Observation matrix, chol(observation covariance), observations at
% time index tidx.
varargout(1:3) = {p.H p.Rc p.Y(:, tidx)};
case 'll'
% Contribution to log-likelihood at time index tidx.
[Sc z] = deal(varargin{1:2});
q = z' / Sc;
varargout{1} = -sum(log(diag(Sc))) - 0.5*q*q';
case 'p'
% Partial derivatives of the log-likelihood term at time index tidx
% with respect to S = R + H Pp H' and z = y - H xp.
[Sc Si z] = deal(varargin{1:3});
p_S = -0.5*(kf_grad_Calc_LogDetC_C(Si) + kf_grad_Calc_ztinvCz_C(z, Sc));
p_z = -Sc \ (z' / Sc)';
varargout = {p_S p_z};
case 'g'
% Contribution of the term for time index tidx to the gradient of the
% log-likelihood function.
% If tidx == 1, there was no prediction and so no Q. However, there was
% a filter ('update').
[lambda mu] = deal(varargin{1:2});
switch (p.grad_test)
case 1
% Gradient wrt to diagonal elements of R and Q.
[m n] = size(p.H);
if (tidx > 1) mu1 = diag(reshape(mu, n, n));
else mu1 = zeros(n, 1); end
varargout{1} = -[mu1; diag(reshape(lambda, m, m))].';
case 2
% Gradient wrt a scalar multiple of R and Q.
if (tidx > 1) mu1 = mu(:)'*p.Q(:);
else mu1 = 0; end
varargout{1} = -[mu1, lambda(:)'*p.R(:)];
end
end
end
function kl_ofn (it, xp, Ppc, xf, Pfc, z, Sc, xs, Psc)
% I do the simplest thing possible: just save certain parts of the data to a
% global struct. More complicated ideas:
% - save smoothed state always, but the full cov matrix only every Kth time
% step
% - write data to disk; this is necessary if the total amount of data to
% save is large.
global gs;
gs.xss(:, it) = xs;
gs.vars(:, it) = diag(Psc'*Psc);
end