Kalman filter and smoother

chapter13/kalmanFilter.m

@@ -0,0 +1,39 @@
+function [mu, V, llh] = kalmanFilter(X, model)
+% DONE
+% Kalman filter 
+A = model.A; % transition matrix 
+G = model.G; % transition covariance
+C = model.C; % emission matrix
+S = model.S;  % emision covariance
+mu0 = model.mu; % prior mean
+P = model.P;  % prior covairance
+
+n = size(X,2);
+q = size(mu0,1);
+mu = zeros(q,n);
+V = zeros(q,q,n);
+llh = zeros(1,n);
+I = eye(q);
+
+PC = P*C';
+R = (C*PC+S);
+K = PC/R;
+mu(:,1) = mu0+K*(X(:,1)-C*mu0);
+V(:,:,1) = (I-K*C)*P;
+llh(1) = pdfGaussLn(X(:,1),C*mu0,R);
+for i = 2:n
+    [mu(:,i), V(:,:,i), llh(i)] = ...
+        forwardStep(X(:,i), mu(:,i-1), V(:,:,i-1), A, G, C, S, I);
+end
+llh = sum(llh);
+
+function [mu, V, llh] = forwardStep(x, mu, V, A, G, C, S, I)
+P = A*V*A'+G;
+PC = P*C';
+R = C*PC+S;
+K = PC/R;
+Amu = A*mu;
+CAmu = C*Amu;
+mu = Amu+K*(x-CAmu);
+V = (I-K*C)*P;
+llh = pdfGaussLn(x,CAmu,R);

chapter13/kalmanSmoother.m

@@ -0,0 +1,60 @@
+function [nu, U, UU, llh] = kalmanSmoother(X, model)
+% DONE
+% Kalman smoother
+A = model.A; % transition matrix 
+G = model.G; % transition covariance
+C = model.C; % emission matrix
+S = model.S;  % emision covariance
+mu0 = model.mu; % prior mean
+P0 = model.P;  % prior covairance
+
+n = size(X,2);
+q = size(mu0,1);
+mu = zeros(q,n);
+V = zeros(q,q,n);
+P = zeros(q,q,n); % C_{t+1|t}
+Amu = zeros(q,n); % u_{t+1|t}
+llh = zeros(1,n);
+I = eye(q);
+
+% forward
+PC = P0*C';
+R = (C*PC+S);
+K = PC/R;
+mu(:,1) = mu0+K*(X(:,1)-C*mu0);
+V(:,:,1) = (I-K*C)*P0;
+P(:,:,1) = P0;  % useless, just make a point
+Amu(:,1) = mu0; % useless, just make a point
+llh(1) = pdfGaussLn(X(:,1),C*mu0,R);
+for i = 2:n    
+    [mu(:,i), V(:,:,i), Amu(:,i), P(:,:,i), llh(i)] = ...
+        forwardStep(X(:,i), mu(:,i-1), V(:,:,i-1), A, G, C, S, I);
+end
+llh = sum(llh);
+% backward
+nu = zeros(q,n);
+U = zeros(q,q,n);
+UU = zeros(q,q,n-1);
+nu(:,n) = mu(:,n);
+U(:,:,n) = V(:,:,n);
+for i = n-1:-1:1  
+    [nu(:,i), U(:,:,i), UU(:,:,i)] = ...
+        backwardStep(nu(:,i+1), U(:,:,i+1), mu(:,i), V(:,:,i), Amu(:,i+1), P(:,:,i+1), A);
+end
+
+function [mu, V, Amu, P, llh] = forwardStep(x, mu, V, A, G, C, S, I)
+P = A*V*A'+G;
+PC = P*C';
+R = C*PC+S;
+K = PC/R;
+Amu = A*mu;
+CAmu = C*Amu;
+mu = Amu+K*(x-CAmu);
+V = (I-K*C)*P;
+llh = pdfGaussLn(x,CAmu,R);
+
+function [nu, U, UU] = backwardStep(nu, U, mu, V, Amu, P, A)
+J = V*A'/P;   % smoother gain matrix
+nu = mu+J*(nu-Amu);
+UU = J*U; % Bishop eqn 13.104
+U = V+J*(U-P)*J';

chapter13/pdfGaussLn.m

@@ -0,0 +1,36 @@
+function y = pdfGaussLn(X, mu, sigma)
+% Compute log pdf of a Gaussian distribution.
+% Written by Mo Chen ([email protected]).
+
+[d,n] = size(X);
+k = size(mu,2);
+if n == k && size(sigma,1) == 1           
+    X = bsxfun(@times,X-mu,1./sigma);
+    q = dot(X,X,1);  % M distance
+    c = d*log(2*pi)+2*log(sigma);          % normalization constant
+    y = -0.5*(c+q);
+elseif size(sigma,1)==d && size(sigma,2)==d && k==1   % one mu and one dxd sigma
+    X = bsxfun(@minus,X,mu);
+    [R,p]= chol(sigma);
+    if p ~= 0
+        error('ERROR: sigma is not PD.');
+    end
+    Q = R'\X;
+    q = dot(Q,Q,1);  % quadratic term (M distance)
+    c = d*log(2*pi)+2*sum(log(diag(R)));   % normalization constant
+    y = -0.5*(c+q);
+elseif size(sigma,1)==d && size(sigma,2)==k % k mu and k diagonal sigma
+    lambda = 1./sigma;
+    ml = mu.*lambda;
+    q = bsxfun(@plus,X'.^2*lambda-2*X'*ml,dot(mu,ml,1)); % M distance
+    c = d*log(2*pi)+2*sum(log(sigma),1); % normalization constant
+    y = -0.5*bsxfun(@plus,q,c);
+elseif size(sigma,1)==1 && (size(sigma,2)==k || size(sigma,2)==1) % k mu and (k or one) scalar sigma
+    X2 = repmat(dot(X,X,1)',1,k);
+    D = bsxfun(@plus,X2-2*X'*mu,dot(mu,mu,1));
+    q = bsxfun(@times,D,1./sigma);  % M distance
+    c = d*(log(2*pi)+2*log(sigma));          % normalization constant
+    y = -0.5*bsxfun(@plus,q,c);
+else
+    error('Parameters mismatched.');
+end