wolffd@0: % KALMAN_FORWARD_BACKWARD		Forward Backward Propogation in Information Form 
wolffd@0: %
wolffd@0: %
wolffd@0: % Note	:
wolffd@0: %
wolffd@0: %  M file accompanying my technical note
wolffd@0: %
wolffd@0: %    A Technique for Painless Derivation of Kalman Filtering Recursions
wolffd@0: %
wolffd@0: % available from http://www.mbfys.kun.nl/~cemgil/papers/painless-kalman.ps
wolffd@0: % 
wolffd@0: 
wolffd@0: % Uses :
wolffd@0: 
wolffd@0: % Change History :
wolffd@0: % Date		Time		Prog	Note
wolffd@0: % 07-Jun-2001	 2:24 PM	ATC	Created under MATLAB 5.3.1.29215a (R11.1)
wolffd@0: 
wolffd@0: % ATC = Ali Taylan Cemgil,
wolffd@0: % SNN - University of Nijmegen, Department of Medical Physics and Biophysics
wolffd@0: % e-mail : cemgil@mbfys.kun.nl 
wolffd@0: 
wolffd@0: A = [1 1;0 1];
wolffd@0: C = [1 0];
wolffd@0: Q = eye(2)*0.01^2;
wolffd@0: R = 0.001^2;
wolffd@0: mu1 = [0;1];
wolffd@0: P1 = 3*Q;
wolffd@0: 
wolffd@0: inv_Q = inv(Q);
wolffd@0: inv_R = inv(R);
wolffd@0: 
wolffd@0: y = [0 1.1 2 2.95 3.78];
wolffd@0: 
wolffd@0: T = length(y);
wolffd@0: L = size(Q,1);
wolffd@0: 
wolffd@0: %%%%% Forward message Passing 
wolffd@0: h_f = zeros(L, T);
wolffd@0: K_f = zeros(L, L, T);
wolffd@0: g_f = zeros(1, T);
wolffd@0: h_f_pre = zeros(L, T);
wolffd@0: K_f_pre = zeros(L, L, T);
wolffd@0: g_f_pre = zeros(1, T);
wolffd@0: 
wolffd@0: 
wolffd@0: K_f_pre(:, :, 1) = inv(P1);
wolffd@0: h_f_pre(:,1) = K_f_pre(:, :, 1)*mu1;
wolffd@0: g_f_pre(1) = -0.5*log(det(2*pi*P1)) - 0.5*mu1'*inv(P1)*mu1;
wolffd@0: 
wolffd@0: for i=1:T,
wolffd@0:   h_f(:,i) = h_f_pre(:,i) + C'*inv_R*y(:,i);
wolffd@0:   K_f(:,:,i) = K_f_pre(:,:,i) + C'*inv_R*C;
wolffd@0:   g_f(i) = g_f_pre(i) -0.5*log(det(2*pi*R)) - 0.5*y(:,i)'*inv_R*y(:,i);
wolffd@0:   if i<T,
wolffd@0:     M = inv(A'*inv_Q*A + K_f(:,:,i));
wolffd@0:     h_f_pre(:,i+1) = inv_Q*A*M*h_f(:,i);
wolffd@0:     K_f_pre(:,:,i+1) = inv_Q - inv_Q*A*M*A'*inv_Q;
wolffd@0:     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);
wolffd@0:   end;
wolffd@0: end
wolffd@0: 
wolffd@0: %%% Backward Message Passing
wolffd@0: h_b = zeros(L, T);
wolffd@0: K_b = zeros(L, L, T);
wolffd@0: g_b = zeros(1, T);
wolffd@0: 
wolffd@0: h_b_post = zeros(L, T);
wolffd@0: K_b_post = zeros(L, L, T);
wolffd@0: g_b_post = zeros(1, T);
wolffd@0: 
wolffd@0: for i=T:-1:1,
wolffd@0:   h_b(:,i) = h_b_post(:,i) + C'*inv_R*y(:,i);
wolffd@0:   K_b(:,:,i) = K_b_post(:,:,i) + C'*inv_R*C;
wolffd@0:   g_b(i) = g_b_post(i) - 0.5*log(det(2*pi*R)) - 0.5*y(:,i)'*inv_R*y(:,i);
wolffd@0:   if i>1,
wolffd@0:     M = inv(inv_Q + K_b(:,:,i));
wolffd@0:     h_b_post(:,i-1) = A'*inv(Q)*M*h_b(:,i);
wolffd@0:     K_b_post(:,:,i-1) = A'*inv_Q*(Q - M)*inv_Q*A;
wolffd@0:     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);
wolffd@0:   end;
wolffd@0: end;
wolffd@0: 
wolffd@0: 
wolffd@0: %%%% Smoothed Estimates
wolffd@0: 
wolffd@0: mu = zeros(size(h_f));
wolffd@0: Sig = zeros(size(K_f));
wolffd@0: g = zeros(size(g_f));
wolffd@0: lalpha = zeros(size(g_f));
wolffd@0: 
wolffd@0: for i=1:T,
wolffd@0:   Sig(:,:,i) = inv(K_b_post(:,:,i) + K_f(:,:,i));
wolffd@0:   mu(:,i) = Sig(:,:,i)*(h_b_post(:,i) + h_f(:,i));
wolffd@0:   g(i) = g_b_post(i) + g_f(:,i);
wolffd@0:   lalpha(i) = g(i) + 0.5*log(det(2*pi*Sig(:,:,i))) + 0.5*mu(:,i)'*inv(Sig(:,:,i))*mu(:,i);
wolffd@0: end;