|
wolffd@0
|
1 % KALMAN_FORWARD_BACKWARD Forward Backward Propogation in Information Form
|
|
wolffd@0
|
2 %
|
|
wolffd@0
|
3 %
|
|
wolffd@0
|
4 % Note :
|
|
wolffd@0
|
5 %
|
|
wolffd@0
|
6 % M file accompanying my technical note
|
|
wolffd@0
|
7 %
|
|
wolffd@0
|
8 % A Technique for Painless Derivation of Kalman Filtering Recursions
|
|
wolffd@0
|
9 %
|
|
wolffd@0
|
10 % available from http://www.mbfys.kun.nl/~cemgil/papers/painless-kalman.ps
|
|
wolffd@0
|
11 %
|
|
wolffd@0
|
12
|
|
wolffd@0
|
13 % Uses :
|
|
wolffd@0
|
14
|
|
wolffd@0
|
15 % Change History :
|
|
wolffd@0
|
16 % Date Time Prog Note
|
|
wolffd@0
|
17 % 07-Jun-2001 2:24 PM ATC Created under MATLAB 5.3.1.29215a (R11.1)
|
|
wolffd@0
|
18
|
|
wolffd@0
|
19 % ATC = Ali Taylan Cemgil,
|
|
wolffd@0
|
20 % SNN - University of Nijmegen, Department of Medical Physics and Biophysics
|
|
wolffd@0
|
21 % e-mail : cemgil@mbfys.kun.nl
|
|
wolffd@0
|
22
|
|
wolffd@0
|
23 A = [1 1;0 1];
|
|
wolffd@0
|
24 C = [1 0];
|
|
wolffd@0
|
25 Q = eye(2)*0.01^2;
|
|
wolffd@0
|
26 R = 0.001^2;
|
|
wolffd@0
|
27 mu1 = [0;1];
|
|
wolffd@0
|
28 P1 = 3*Q;
|
|
wolffd@0
|
29
|
|
wolffd@0
|
30 inv_Q = inv(Q);
|
|
wolffd@0
|
31 inv_R = inv(R);
|
|
wolffd@0
|
32
|
|
wolffd@0
|
33 y = [0 1.1 2 2.95 3.78];
|
|
wolffd@0
|
34
|
|
wolffd@0
|
35 T = length(y);
|
|
wolffd@0
|
36 L = size(Q,1);
|
|
wolffd@0
|
37
|
|
wolffd@0
|
38 %%%%% Forward message Passing
|
|
wolffd@0
|
39 h_f = zeros(L, T);
|
|
wolffd@0
|
40 K_f = zeros(L, L, T);
|
|
wolffd@0
|
41 g_f = zeros(1, T);
|
|
wolffd@0
|
42 h_f_pre = zeros(L, T);
|
|
wolffd@0
|
43 K_f_pre = zeros(L, L, T);
|
|
wolffd@0
|
44 g_f_pre = zeros(1, T);
|
|
wolffd@0
|
45
|
|
wolffd@0
|
46
|
|
wolffd@0
|
47 K_f_pre(:, :, 1) = inv(P1);
|
|
wolffd@0
|
48 h_f_pre(:,1) = K_f_pre(:, :, 1)*mu1;
|
|
wolffd@0
|
49 g_f_pre(1) = -0.5*log(det(2*pi*P1)) - 0.5*mu1'*inv(P1)*mu1;
|
|
wolffd@0
|
50
|
|
wolffd@0
|
51 for i=1:T,
|
|
wolffd@0
|
52 h_f(:,i) = h_f_pre(:,i) + C'*inv_R*y(:,i);
|
|
wolffd@0
|
53 K_f(:,:,i) = K_f_pre(:,:,i) + C'*inv_R*C;
|
|
wolffd@0
|
54 g_f(i) = g_f_pre(i) -0.5*log(det(2*pi*R)) - 0.5*y(:,i)'*inv_R*y(:,i);
|
|
wolffd@0
|
55 if i<T,
|
|
wolffd@0
|
56 M = inv(A'*inv_Q*A + K_f(:,:,i));
|
|
wolffd@0
|
57 h_f_pre(:,i+1) = inv_Q*A*M*h_f(:,i);
|
|
wolffd@0
|
58 K_f_pre(:,:,i+1) = inv_Q - inv_Q*A*M*A'*inv_Q;
|
|
wolffd@0
|
59 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
|
60 end;
|
|
wolffd@0
|
61 end
|
|
wolffd@0
|
62
|
|
wolffd@0
|
63 %%% Backward Message Passing
|
|
wolffd@0
|
64 h_b = zeros(L, T);
|
|
wolffd@0
|
65 K_b = zeros(L, L, T);
|
|
wolffd@0
|
66 g_b = zeros(1, T);
|
|
wolffd@0
|
67
|
|
wolffd@0
|
68 h_b_post = zeros(L, T);
|
|
wolffd@0
|
69 K_b_post = zeros(L, L, T);
|
|
wolffd@0
|
70 g_b_post = zeros(1, T);
|
|
wolffd@0
|
71
|
|
wolffd@0
|
72 for i=T:-1:1,
|
|
wolffd@0
|
73 h_b(:,i) = h_b_post(:,i) + C'*inv_R*y(:,i);
|
|
wolffd@0
|
74 K_b(:,:,i) = K_b_post(:,:,i) + C'*inv_R*C;
|
|
wolffd@0
|
75 g_b(i) = g_b_post(i) - 0.5*log(det(2*pi*R)) - 0.5*y(:,i)'*inv_R*y(:,i);
|
|
wolffd@0
|
76 if i>1,
|
|
wolffd@0
|
77 M = inv(inv_Q + K_b(:,:,i));
|
|
wolffd@0
|
78 h_b_post(:,i-1) = A'*inv(Q)*M*h_b(:,i);
|
|
wolffd@0
|
79 K_b_post(:,:,i-1) = A'*inv_Q*(Q - M)*inv_Q*A;
|
|
wolffd@0
|
80 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
|
81 end;
|
|
wolffd@0
|
82 end;
|
|
wolffd@0
|
83
|
|
wolffd@0
|
84
|
|
wolffd@0
|
85 %%%% Smoothed Estimates
|
|
wolffd@0
|
86
|
|
wolffd@0
|
87 mu = zeros(size(h_f));
|
|
wolffd@0
|
88 Sig = zeros(size(K_f));
|
|
wolffd@0
|
89 g = zeros(size(g_f));
|
|
wolffd@0
|
90 lalpha = zeros(size(g_f));
|
|
wolffd@0
|
91
|
|
wolffd@0
|
92 for i=1:T,
|
|
wolffd@0
|
93 Sig(:,:,i) = inv(K_b_post(:,:,i) + K_f(:,:,i));
|
|
wolffd@0
|
94 mu(:,i) = Sig(:,:,i)*(h_b_post(:,i) + h_f(:,i));
|
|
wolffd@0
|
95 g(i) = g_b_post(i) + g_f(:,i);
|
|
wolffd@0
|
96 lalpha(i) = g(i) + 0.5*log(det(2*pi*Sig(:,:,i))) + 0.5*mu(:,i)'*inv(Sig(:,:,i))*mu(:,i);
|
|
wolffd@0
|
97 end; |
