diff options
Diffstat (limited to 'sourcecodes/bnt-master/Kalman')
21 files changed, 932 insertions, 0 deletions
diff --git a/sourcecodes/bnt-master/Kalman/AR_to_SS.m b/sourcecodes/bnt-master/Kalman/AR_to_SS.m new file mode 100644 index 00000000..3a60e492 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/AR_to_SS.m @@ -0,0 +1,39 @@ +function [F,H,Q,R,initx, initV] = AR_to_SS(coef, C, y) +% +% Convert a vector auto-regressive model of order k to state-space form. +% [F,H,Q,R] = AR_to_SS(coef, C, y) +% +% X(i) = A(1) X(i-1) + ... + A(k) X(i-k+1) + v, where v ~ N(0, C) +% and A(i) = coef(:,:,i) is the weight matrix for i steps ago. +% We initialize the state vector with [y(:,k)' ... y(:,1)']', since +% the state vector stores [X(i) ... X(i-k+1)]' in order. + +[s s2 k] = size(coef); % s is the size of the state vector +bs = s * ones(1,k); % size of each block + +F = zeros(s*k); +for i=1:k + F(block(1,bs), block(i,bs)) = coef(:,:,i); +end +for i=1:k-1 + F(block(i+1,bs), block(i,bs)) = eye(s); +end + +H = zeros(1*s, k*s); +% we get to see the most recent component of the state vector +H(block(1,bs), block(1,bs)) = eye(s); +%for i=1:k +% H(block(1,bs), block(i,bs)) = eye(s); +%end + +Q = zeros(k*s); +Q(block(1,bs), block(1,bs)) = C; + +R = zeros(s); + +initx = zeros(k*s, 1); +for i=1:k + initx(block(i,bs)) = y(:, k-i+1); % concatenate the first k observation vectors +end + +initV = zeros(k*s); % no uncertainty about the state (since perfectly observable) diff --git a/sourcecodes/bnt-master/Kalman/CVS/Entries b/sourcecodes/bnt-master/Kalman/CVS/Entries new file mode 100644 index 00000000..8f3e6494 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/CVS/Entries @@ -0,0 +1,19 @@ +/AR_to_SS.m/1.1.1.1/Wed May 29 15:59:56 2002// +/README.txt/1.1.1.1/Mon Jun 7 14:39:28 2004// +/SS_to_AR.m/1.1.1.1/Wed May 29 15:59:56 2002// +/convert_to_lagged_form.m/1.1.1.1/Wed May 29 15:59:56 2002// +/ensure_AR.m/1.1.1.1/Wed May 29 15:59:56 2002// +/eval_AR_perf.m/1.1.1.1/Wed May 29 15:59:56 2002// +/kalman_filter.m/1.1.1.1/Wed May 29 15:59:56 2002// +/kalman_forward_backward.m/1.1.1.1/Sat Nov 2 00:32:36 2002// +/kalman_smoother.m/1.1.1.1/Wed May 29 15:59:56 2002// +/kalman_update.m/1.1.1.1/Wed May 29 15:59:56 2002// +/learn_AR.m/1.1.1.1/Wed May 29 15:59:56 2002// +/learn_AR_diagonal.m/1.1.1.1/Wed May 29 15:59:56 2002// +/learn_kalman.m/1.1.1.1/Wed May 29 15:59:56 2002// +/learning_demo.m/1.1.1.1/Wed Oct 23 15:17:42 2002// +/sample_lds.m/1.1.1.1/Fri Jan 24 19:36:02 2003// +/smooth_update.m/1.1.1.1/Wed May 29 15:59:56 2002// +/testKalman.m/1.1.1.1/Thu Jun 9 01:56:34 2005// +/tracking_demo.m/1.1.1.1/Sat Jan 18 22:49:22 2003// +D diff --git a/sourcecodes/bnt-master/Kalman/CVS/Repository b/sourcecodes/bnt-master/Kalman/CVS/Repository new file mode 100644 index 00000000..242601f9 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/CVS/Repository @@ -0,0 +1 @@ +FullBNT/Kalman diff --git a/sourcecodes/bnt-master/Kalman/CVS/Root b/sourcecodes/bnt-master/Kalman/CVS/Root new file mode 100644 index 00000000..f3bd14a6 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/CVS/Root @@ -0,0 +1 @@ +:ext:nsaunier@bnt.cvs.sourceforge.net:/cvsroot/bnt diff --git a/sourcecodes/bnt-master/Kalman/README.txt b/sourcecodes/bnt-master/Kalman/README.txt new file mode 100644 index 00000000..1608acb7 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/README.txt @@ -0,0 +1,17 @@ +Kalman filter toolbox written by Kevin Murphy, 1998. +See http://www.ai.mit.edu/~murphyk/Software/kalman.html for details. + +Installation +------------ + +1. Install KPMtools from http://www.ai.mit.edu/~murphyk/Software/KPMtools.html +3. Assuming you installed all these files in your matlab directory, In Matlab type + +addpath matlab/KPMtools +addpath matlab/Kalman + + +Demos +----- +See tracking_demo.m for a demo of 2D tracking. +See learning_demo.m for a demo of parameter estimation using EM. diff --git a/sourcecodes/bnt-master/Kalman/SS_to_AR.m b/sourcecodes/bnt-master/Kalman/SS_to_AR.m new file mode 100644 index 00000000..084b7141 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/SS_to_AR.m @@ -0,0 +1,22 @@ +function [coef, C] = SS_to_AR(F, Q, k, diagonal) +% +% Extract the parameters of a vector autoregresssive process of order k from the state-space form. +% [coef, C] = SS_to_AR(F, Q, k, diagonal) + +if nargin<4, diagonal = 0; end + +s = length(Q) / k; +bs = s*ones(1,k); +coef = zeros(s,s,k); +for i=1:k + if diagonal + coef(:,:,i) = diag(diag(F(block(1,bs), block(i,bs)))); + else + coef(:,:,i) = F(block(1,bs), block(i,bs)); + end +end +C = Q(block(1,bs), block(1,bs)); +if diagonal + C = diag(diag(C)); +end +%C = sqrt(Q(block(1,bs), block(1,bs))); % since cov(1,1) of full vector = C C' diff --git a/sourcecodes/bnt-master/Kalman/convert_to_lagged_form.m b/sourcecodes/bnt-master/Kalman/convert_to_lagged_form.m new file mode 100644 index 00000000..f972da5a --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/convert_to_lagged_form.m @@ -0,0 +1,14 @@ +function yy = convert_to_lagged_form(y, k) +% Create an observation vector yy(:,t) containing the last k values of y, newest first +% e.g., k=2, y = (a1 a2 a3) yy = a2 a3 +% (b1 b2 b3) b2 b2 +% a1 a2 +% b1 b2 + +[s T] = size(y); +bs = s*ones(1,k); +yy = zeros(k*s, T-k+1); +for i=1:k + yy(block(i,bs), :) = y(:, k-i+1:end-i+1); +end + diff --git a/sourcecodes/bnt-master/Kalman/ensure_AR.m b/sourcecodes/bnt-master/Kalman/ensure_AR.m new file mode 100644 index 00000000..e89edfa3 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/ensure_AR.m @@ -0,0 +1,10 @@ +function [A, C, Q, R, initx, initV] = ensure_AR(A, C, Q, R, initx, initV, k, obs, diagonal) +% +% Ensure that the system matrices have the right form for an autoregressive process. + +ss = length(A); +if nargin<8, obs=ones(ss, 1); end +if nargin<9, diagonal=0; end + +[coef, C] = SS_to_AR(A, Q, k, diagonal); +[A, C, Q, R, initx, initV] = AR_to_SS(coef, C, obs); diff --git a/sourcecodes/bnt-master/Kalman/eval_AR_perf.m b/sourcecodes/bnt-master/Kalman/eval_AR_perf.m new file mode 100644 index 00000000..9e6f05d8 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/eval_AR_perf.m @@ -0,0 +1,40 @@ +function [ypred, ll, mse] = eval_AR_perf(coef, C, y, model) +% Evaluate the performance of an AR model. +% +% Inputs +% coef(:,:,k,m) - coef. matrix to use for k steps back, model m +% C(:,:,m) - cov. matrix for model m +% y(:,t) - observation at time t +% model(t) - which model to use at time t (defaults to 1 if not specified) +% +% Outputs +% ypred(:,t) - the predicted value of y at t based on the evidence thru t-1. +% ll - log likelihood +% mse - mean squared error = sum_t d_t . d_t, where d_t = pred(y_t) - y(t) + +[s T] = size(y); +k = size(coef, 3); +M = size(coef, 4); + +if nargin<4, model = ones(1, T); end + +ypred = zeros(s, T); +ypred(:, 1:k) = y(:, 1:k); +mse = 0; +ll = 0; +for j=1:M + c(j) = log(normal_coef(C(:,:,j))); + invC(:,:,j) = inv(C(:,:,j)); +end +coef = reshape(coef, [s s*k M]); + +for t=k+1:T + m = model(t-k); + past = y(:,t-1:-1:t-k); + ypred(:,t) = coef(:, :, m) * past(:); + d = ypred(:,t) - y(:,t); + mse = mse + d' * d; + ll = ll + c(m) - 0.5*(d' * invC(:,:,m) * d); +end +mse = mse / (T-k+1); + diff --git a/sourcecodes/bnt-master/Kalman/kalman_filter.m b/sourcecodes/bnt-master/Kalman/kalman_filter.m new file mode 100644 index 00000000..a410204f --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/kalman_filter.m @@ -0,0 +1,101 @@ +function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin) +% Kalman filter. +% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...) +% +% INPUTS: +% y(:,t) - the observation at time t +% A - the system matrix +% C - the observation matrix +% Q - the system covariance +% R - the observation covariance +% init_x - the initial state (column) vector +% init_V - the initial state covariance +% +% OPTIONAL INPUTS (string/value pairs [default in brackets]) +% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ] +% In this case, all the above matrices take an additional final dimension, +% i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m). +% However, init_x and init_V are independent of model(1). +% 'u' - u(:,t) the control signal at time t [ [] ] +% 'B' - B(:,:,m) the input regression matrix for model m +% +% OUTPUTS (where X is the hidden state being estimated) +% x(:,t) = E[X(:,t) | y(:,1:t)] +% V(:,:,t) = Cov[X(:,t) | y(:,1:t)] +% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2 +% loglik = sum{t=1}^T log P(y(:,t)) +% +% If an input signal is specified, we also condition on it: +% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)] +% If a model sequence is specified, we also condition on it: +% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)] + +[os T] = size(y); +ss = size(A,1); % size of state space + +% set default params +model = ones(1,T); +u = []; +B = []; +ndx = []; + +args = varargin; +nargs = length(args); +for i=1:2:nargs + switch args{i} + case 'model', model = args{i+1}; + case 'u', u = args{i+1}; + case 'B', B = args{i+1}; + case 'ndx', ndx = args{i+1}; + otherwise, error(['unrecognized argument ' args{i}]) + end +end + +x = zeros(ss, T); +V = zeros(ss, ss, T); +VV = zeros(ss, ss, T); + +loglik = 0; +for t=1:T + m = model(t); + if t==1 + %prevx = init_x(:,m); + %prevV = init_V(:,:,m); + prevx = init_x; + prevV = init_V; + initial = 1; + else + prevx = x(:,t-1); + prevV = V(:,:,t-1); + initial = 0; + end + if isempty(u) + [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ... + kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial); + else + if isempty(ndx) + [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ... + kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ... + 'initial', initial, 'u', u(:,t), 'B', B(:,:,m)); + else + i = ndx{t}; + % copy over all elements; only some will get updated + x(:,t) = prevx; + prevP = inv(prevV); + prevPsmall = prevP(i,i); + prevVsmall = inv(prevPsmall); + [x(i,t), smallV, LL, VV(i,i,t)] = ... + kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ... + 'initial', initial, 'u', u(:,t), 'B', B(i,:,m)); + smallP = inv(smallV); + prevP(i,i) = smallP; + V(:,:,t) = inv(prevP); + end + end + loglik = loglik + LL; +end + + + + + diff --git a/sourcecodes/bnt-master/Kalman/kalman_forward_backward.m b/sourcecodes/bnt-master/Kalman/kalman_forward_backward.m new file mode 100644 index 00000000..9df240c4 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/kalman_forward_backward.m @@ -0,0 +1,97 @@ +% KALMAN_FORWARD_BACKWARD Forward Backward Propogation in Information Form +% +% +% Note : +% +% M file accompanying my technical note +% +% A Technique for Painless Derivation of Kalman Filtering Recursions +% +% available from http://www.mbfys.kun.nl/~cemgil/papers/painless-kalman.ps +% + +% Uses : + +% Change History : +% Date Time Prog Note +% 07-Jun-2001 2:24 PM ATC Created under MATLAB 5.3.1.29215a (R11.1) + +% ATC = Ali Taylan Cemgil, +% SNN - University of Nijmegen, Department of Medical Physics and Biophysics +% e-mail : cemgil@mbfys.kun.nl + +A = [1 1;0 1]; +C = [1 0]; +Q = eye(2)*0.01^2; +R = 0.001^2; +mu1 = [0;1]; +P1 = 3*Q; + +inv_Q = inv(Q); +inv_R = inv(R); + +y = [0 1.1 2 2.95 3.78]; + +T = length(y); +L = size(Q,1); + +%%%%% Forward message Passing +h_f = zeros(L, T); +K_f = zeros(L, L, T); +g_f = zeros(1, T); +h_f_pre = zeros(L, T); +K_f_pre = zeros(L, L, T); +g_f_pre = zeros(1, T); + + +K_f_pre(:, :, 1) = inv(P1); +h_f_pre(:,1) = K_f_pre(:, :, 1)*mu1; +g_f_pre(1) = -0.5*log(det(2*pi*P1)) - 0.5*mu1'*inv(P1)*mu1; + +for i=1:T, + h_f(:,i) = h_f_pre(:,i) + C'*inv_R*y(:,i); + K_f(:,:,i) = K_f_pre(:,:,i) + C'*inv_R*C; + g_f(i) = g_f_pre(i) -0.5*log(det(2*pi*R)) - 0.5*y(:,i)'*inv_R*y(:,i); + if i<T, + M = inv(A'*inv_Q*A + K_f(:,:,i)); + h_f_pre(:,i+1) = inv_Q*A*M*h_f(:,i); + K_f_pre(:,:,i+1) = inv_Q - inv_Q*A*M*A'*inv_Q; + g_f_pre(i+1) = g_f(i) -0.5*log(det(2*pi*Q)) + 0.5*log(det(2*pi*M)) + 0.5*h_f(:,i)'*M*h_f(:,i); + end; +end + +%%% Backward Message Passing +h_b = zeros(L, T); +K_b = zeros(L, L, T); +g_b = zeros(1, T); + +h_b_post = zeros(L, T); +K_b_post = zeros(L, L, T); +g_b_post = zeros(1, T); + +for i=T:-1:1, + h_b(:,i) = h_b_post(:,i) + C'*inv_R*y(:,i); + K_b(:,:,i) = K_b_post(:,:,i) + C'*inv_R*C; + g_b(i) = g_b_post(i) - 0.5*log(det(2*pi*R)) - 0.5*y(:,i)'*inv_R*y(:,i); + if i>1, + M = inv(inv_Q + K_b(:,:,i)); + h_b_post(:,i-1) = A'*inv(Q)*M*h_b(:,i); + K_b_post(:,:,i-1) = A'*inv_Q*(Q - M)*inv_Q*A; + g_b_post(i-1) = g_b(i) -0.5*log(det(2*pi*Q)) + 0.5*log(det(2*pi*M)) + 0.5*h_b(:,i)'*M*h_b(:,i); + end; +end; + + +%%%% Smoothed Estimates + +mu = zeros(size(h_f)); +Sig = zeros(size(K_f)); +g = zeros(size(g_f)); +lalpha = zeros(size(g_f)); + +for i=1:T, + Sig(:,:,i) = inv(K_b_post(:,:,i) + K_f(:,:,i)); + mu(:,i) = Sig(:,:,i)*(h_b_post(:,i) + h_f(:,i)); + g(i) = g_b_post(i) + g_f(:,i); + lalpha(i) = g(i) + 0.5*log(det(2*pi*Sig(:,:,i))) + 0.5*mu(:,i)'*inv(Sig(:,:,i))*mu(:,i); +end; \ No newline at end of file diff --git a/sourcecodes/bnt-master/Kalman/kalman_smoother.m b/sourcecodes/bnt-master/Kalman/kalman_smoother.m new file mode 100644 index 00000000..a3047011 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/kalman_smoother.m @@ -0,0 +1,55 @@ +function [xsmooth, Vsmooth, VVsmooth, loglik] = kalman_smoother(y, A, C, Q, R, init_x, init_V, varargin) +% Kalman/RTS smoother. +% [xsmooth, Vsmooth, VVsmooth, loglik] = kalman_smoother(y, A, C, Q, R, init_x, init_V, ...) +% +% The inputs are the same as for kalman_filter. +% The outputs are almost the same, except we condition on y(:, 1:T) (and u(:, 1:T) if specified), +% instead of on y(:, 1:t). + +[os T] = size(y); +ss = length(A); + +% set default params +model = ones(1,T); +u = []; +B = []; + +args = varargin; +nargs = length(args); +for i=1:2:nargs + switch args{i} + case 'model', model = args{i+1}; + case 'u', u = args{i+1}; + case 'B', B = args{i+1}; + otherwise, error(['unrecognized argument ' args{i}]) + end +end + +xsmooth = zeros(ss, T); +Vsmooth = zeros(ss, ss, T); +VVsmooth = zeros(ss, ss, T); + +% Forward pass +[xfilt, Vfilt, VVfilt, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ... + 'model', model, 'u', u, 'B', B); + +% Backward pass +xsmooth(:,T) = xfilt(:,T); +Vsmooth(:,:,T) = Vfilt(:,:,T); +%VVsmooth(:,:,T) = VVfilt(:,:,T); + +for t=T-1:-1:1 + m = model(t+1); + if isempty(B) + [xsmooth(:,t), Vsmooth(:,:,t), VVsmooth(:,:,t+1)] = ... + smooth_update(xsmooth(:,t+1), Vsmooth(:,:,t+1), xfilt(:,t), Vfilt(:,:,t), ... + Vfilt(:,:,t+1), VVfilt(:,:,t+1), A(:,:,m), Q(:,:,m), [], []); + else + [xsmooth(:,t), Vsmooth(:,:,t), VVsmooth(:,:,t+1)] = ... + smooth_update(xsmooth(:,t+1), Vsmooth(:,:,t+1), xfilt(:,t), Vfilt(:,:,t), ... + Vfilt(:,:,t+1), VVfilt(:,:,t+1), A(:,:,m), Q(:,:,m), B(:,:,m), u(:,t+1)); + end +end + +VVsmooth(:,:,1) = zeros(ss,ss); + diff --git a/sourcecodes/bnt-master/Kalman/kalman_update.m b/sourcecodes/bnt-master/Kalman/kalman_update.m new file mode 100644 index 00000000..ed8fc283 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/kalman_update.m @@ -0,0 +1,71 @@ +function [xnew, Vnew, loglik, VVnew] = kalman_update(A, C, Q, R, y, x, V, varargin) +% KALMAN_UPDATE Do a one step update of the Kalman filter +% [xnew, Vnew, loglik] = kalman_update(A, C, Q, R, y, x, V, ...) +% +% INPUTS: +% A - the system matrix +% C - the observation matrix +% Q - the system covariance +% R - the observation covariance +% y(:) - the observation at time t +% x(:) - E[X | y(:, 1:t-1)] prior mean +% V(:,:) - Cov[X | y(:, 1:t-1)] prior covariance +% +% OPTIONAL INPUTS (string/value pairs [default in brackets]) +% 'initial' - 1 means x and V are taken as initial conditions (so A and Q are ignored) [0] +% 'u' - u(:) the control signal at time t [ [] ] +% 'B' - the input regression matrix +% +% OUTPUTS (where X is the hidden state being estimated) +% xnew(:) = E[ X | y(:, 1:t) ] +% Vnew(:,:) = Var[ X(t) | y(:, 1:t) ] +% VVnew(:,:) = Cov[ X(t), X(t-1) | y(:, 1:t) ] +% loglik = log P(y(:,t) | y(:,1:t-1)) log-likelihood of innovatio + +% set default params +u = []; +B = []; +initial = 0; + +args = varargin; +for i=1:2:length(args) + switch args{i} + case 'u', u = args{i+1}; + case 'B', B = args{i+1}; + case 'initial', initial = args{i+1}; + otherwise, error(['unrecognized argument ' args{i}]) + end +end + +% xpred(:) = E[X_t+1 | y(:, 1:t)] +% Vpred(:,:) = Cov[X_t+1 | y(:, 1:t)] + +if initial + if isempty(u) + xpred = x; + else + xpred = x + B*u; + end + Vpred = V; +else + if isempty(u) + xpred = A*x; + else + xpred = A*x + B*u; + end + Vpred = A*V*A' + Q; +end + +e = y - C*xpred; % error (innovation) +n = length(e); +ss = length(A); +S = C*Vpred*C' + R; +Sinv = inv(S); +ss = length(V); +loglik = gaussian_prob(e, zeros(1,length(e)), S, 1); +K = Vpred*C'*Sinv; % Kalman gain matrix +% If there is no observation vector, set K = zeros(ss). +xnew = xpred + K*e; +Vnew = (eye(ss) - K*C)*Vpred; +VVnew = (eye(ss) - K*C)*A*V; + diff --git a/sourcecodes/bnt-master/Kalman/learn_AR.m b/sourcecodes/bnt-master/Kalman/learn_AR.m new file mode 100644 index 00000000..2d846c3a --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/learn_AR.m @@ -0,0 +1,30 @@ +function [coef, C] = learn_AR(data, k) +% Find the ML parameters of a vector autoregressive process of order k. +% [coef, C] = learn_AR(k, data) +% data{l}(:,t) = the observations at time t in sequence l + +warning('learn_AR seems to be broken'); + +nex = length(data); +obs = cell(1, nex); +for l=1:nex + obs{l} = convert_to_lagged_form(data{l}, k); +end + +% The initial parameter values don't matter, since this is a perfectly observable problem. +% However, the size of F must be set correctly. +y = data{1}; +[s T] = size(y); +coef = rand(s,s,k); +C = rand_psd(s); +[F,H,Q,R,initx,initV] = AR_to_SS(coef, C, y); + +max_iter = 1; +fully_observed = 1; +diagQ = 0; +diagR = 0; +[F, H, Q, R, initx, initV, loglik] = ... + learn_kalman(obs, F, H, Q, R, initx, initV, max_iter, diagQ, diagR, fully_observed); + +[coef, C] = SS_to_AR(F, Q, k); + diff --git a/sourcecodes/bnt-master/Kalman/learn_AR_diagonal.m b/sourcecodes/bnt-master/Kalman/learn_AR_diagonal.m new file mode 100644 index 00000000..8d31f217 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/learn_AR_diagonal.m @@ -0,0 +1,20 @@ +function [coef, C] = learn_AR_diagonal(y, k) +% Find the ML parameters for a collection of independent scalar AR processes. + +% sep_coef(1,1,t,i) is the coefficient to apply to compopnent i of the state vector t steps ago +% eg. consider two components L and R and let A = coef(:,:,1,:), B = coef(:,:,2,:) +% L3 (AL 0 BL 0) (L2) (CL 0 0 0) +% R3 = (0 AR 0 BR) (R2) (0 CR 0 0) +% L2 (1 0 0 0 ) (L1) + (0 0 0 0) +% R2 (0 1 0 0 ) (R1) (0 0 0 0) + +ss = size(y, 1); +sep_coef = zeros(1,1,k,ss); +for i=1:ss + [sep_coef(:,:,:,i), sep_cov(i)] = learn_AR(k, y(i,:)); +end +C = diag(sep_cov); +for t=1:k + x = sep_coef(1,1,t,:); + coef(:,:,t) = diag(x(:)); +end diff --git a/sourcecodes/bnt-master/Kalman/learn_kalman.m b/sourcecodes/bnt-master/Kalman/learn_kalman.m new file mode 100644 index 00000000..5924a48c --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/learn_kalman.m @@ -0,0 +1,182 @@ +function [A, C, Q, R, initx, initV, LL] = ... + learn_kalman(data, A, C, Q, R, initx, initV, max_iter, diagQ, diagR, ARmode, constr_fun, varargin) +% LEARN_KALMAN Find the ML parameters of a stochastic Linear Dynamical System using EM. +% +% [A, C, Q, R, INITX, INITV, LL] = LEARN_KALMAN(DATA, A0, C0, Q0, R0, INITX0, INITV0) fits +% the parameters which are defined as follows +% x(t+1) = A*x(t) + w(t), w ~ N(0, Q), x(0) ~ N(init_x, init_V) +% y(t) = C*x(t) + v(t), v ~ N(0, R) +% A0 is the initial value, A is the final value, etc. +% DATA(:,t,l) is the observation vector at time t for sequence l. If the sequences are of +% different lengths, you can pass in a cell array, so DATA{l} is an O*T matrix. +% LL is the "learning curve": a vector of the log lik. values at each iteration. +% LL might go positive, since prob. densities can exceed 1, although this probably +% indicates that something has gone wrong e.g., a variance has collapsed to 0. +% +% There are several optional arguments, that should be passed in the following order. +% LEARN_KALMAN(DATA, A0, C0, Q0, R0, INITX0, INITV0, MAX_ITER, DIAGQ, DIAGR, ARmode) +% MAX_ITER specifies the maximum number of EM iterations (default 10). +% DIAGQ=1 specifies that the Q matrix should be diagonal. (Default 0). +% DIAGR=1 specifies that the R matrix should also be diagonal. (Default 0). +% ARMODE=1 specifies that C=I, R=0. i.e., a Gauss-Markov process. (Default 0). +% This problem has a global MLE. Hence the initial parameter values are not important. +% +% LEARN_KALMAN(DATA, A0, C0, Q0, R0, INITX0, INITV0, MAX_ITER, DIAGQ, DIAGR, F, P1, P2, ...) +% calls [A,C,Q,R,initx,initV] = f(A,C,Q,R,initx,initV,P1,P2,...) after every M step. f can be +% used to enforce any constraints on the params. +% +% For details, see +% - Ghahramani and Hinton, "Parameter Estimation for LDS", U. Toronto tech. report, 1996 +% - Digalakis, Rohlicek and Ostendorf, "ML Estimation of a stochastic linear system with the EM +% algorithm and its application to speech recognition", +% IEEE Trans. Speech and Audio Proc., 1(4):431--442, 1993. + + +% learn_kalman(data, A, C, Q, R, initx, initV, max_iter, diagQ, diagR, ARmode, constr_fun, varargin) +if nargin < 8, max_iter = 10; end +if nargin < 9, diagQ = 0; end +if nargin < 10, diagR = 0; end +if nargin < 11, ARmode = 0; end +if nargin < 12, constr_fun = []; end +verbose = 1; +thresh = 1e-4; + + +if ~iscell(data) + N = size(data, 3); + data = num2cell(data, [1 2]); % each elt of the 3rd dim gets its own cell +else + N = length(data); +end + +N = length(data); +ss = size(A, 1); +os = size(C,1); + +alpha = zeros(os, os); +Tsum = 0; +for ex = 1:N + %y = data(:,:,ex); + y = data{ex}; + T = length(y); + Tsum = Tsum + T; + alpha_temp = zeros(os, os); + for t=1:T + alpha_temp = alpha_temp + y(:,t)*y(:,t)'; + end + alpha = alpha + alpha_temp; +end + +previous_loglik = -inf; +loglik = 0; +converged = 0; +num_iter = 1; +LL = []; + +% Convert to inline function as needed. +if ~isempty(constr_fun) + constr_fun = fcnchk(constr_fun,length(varargin)); +end + + +while ~converged & (num_iter <= max_iter) + + %%% E step + + delta = zeros(os, ss); + gamma = zeros(ss, ss); + gamma1 = zeros(ss, ss); + gamma2 = zeros(ss, ss); + beta = zeros(ss, ss); + P1sum = zeros(ss, ss); + x1sum = zeros(ss, 1); + loglik = 0; + + for ex = 1:N + y = data{ex}; + T = length(y); + [beta_t, gamma_t, delta_t, gamma1_t, gamma2_t, x1, V1, loglik_t] = ... + Estep(y, A, C, Q, R, initx, initV, ARmode); + beta = beta + beta_t; + gamma = gamma + gamma_t; + delta = delta + delta_t; + gamma1 = gamma1 + gamma1_t; + gamma2 = gamma2 + gamma2_t; + P1sum = P1sum + V1 + x1*x1'; + x1sum = x1sum + x1; + %fprintf(1, 'example %d, ll/T %5.3f\n', ex, loglik_t/T); + loglik = loglik + loglik_t; + end + LL = [LL loglik]; + if verbose, fprintf(1, 'iteration %d, loglik = %f\n', num_iter, loglik); end + %fprintf(1, 'iteration %d, loglik/NT = %f\n', num_iter, loglik/Tsum); + num_iter = num_iter + 1; + + %%% M step + + % Tsum = N*T + % Tsum1 = N*(T-1); + Tsum1 = Tsum - N; + A = beta * inv(gamma1); + %A = (gamma1' \ beta')'; + Q = (gamma2 - A*beta') / Tsum1; + if diagQ + Q = diag(diag(Q)); + end + if ~ARmode + C = delta * inv(gamma); + %C = (gamma' \ delta')'; + R = (alpha - C*delta') / Tsum; + if diagR + R = diag(diag(R)); + end + end + initx = x1sum / N; + initV = P1sum/N - initx*initx'; + + if ~isempty(constr_fun) + [A,C,Q,R,initx,initV] = feval(constr_fun, A, C, Q, R, initx, initV, varargin{:}); + end + + converged = em_converged(loglik, previous_loglik, thresh); + previous_loglik = loglik; +end + + + +%%%%%%%%% + +function [beta, gamma, delta, gamma1, gamma2, x1, V1, loglik] = ... + Estep(y, A, C, Q, R, initx, initV, ARmode) +% +% Compute the (expected) sufficient statistics for a single Kalman filter sequence. +% + +[os T] = size(y); +ss = length(A); + +if ARmode + xsmooth = y; + Vsmooth = zeros(ss, ss, T); % no uncertainty about the hidden states + VVsmooth = zeros(ss, ss, T); + loglik = 0; +else + [xsmooth, Vsmooth, VVsmooth, loglik] = kalman_smoother(y, A, C, Q, R, initx, initV); +end + +delta = zeros(os, ss); +gamma = zeros(ss, ss); +beta = zeros(ss, ss); +for t=1:T + delta = delta + y(:,t)*xsmooth(:,t)'; + gamma = gamma + xsmooth(:,t)*xsmooth(:,t)' + Vsmooth(:,:,t); + if t>1 beta = beta + xsmooth(:,t)*xsmooth(:,t-1)' + VVsmooth(:,:,t); end +end +gamma1 = gamma - xsmooth(:,T)*xsmooth(:,T)' - Vsmooth(:,:,T); +gamma2 = gamma - xsmooth(:,1)*xsmooth(:,1)' - Vsmooth(:,:,1); + +x1 = xsmooth(:,1); +V1 = Vsmooth(:,:,1); + + + diff --git a/sourcecodes/bnt-master/Kalman/learning_demo.m b/sourcecodes/bnt-master/Kalman/learning_demo.m new file mode 100644 index 00000000..f44716e7 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/learning_demo.m @@ -0,0 +1,36 @@ +% Make a point move in the 2D plane +% State = (x y xdot ydot). We only observe (x y). +% Generate data from this process, and try to learn the dynamics back. + +% X(t+1) = F X(t) + noise(Q) +% Y(t) = H X(t) + noise(R) + +ss = 4; % state size +os = 2; % observation size +F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1]; +H = [1 0 0 0; 0 1 0 0]; +Q = 0.1*eye(ss); +R = 1*eye(os); +initx = [10 10 1 0]'; +initV = 10*eye(ss); + +seed = 1; +rand('state', seed); +randn('state', seed); +T = 100; +[x,y] = sample_lds(F, H, Q, R, initx, T); + +% Initializing the params to sensible values is crucial. +% Here, we use the true values for everything except F and H, +% which we initialize randomly (bad idea!) +% Lack of identifiability means the learned params. are often far from the true ones. +% All that EM guarantees is that the likelihood will increase. +F1 = randn(ss,ss); +H1 = randn(os,ss); +Q1 = Q; +R1 = R; +initx1 = initx; +initV1 = initV; +max_iter = 10; +[F2, H2, Q2, R2, initx2, initV2, LL] = learn_kalman(y, F1, H1, Q1, R1, initx1, initV1, max_iter); + diff --git a/sourcecodes/bnt-master/Kalman/sample_lds.m b/sourcecodes/bnt-master/Kalman/sample_lds.m new file mode 100644 index 00000000..a5b1aff4 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/sample_lds.m @@ -0,0 +1,65 @@ +function [x,y] = sample_lds(F, H, Q, R, init_state, T, models, G, u) +% SAMPLE_LDS Simulate a run of a (switching) stochastic linear dynamical system. +% [x,y] = switching_lds_draw(F, H, Q, R, init_state, models, G, u) +% +% x(t+1) = F*x(t) + G*u(t) + w(t), w ~ N(0, Q), x(0) = init_state +% y(t) = H*x(t) + v(t), v ~ N(0, R) +% +% Input: +% F(:,:,i) - the transition matrix for the i'th model +% H(:,:,i) - the observation matrix for the i'th model +% Q(:,:,i) - the transition covariance for the i'th model +% R(:,:,i) - the observation covariance for the i'th model +% init_state(:,i) - the initial mean for the i'th model +% T - the num. time steps to run for +% +% Optional inputs: +% models(t) - which model to use at time t. Default = ones(1,T) +% G(:,:,i) - the input matrix for the i'th model. Default = 0. +% u(:,t) - the input vector at time t. Default = zeros(1,T) +% +% Output: +% x(:,t) - the hidden state vector at time t. +% y(:,t) - the observation vector at time t. + + +if ~iscell(F) + F = num2cell(F, [1 2]); + H = num2cell(H, [1 2]); + Q = num2cell(Q, [1 2]); + R = num2cell(R, [1 2]); +end + +M = length(F); +%T = length(models); + +if nargin < 7, + models = ones(1,T); +end +if nargin < 8, + G = num2cell(repmat(0, [1 1 M])); + u = zeros(1,T); +end + +[os ss] = size(H{1}); +state_noise_samples = cell(1,M); +obs_noise_samples = cell(1,M); +for i=1:M + state_noise_samples{i} = sample_gaussian(zeros(length(Q{i}),1), Q{i}, T)'; + obs_noise_samples{i} = sample_gaussian(zeros(length(R{i}),1), R{i}, T)'; +end + +x = zeros(ss, T); +y = zeros(os, T); + +m = models(1); +x(:,1) = init_state(:,m); +y(:,1) = H{m}*x(:,1) + obs_noise_samples{m}(:,1); + +for t=2:T + m = models(t); + x(:,t) = F{m}*x(:,t-1) + G{m}*u(:,t-1) + state_noise_samples{m}(:,t); + y(:,t) = H{m}*x(:,t) + obs_noise_samples{m}(:,t); +end + + diff --git a/sourcecodes/bnt-master/Kalman/smooth_update.m b/sourcecodes/bnt-master/Kalman/smooth_update.m new file mode 100644 index 00000000..bd29fe31 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/smooth_update.m @@ -0,0 +1,36 @@ +function [xsmooth, Vsmooth, VVsmooth_future] = smooth_update(xsmooth_future, Vsmooth_future, ... + xfilt, Vfilt, Vfilt_future, VVfilt_future, A, Q, B, u) +% One step of the backwards RTS smoothing equations. +% function [xsmooth, Vsmooth, VVsmooth_future] = smooth_update(xsmooth_future, Vsmooth_future, ... +% xfilt, Vfilt, Vfilt_future, VVfilt_future, A, B, u) +% +% INPUTS: +% xsmooth_future = E[X_t+1|T] +% Vsmooth_future = Cov[X_t+1|T] +% xfilt = E[X_t|t] +% Vfilt = Cov[X_t|t] +% Vfilt_future = Cov[X_t+1|t+1] +% VVfilt_future = Cov[X_t+1,X_t|t+1] +% A = system matrix for time t+1 +% Q = system covariance for time t+1 +% B = input matrix for time t+1 (or [] if none) +% u = input vector for time t+1 (or [] if none) +% +% OUTPUTS: +% xsmooth = E[X_t|T] +% Vsmooth = Cov[X_t|T] +% VVsmooth_future = Cov[X_t+1,X_t|T] + +%xpred = E[X(t+1) | t] +if isempty(B) + xpred = A*xfilt; +else + xpred = A*xfilt + B*u; +end +Vpred = A*Vfilt*A' + Q; % Vpred = Cov[X(t+1) | t] +J = Vfilt * A' * inv(Vpred); % smoother gain matrix +xsmooth = xfilt + J*(xsmooth_future - xpred); +Vsmooth = Vfilt + J*(Vsmooth_future - Vpred)*J'; +VVsmooth_future = VVfilt_future + (Vsmooth_future - Vfilt_future)*inv(Vfilt_future)*VVfilt_future; + + diff --git a/sourcecodes/bnt-master/Kalman/testKalman.m b/sourcecodes/bnt-master/Kalman/testKalman.m new file mode 100644 index 00000000..1cfda86c --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/testKalman.m @@ -0,0 +1,2 @@ +tracking_demo +learning_demo diff --git a/sourcecodes/bnt-master/Kalman/tracking_demo.m b/sourcecodes/bnt-master/Kalman/tracking_demo.m new file mode 100644 index 00000000..70cf6484 --- /dev/null +++ b/sourcecodes/bnt-master/Kalman/tracking_demo.m @@ -0,0 +1,74 @@ +% Make a point move in the 2D plane +% State = (x y xdot ydot). We only observe (x y). + +% This code was used to generate Figure 15.9 of "Artificial Intelligence: a Modern Approach", +% Russell and Norvig, 2nd edition, Prentice Hall, 2003. + +% X(t+1) = F X(t) + noise(Q) +% Y(t) = H X(t) + noise(R) + +ss = 4; % state size +os = 2; % observation size +F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1]; +H = [1 0 0 0; 0 1 0 0]; +Q = 0.1*eye(ss); +R = 1*eye(os); +initx = [10 10 1 0]'; +initV = 10*eye(ss); + +seed = 9; +rand('state', seed); +randn('state', seed); +T = 15; +[x,y] = sample_lds(F, H, Q, R, initx, T); + +[xfilt, Vfilt, VVfilt, loglik] = kalman_filter(y, F, H, Q, R, initx, initV); +[xsmooth, Vsmooth] = kalman_smoother(y, F, H, Q, R, initx, initV); + +dfilt = x([1 2],:) - xfilt([1 2],:); +mse_filt = sqrt(sum(sum(dfilt.^2))) + +dsmooth = x([1 2],:) - xsmooth([1 2],:); +mse_smooth = sqrt(sum(sum(dsmooth.^2))) + + +figure(1) +clf +%subplot(2,1,1) +hold on +plot(x(1,:), x(2,:), 'ks-'); +plot(y(1,:), y(2,:), 'g*'); +plot(xfilt(1,:), xfilt(2,:), 'rx:'); +for t=1:T, plotgauss2d(xfilt(1:2,t), Vfilt(1:2, 1:2, t)); end +hold off +legend('true', 'observed', 'filtered', 3) +xlabel('x') +ylabel('y') + + + +% 3x3 inches +set(gcf,'units','inches'); +set(gcf,'PaperPosition',[0 0 3 3]) +%print(gcf,'-depsc','/home/eecs/murphyk/public_html/Bayes/Figures/aima_filtered.eps'); +%print(gcf,'-djpeg','-r100', '/home/eecs/murphyk/public_html/Bayes/Figures/aima_filtered.jpg'); + + +figure(2) +%subplot(2,1,2) +hold on +plot(x(1,:), x(2,:), 'ks-'); +plot(y(1,:), y(2,:), 'g*'); +plot(xsmooth(1,:), xsmooth(2,:), 'rx:'); +for t=1:T, plotgauss2d(xsmooth(1:2,t), Vsmooth(1:2, 1:2, t)); end +hold off +legend('true', 'observed', 'smoothed', 3) +xlabel('x') +ylabel('y') + + +% 3x3 inches +set(gcf,'units','inches'); +set(gcf,'PaperPosition',[0 0 3 3]) +%print(gcf,'-djpeg','-r100', '/home/eecs/murphyk/public_html/Bayes/Figures/aima_smoothed.jpg'); +%print(gcf,'-depsc','/home/eecs/murphyk/public_html/Bayes/Figures/aima_smoothed.eps'); |