Home > src > demos > kf_cwpa_demo > kf_cwpa_demo.m

kf_cwpa_demo

PURPOSE ^

Demonstration for Kalman filter and smoother using a 2D CWPA model

SYNOPSIS ^

function kf_cwpa_demo

DESCRIPTION ^

 Demonstration for Kalman filter and smoother using a 2D CWPA model

 Copyright (C) 2007 Jouni Hartikainen

 This software is distributed under the GNU General Public 
 Licence (version 2 or later); please refer to the file 
 Licence.txt, included with the software, for details.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 % Demonstration for Kalman filter and smoother using a 2D CWPA model
0002 %
0003 % Copyright (C) 2007 Jouni Hartikainen
0004 %
0005 % This software is distributed under the GNU General Public
0006 % Licence (version 2 or later); please refer to the file
0007 % Licence.txt, included with the software, for details.
0008 
0009 function kf_cwpa_demo
0010 
0011 % Transition matrix for the continous-time system.
0012 F = [0 0 1 0 0 0;
0013      0 0 0 1 0 0;
0014      0 0 0 0 1 0;
0015      0 0 0 0 0 1;
0016      0 0 0 0 0 0;
0017      0 0 0 0 0 0];
0018 
0019 % Noise effect matrix for the continous-time system.
0020 L = [0 0;
0021      0 0;
0022      0 0;
0023      0 0;
0024      1 0;
0025      0 1];
0026 
0027 % Stepsize
0028 dt = 0.5;
0029 
0030 % Process noise variance
0031 q = 0.2;
0032 Qc = diag([q q]);
0033 
0034 % Discretization of the continous-time system.
0035 [A,Q] = lti_disc(F,L,Qc,dt);
0036 
0037 % Measurement model.
0038 H = [1 0 0 0 0 0;
0039      0 1 0 0 0 0];
0040 
0041 % Variance in the measurements.
0042 r1 = 10;
0043 r2 = 5;
0044 R = diag([r1 r1]);
0045 
0046 % Generate the data.
0047 n = 50;
0048 Y = zeros(size(H,1),n);
0049 X_r = zeros(size(F,1),n);
0050 X_r(:,1) = [0 0 0 0 0 0]';
0051 for i = 2:n
0052    X_r(:,i) = A*X_r(:,i-1) + gauss_rnd(zeros(size(F,1),1), Q);
0053 end
0054 
0055 % Generate the measurements.
0056 for i = 1:n
0057    Y(:,i) = H*X_r(:,i) + gauss_rnd(zeros(size(Y,1),1), R);
0058 end
0059 
0060 clf; clc;
0061 disp('This is a demonstration program for the classical Kalman filter.');
0062 disp(' ');
0063 disp(['KF is used here to estimate the position of a moving object, whos ',...
0064      'dynamics follow the CWPA-model described in the documentation ',...
0065       'provided with the toolbox.']);
0066 disp(' ');
0067 disp(['We get noisy measurements from the objects position and velocity ',...
0068       'on discrete time steps. The real position of the object and the ',...
0069       'measurements made of them are displayed now. The blue line is the ',...
0070       'real path of the object and the green dots represents the ',...
0071       'measurements. The red circle represents the starting point ',...
0072       'of the object.']);
0073 
0074 disp(' ');
0075 fprintf('Filtering with KF...');
0076 
0077 plot(X_r(1,:),X_r(2,:),Y(1,:),Y(2,:),'.',X_r(1,1),...
0078      X_r(2,1),'ro','MarkerSize',12);
0079 legend('Real trajectory', 'Measurements');
0080 title('Position');
0081 
0082 % Uncomment if you want to save an image
0083 % print -dpsc demo1_f1.ps
0084 
0085 % Initial guesses for the state mean and covariance.
0086 m = [0 0 0 0 0 0]';
0087 P = diag([0.1 0.1 0.1 0.1 0.5 0.5]);
0088 
0089 %% Space for the estimates.
0090 MM = zeros(size(m,1), size(Y,2));
0091 PP = zeros(size(m,1), size(m,1), size(Y,2));
0092 
0093 % Filtering steps.
0094 for i = 1:size(Y,2)
0095    [m,P] = kf_predict(m,P,A,Q);
0096    [m,P] = kf_update(m,P,Y(:,i),H,R);
0097    MM(:,i) = m;
0098    PP(:,:,i) = P;
0099 end
0100 
0101 % Smoothing step.
0102 [SM,SP] = rts_smooth(MM,PP,A,Q);
0103 [SM2,SP2] = tf_smooth(MM,PP,Y,A,Q,H,R,1);
0104 
0105 fprintf('ready.\n');
0106 disp(' ');
0107 disp('<push any button to see the results>');
0108 pause
0109 
0110 subplot(1,2,1);
0111 plot(X_r(1,:), X_r(2,:),'--', MM(1,:), MM(2,:),X_r(1,1),X_r(2,1),...
0112      'o','MarkerSize',12)
0113 legend('Real trajectory', 'Filtered');
0114 title('Position estimation with Kalman filter.');
0115 xlabel('x');
0116 ylabel('y');
0117 
0118 subplot(1,2,2);
0119 plot(X_r(3,:), X_r(4,:),'--', MM(3,:), MM(4,:),X_r(3,1),...
0120      X_r(4,1),'ro','MarkerSize',12);
0121 legend('Real velocity', 'Filtered');
0122 title('Velocity estimation with Kalman filter.');
0123 xlabel('x^.');
0124 ylabel('y^.');
0125 
0126 % Uncomment if you want to save an image
0127 % print -dpsc demo1_f2.ps
0128 
0129 
0130 clc; 
0131 disp(['The filtering results are displayed now. In the left panel the ',...
0132       'estimated path is shown, and, for comparison, in the right panel ',...
0133       'the estimated velocity is shown.']);
0134 disp(' ')
0135 disp('<push any key to see the smoothing results of a RTS smoother>');
0136 
0137 pause
0138 subplot(1,2,1);
0139 plot(X_r(1,:), X_r(2,:),'--', SM(1,:), SM(2,:),X_r(1,1),...
0140      X_r(2,1),'ro','MarkerSize',12);
0141 legend('Real trajectory', 'Smoothed');
0142 title('Position estimation with RTS smoother.');
0143 xlabel('x');
0144 ylabel('y');
0145 
0146 subplot(1,2,2);
0147 plot(X_r(3,:), X_r(4,:),'--', SM(3,:), SM(4,:),X_r(3,1),...
0148      X_r(4,1),'ro','MarkerSize',12);
0149 legend('Real velocity', 'Smoothed');
0150 title('Velocity estimation with RTS smoother.');
0151 xlabel('x^.');
0152 ylabel('y^.');
0153 
0154 % Uncomment if you want to save an image
0155 % print -dpsc demo1_f3.ps
0156 
0157 clc; 
0158 disp(['The smoothing results are displayed now. In the left panel the ',...
0159       'estimated path is shown, and, for comparison, in the right panel ',...
0160       'the estimated velocity is shown.']);
0161 disp(' ')
0162 disp('<push any key to see the filtering results sequentially>');
0163 
0164 pause
0165 subplot(1,2,1);
0166 plot(X_r(1,:), X_r(2,:),'--', SM2(1,:), SM2(2,:),X_r(1,1),...
0167      X_r(2,1),'ro','MarkerSize',12);
0168 legend('Real trajectory', 'Smoothed');
0169 title('Position estimation with TF smoother.');
0170 xlabel('x');
0171 ylabel('y');
0172 
0173 subplot(1,2,2);
0174 plot(X_r(3,:), X_r(4,:),'--', SM2(3,:), SM2(4,:),X_r(3,1),...
0175      X_r(4,1),'ro','MarkerSize',12);
0176 legend('Real velocity', 'Smoothed');
0177 title('Velocity estimation with TF smoother.');
0178 xlabel('x^.');
0179 ylabel('y^.');
0180 
0181 % Track and animate the filtering result.
0182 clc
0183 disp(['Filtering result is now displayed sequentially on ',...
0184      'every time step.']);
0185 disp(' ');
0186 disp(['The observations are displayed as green dots and ',...
0187      'the real signal as solid blue line. The filtering ',...
0188      'result of previous steps is plotted as dashed black ',...
0189      'line. The covariance in each step is displayed as a ',...
0190      'green ellipse around the mean (black circle) on each ',...
0191      'step. The predicted position on next step is displayed ',...
0192      'as a red circle']);
0193 disp(' ');
0194 disp('<push any key to proceed to next step>');
0195 
0196 clf
0197 EST = [];
0198 for k=1:size(Y,2)
0199     M = MM(:,k);  
0200     P = PP(:,:,k);
0201     EST = [EST M];
0202     M_pred = kf_predict(M,P,A,Q);
0203     
0204     % Confidence ellipse
0205     tt = (0:0.01:1)*2*pi;
0206     cc = repmat(M(1:2),1,length(tt)) + ...
0207      2*chol(P(1:2,1:2))'*[cos(tt);sin(tt)];
0208 
0209     % Animate
0210     plot(X_r(1,:),X_r(2,:),'-',...
0211          Y(1,:), Y(2,:), '.',...
0212          M(1),M(2),'ko',...
0213          M_pred(1),M_pred(2),'ro',...
0214          EST(1,:),EST(2,:),'k--',...
0215      cc(1,:),cc(2,:),'g-');
0216     drawnow;
0217     pause;
0218 end
0219 
0220 % Smoothing result.
0221 clf; clc;
0222 disp(['Smoothing (using RTS smoother) result is now displayed ',...
0223      'sequentially on every time step in reverse order.']);
0224 disp(' ');
0225 disp(['The observations are displayed as green dots and the ',...
0226      'real signal as solid blue line. The smoothing result ',...
0227      'of previous steps is plotted as dashed black line.',...
0228      'The covariance in each step is displayed as a green',...
0229      'ellipse around the mean (black circle) on each step.']);
0230 disp(' ');
0231 disp('<push any key to proceed to next step>');
0232 
0233 EST = [];
0234 for k=size(Y,2):-1:1
0235     M = SM(:,k);  
0236     P = SP(:,:,k);
0237     EST = [EST M];
0238 
0239     % Confidence ellipse
0240     tt = (0:0.01:1)*2*pi;
0241     cc = repmat(M(1:2),1,length(tt)) + ...
0242      2*chol(P(1:2,1:2))'*[cos(tt);sin(tt)];
0243 
0244     % Animate
0245     len = 1.5;
0246     plot(X_r(1,:),X_r(2,:),'-',...
0247          Y(1,:),Y(2,:),'.',...         
0248          M(1),M(2),'ko',...
0249          EST(1,:),EST(2,:),'k--',...
0250      cc(1,:),cc(2,:),'g-');
0251     drawnow;
0252     pause;
0253 end
0254 
0255 % MSEs of position estimates
0256 MSE_KF1 = mean((X_r(1,:)-MM(1,:)).^2);
0257 MSE_KF2 = mean((X_r(2,:)-MM(2,:)).^2);
0258 MSE_KF = 1/2*(MSE_KF1 + MSE_KF2);
0259 
0260 MSE_RTS1 = mean((X_r(1,:)-SM(1,:)).^2);
0261 MSE_RTS2 = mean((X_r(2,:)-SM(2,:)).^2);
0262 MSE_RTS = 1/2*(MSE_RTS1 + MSE_RTS2);
0263 
0264 MSE_TF1 = mean((X_r(1,:)-SM2(1,:)).^2);
0265 MSE_TF2 = mean((X_r(2,:)-SM2(2,:)).^2);
0266 MSE_TF = 1/2*(MSE_TF1 + MSE_TF2);
0267 
0268 clc;
0269 fprintf('Mean square errors of position estimates:\n');
0270 fprintf('KF-RMSE = %.4f\n',MSE_KF);
0271 fprintf('RTS-RMSE = %.4f\n',MSE_RTS);
0272 fprintf('TF-RMSE = %.4f\n\n',MSE_TF);
0273 
0274

Generated on Fri 12-Aug-2011 15:15:16 by m2html © 2005