|
wolffd@0
|
1 % Make a point move in the 2D plane
|
|
wolffd@0
|
2 % State = (x y xdot ydot). We only observe (x y).
|
|
wolffd@0
|
3
|
|
wolffd@0
|
4 % This code was used to generate Figure 15.9 of "Artificial Intelligence: a Modern Approach",
|
|
wolffd@0
|
5 % Russell and Norvig, 2nd edition, Prentice Hall, 2003.
|
|
wolffd@0
|
6
|
|
wolffd@0
|
7 % X(t+1) = F X(t) + noise(Q)
|
|
wolffd@0
|
8 % Y(t) = H X(t) + noise(R)
|
|
wolffd@0
|
9
|
|
wolffd@0
|
10 ss = 4; % state size
|
|
wolffd@0
|
11 os = 2; % observation size
|
|
wolffd@0
|
12 F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];
|
|
wolffd@0
|
13 H = [1 0 0 0; 0 1 0 0];
|
|
wolffd@0
|
14 Q = 0.1*eye(ss);
|
|
wolffd@0
|
15 R = 1*eye(os);
|
|
wolffd@0
|
16 initx = [10 10 1 0]';
|
|
wolffd@0
|
17 initV = 10*eye(ss);
|
|
wolffd@0
|
18
|
|
wolffd@0
|
19 seed = 9;
|
|
wolffd@0
|
20 rand('state', seed);
|
|
wolffd@0
|
21 randn('state', seed);
|
|
wolffd@0
|
22 T = 15;
|
|
wolffd@0
|
23 [x,y] = sample_lds(F, H, Q, R, initx, T);
|
|
wolffd@0
|
24
|
|
wolffd@0
|
25 [xfilt, Vfilt, VVfilt, loglik] = kalman_filter(y, F, H, Q, R, initx, initV);
|
|
wolffd@0
|
26 [xsmooth, Vsmooth] = kalman_smoother(y, F, H, Q, R, initx, initV);
|
|
wolffd@0
|
27
|
|
wolffd@0
|
28 dfilt = x([1 2],:) - xfilt([1 2],:);
|
|
wolffd@0
|
29 mse_filt = sqrt(sum(sum(dfilt.^2)))
|
|
wolffd@0
|
30
|
|
wolffd@0
|
31 dsmooth = x([1 2],:) - xsmooth([1 2],:);
|
|
wolffd@0
|
32 mse_smooth = sqrt(sum(sum(dsmooth.^2)))
|
|
wolffd@0
|
33
|
|
wolffd@0
|
34
|
|
wolffd@0
|
35 figure(1)
|
|
wolffd@0
|
36 clf
|
|
wolffd@0
|
37 %subplot(2,1,1)
|
|
wolffd@0
|
38 hold on
|
|
wolffd@0
|
39 plot(x(1,:), x(2,:), 'ks-');
|
|
wolffd@0
|
40 plot(y(1,:), y(2,:), 'g*');
|
|
wolffd@0
|
41 plot(xfilt(1,:), xfilt(2,:), 'rx:');
|
|
wolffd@0
|
42 for t=1:T, plotgauss2d(xfilt(1:2,t), Vfilt(1:2, 1:2, t)); end
|
|
wolffd@0
|
43 hold off
|
|
wolffd@0
|
44 legend('true', 'observed', 'filtered', 3)
|
|
wolffd@0
|
45 xlabel('x')
|
|
wolffd@0
|
46 ylabel('y')
|
|
wolffd@0
|
47
|
|
wolffd@0
|
48
|
|
wolffd@0
|
49
|
|
wolffd@0
|
50 % 3x3 inches
|
|
wolffd@0
|
51 set(gcf,'units','inches');
|
|
wolffd@0
|
52 set(gcf,'PaperPosition',[0 0 3 3])
|
|
wolffd@0
|
53 %print(gcf,'-depsc','/home/eecs/murphyk/public_html/Bayes/Figures/aima_filtered.eps');
|
|
wolffd@0
|
54 %print(gcf,'-djpeg','-r100', '/home/eecs/murphyk/public_html/Bayes/Figures/aima_filtered.jpg');
|
|
wolffd@0
|
55
|
|
wolffd@0
|
56
|
|
wolffd@0
|
57 figure(2)
|
|
wolffd@0
|
58 %subplot(2,1,2)
|
|
wolffd@0
|
59 hold on
|
|
wolffd@0
|
60 plot(x(1,:), x(2,:), 'ks-');
|
|
wolffd@0
|
61 plot(y(1,:), y(2,:), 'g*');
|
|
wolffd@0
|
62 plot(xsmooth(1,:), xsmooth(2,:), 'rx:');
|
|
wolffd@0
|
63 for t=1:T, plotgauss2d(xsmooth(1:2,t), Vsmooth(1:2, 1:2, t)); end
|
|
wolffd@0
|
64 hold off
|
|
wolffd@0
|
65 legend('true', 'observed', 'smoothed', 3)
|
|
wolffd@0
|
66 xlabel('x')
|
|
wolffd@0
|
67 ylabel('y')
|
|
wolffd@0
|
68
|
|
wolffd@0
|
69
|
|
wolffd@0
|
70 % 3x3 inches
|
|
wolffd@0
|
71 set(gcf,'units','inches');
|
|
wolffd@0
|
72 set(gcf,'PaperPosition',[0 0 3 3])
|
|
wolffd@0
|
73 %print(gcf,'-djpeg','-r100', '/home/eecs/murphyk/public_html/Bayes/Figures/aima_smoothed.jpg');
|
|
wolffd@0
|
74 %print(gcf,'-depsc','/home/eecs/murphyk/public_html/Bayes/Figures/aima_smoothed.eps');
|
