wolffd@0: % Make a point move in the 2D plane
wolffd@0: % State = (x y xdot ydot). We only observe (x y).
wolffd@0: 
wolffd@0: % This code was used to generate Figure 15.9 of "Artificial Intelligence: a Modern Approach",
wolffd@0: % Russell and Norvig, 2nd edition, Prentice Hall, 2003.
wolffd@0: 
wolffd@0: % X(t+1) = F X(t) + noise(Q)
wolffd@0: % Y(t) = H X(t) + noise(R)
wolffd@0: 
wolffd@0: ss = 4; % state size
wolffd@0: os = 2; % observation size
wolffd@0: F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1]; 
wolffd@0: H = [1 0 0 0; 0 1 0 0];
wolffd@0: Q = 0.1*eye(ss);
wolffd@0: R = 1*eye(os);
wolffd@0: initx = [10 10 1 0]';
wolffd@0: initV = 10*eye(ss);
wolffd@0: 
wolffd@0: seed = 9;
wolffd@0: rand('state', seed);
wolffd@0: randn('state', seed);
wolffd@0: T = 15;
wolffd@0: [x,y] = sample_lds(F, H, Q, R, initx, T);
wolffd@0: 
wolffd@0: [xfilt, Vfilt, VVfilt, loglik] = kalman_filter(y, F, H, Q, R, initx, initV);
wolffd@0: [xsmooth, Vsmooth] = kalman_smoother(y, F, H, Q, R, initx, initV);
wolffd@0: 
wolffd@0: dfilt = x([1 2],:) - xfilt([1 2],:);
wolffd@0: mse_filt = sqrt(sum(sum(dfilt.^2)))
wolffd@0: 
wolffd@0: dsmooth = x([1 2],:) - xsmooth([1 2],:);
wolffd@0: mse_smooth = sqrt(sum(sum(dsmooth.^2)))
wolffd@0: 
wolffd@0: 
wolffd@0: figure(1)
wolffd@0: clf
wolffd@0: %subplot(2,1,1)
wolffd@0: hold on
wolffd@0: plot(x(1,:), x(2,:), 'ks-');
wolffd@0: plot(y(1,:), y(2,:), 'g*');
wolffd@0: plot(xfilt(1,:), xfilt(2,:), 'rx:');
wolffd@0: for t=1:T, plotgauss2d(xfilt(1:2,t), Vfilt(1:2, 1:2, t)); end
wolffd@0: hold off
wolffd@0: legend('true', 'observed', 'filtered', 3)
wolffd@0: xlabel('x')
wolffd@0: ylabel('y')
wolffd@0: 
wolffd@0: 
wolffd@0: 
wolffd@0: % 3x3 inches
wolffd@0: set(gcf,'units','inches');
wolffd@0: set(gcf,'PaperPosition',[0 0 3 3])  
wolffd@0: %print(gcf,'-depsc','/home/eecs/murphyk/public_html/Bayes/Figures/aima_filtered.eps');
wolffd@0: %print(gcf,'-djpeg','-r100', '/home/eecs/murphyk/public_html/Bayes/Figures/aima_filtered.jpg');
wolffd@0: 
wolffd@0: 
wolffd@0: figure(2)
wolffd@0: %subplot(2,1,2)
wolffd@0: hold on
wolffd@0: plot(x(1,:), x(2,:), 'ks-');
wolffd@0: plot(y(1,:), y(2,:), 'g*');
wolffd@0: plot(xsmooth(1,:), xsmooth(2,:), 'rx:');
wolffd@0: for t=1:T, plotgauss2d(xsmooth(1:2,t), Vsmooth(1:2, 1:2, t)); end
wolffd@0: hold off
wolffd@0: legend('true', 'observed', 'smoothed', 3)
wolffd@0: xlabel('x')
wolffd@0: ylabel('y')
wolffd@0: 
wolffd@0: 
wolffd@0: % 3x3 inches
wolffd@0: set(gcf,'units','inches');
wolffd@0: set(gcf,'PaperPosition',[0 0 3 3])  
wolffd@0: %print(gcf,'-djpeg','-r100', '/home/eecs/murphyk/public_html/Bayes/Figures/aima_smoothed.jpg');
wolffd@0: %print(gcf,'-depsc','/home/eecs/murphyk/public_html/Bayes/Figures/aima_smoothed.eps');