Home > src > demos > imm_demo > imm_demo.m

imm_demo

PURPOSE ^

IMM_DEMO Tracking a Target with Simple Manouvers demonstration

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

IMM_DEMO  Tracking a Target with Simple Manouvers demonstration
 
 Simple demonstration for linear IMM using the following models:
  1. Standard Wiener process velocity model
  2. Standard Wiener process acceleration model

 The measurement model is linear, which gives noisy measurements 
 of target's position.
 
 Copyright (C) 2007-2008 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 %IMM_DEMO  Tracking a Target with Simple Manouvers demonstration
0002 %
0003 % Simple demonstration for linear IMM using the following models:
0004 %  1. Standard Wiener process velocity model
0005 %  2. Standard Wiener process acceleration model
0006 %
0007 % The measurement model is linear, which gives noisy measurements
0008 % of target's position.
0009 %
0010 % Copyright (C) 2007-2008 Jouni Hartikainen
0011 %
0012 % This software is distributed under the GNU General Public
0013 % Licence (version 2 or later); please refer to the file
0014 % Licence.txt, included with the software, for details.
0015 
0016 
0017 % Simple demonstration for IMM using velocity and acceleration models
0018 
0019 % Save figures
0020 print_figures = 1;
0021 save_figures = 1;
0022 
0023 
0024 % Dimensionality of the state space
0025 dims = 6;
0026 
0027 nmodels = 2;
0028 
0029 % Space for models
0030 ind = cell(1,2);
0031 A = cell(1,2);
0032 Q = cell(1,2);
0033 H = cell(1,2);
0034 R = cell(1,2);
0035 
0036 % Stepsize
0037 dt = 0.1;
0038 
0039 ind{2} = [1 2 3 4 5 6]';
0040 
0041 % Transition matrix for the continous-time acceleration model.
0042 F2 = [0 0 1 0 0 0;
0043       0 0 0 1 0 0;
0044       0 0 0 0 1 0;
0045       0 0 0 0 0 1;
0046       0 0 0 0 0 0;
0047       0 0 0 0 0 0];
0048 
0049 % Noise effect matrix for the continous-time system.
0050 L2 = [0 0;
0051       0 0;
0052       0 0;
0053       0 0;
0054       1 0;
0055       0 1];
0056 
0057 % Process noise variance
0058 q2 = 1.00;
0059 Qc2 = diag([q2 q2]);
0060 
0061 % Discretization of the continous-time system.
0062 [A{2},Q{2}] = lti_disc(F2,L2,Qc2,dt);
0063 
0064 ind{1} = [1 2 3 4]';
0065 
0066 % Transition matrix for the continous-time velocity model.
0067 F1 = [0 0 1 0;
0068       0 0 0 1;
0069       0 0 0 0;
0070       0 0 0 0];
0071 
0072 % Noise effect matrix for the continous-time system.
0073 L1 = [0 0;
0074       0 0;
0075       1 0;
0076       0 1];
0077 
0078 % Process noise variance
0079 q1 = .01;
0080 Qc1 = diag([q1 q1]);
0081 
0082 % Discretization of the continous-time system.
0083 [A{1},Q{1}] = lti_disc(F1,L1,Qc1,dt);
0084 
0085 
0086 H{1} = [1 0 0 0;
0087         0 1 0 0];
0088 
0089 % Measurement models.
0090 H{2} = [1 0 0 0 0 0;
0091         0 1 0 0 0 0];
0092 
0093 hdims = 2;
0094 
0095 % Variance in the measurements.
0096 r1 = .1;
0097 r2 = .1;
0098 R{1} = diag([r1 r2]);
0099 
0100 r1 = .1;
0101 r2 = .1;
0102 R{2} = diag([r1 r2]);
0103 
0104 
0105 % Generate the data.
0106 n = 200;
0107 Y = zeros(hdims,n);
0108 X_r = zeros(dims,n);
0109 X_r(:,1) = [0 0 0 -1 0 0]';
0110 mstate = zeros(1,n);
0111 
0112 mu_ip = [0.9 0.1];
0113 mu_0j = mu_ip;
0114 %p_ij = [0.65 0.35;
0115 %        0.10 0.90];
0116 
0117 p_ij = [0.98 0.02;
0118         0.02 0.98];
0119 
0120 % $$$ % Generate random mode transitions
0121 % $$$ mstate(1) = 2;
0122 % $$$ for i = 2:n
0123 % $$$     r = rand;
0124 % $$$     for j = size(p_ij,1)-1;
0125 % $$$         if r < p_ij(mstate(i-1),j);
0126 % $$$             mstate(i) = j;
0127 % $$$         end
0128 % $$$     end
0129 % $$$     if mstate(i) == 0
0130 % $$$         mstate(i) = size(p_ij,1);
0131 % $$$     end
0132 % $$$ end
0133 
0134 % Forced mode transitions
0135 mstate(1:50) = 1;
0136 mstate(51:70) = 2;
0137 mstate(71:120) = 1;
0138 mstate(121:150) = 2;
0139 mstate(151:200) = 1;
0140 
0141 % Acceleration model
0142 for i = 2:n
0143     st = mstate(i);
0144     X_r(ind{st},i) = A{st}*X_r(ind{st},i-1) + gauss_rnd(zeros(size(A{st},1),1), Q{st});
0145 end
0146 
0147 % Generate the measurements.
0148 for i = 1:n
0149     Y(:,i) = H{mstate(i)}*X_r(ind{mstate(i)},i) + gauss_rnd(zeros(size(Y,1),1), R{mstate(i)});
0150 end
0151 
0152 ac = floor(n/2)+1:floor(n/2)+2;
0153 clf; clc;
0154 %fprintf('Filtering with KF...');
0155 
0156 % $$$ plot(X_r(1,:),X_r(2,:),Y(1,:),Y(2,:),'.',X_r(1,1),...
0157 % $$$      X_r(2,1),'ro','MarkerSize',12);
0158 % $$$ legend('Real trajectory', 'Measurements');
0159 % $$$ title('Position');
0160 
0161 m = [0 0 0 -1 0 0]';
0162 P = diag([0.1 0.1 0.1 0.1 0.5 0.5]);
0163 
0164 %%%% Space for the estimates %%%%
0165 
0166 % KF using model 1
0167 MM1 = zeros(size(A{1},1), size(Y,2));
0168 PP1 = zeros(size(A{1},1), size(A{1},1), size(Y,2));
0169 
0170 % KF using model 2
0171 MM2 = zeros(size(A{2},1), size(Y,2));
0172 PP2 = zeros(size(A{2},1), size(A{2},1), size(Y,2));
0173 
0174 % Overall estimates of IMM filter
0175 MM = zeros(size(m,1), size(Y,2));
0176 PP = zeros(size(m,1), size(m,1), size(Y,2));
0177 % Model-conditioned estimates of IMM
0178 MM_i = cell(2,n);
0179 PP_i = cell(2,n);
0180 
0181 % Model probabilities
0182 MU = zeros(2,size(Y,2));
0183 
0184 %%%% Prior estimates %%%%
0185 
0186 % KF1
0187 M1 = [0 0 0 -1]';
0188 P1 = diag([0.1 0.1 0.1 0.1]);
0189 
0190 % KF2
0191 M2 = [0 0 0 -1 0 0]';
0192 P2 = diag([0.1 0.1 0.1 0.1 0.5 0.5]);
0193 
0194 
0195 % IMM
0196 x_ip{1} = [0 0 0 -1]';
0197 P_ip{1} = diag([0.1 0.1 0.1 0.1]);
0198 x_ip{2} = [0 0 0 -1 0 0]';
0199 P_ip{2} = diag([0.1 0.1 0.1 0.1 0.5 0.5]);
0200 
0201 
0202 % Filtering steps.
0203 for i = 1:size(Y,2)
0204     % KF with model 1
0205     [M1,P1] = kf_predict(M1,P1,A{1},Q{1});
0206     [M1,P1] = kf_update(M1,P1,Y(:,i),H{1},R{1});
0207     MM1(:,i) = M1;
0208     PP1(:,:,i) = P1;
0209 
0210     % KF with model 2
0211     [M2,P2] = kf_predict(M2,P2,A{2},Q{2});
0212     [M2,P2] = kf_update(M2,P2,Y(:,i),H{2},R{2});
0213     MM2(:,i) = M2;
0214     PP2(:,:,i) = P2;
0215     
0216     % IMM
0217     [x_p,P_p,c_j] = imm_predict(x_ip,P_ip,mu_ip,p_ij,ind,dims,A,Q);
0218     [x_ip,P_ip,mu_ip,m,P] = imm_update(x_p,P_p,c_j,ind,dims,Y(:,i),H,R);
0219     MM(:,i)   = m;
0220     PP(:,:,i) = P;
0221     MU(:,i)   = mu_ip';
0222     MM_i(:,i) = x_ip';
0223     PP_i(:,i) = P_ip';
0224     
0225     % Plot the estimates
0226     if print_figures
0227         plot(X_r(1,1:i),X_r(2,1:i),'g-',...         
0228              MM(1,1:i),MM(2,1:i),'r-',...
0229              MM1(1,1:i),MM1(2,1:i),'y-',...
0230              MM2(1,1:i),MM2(2,1:i),'k-',...
0231              Y(1,1:i),Y(2,1:i),'ko');
0232         drawnow
0233     end
0234 end
0235 
0236 % Smooth the filtered estimates
0237 [SM1,SP1,SS1] = rts_smooth(MM1,PP1,A{1},Q{1});
0238 [SM2,SP2,SS2] = rts_smooth(MM2,PP2,A{2},Q{2});
0239 [SM3,SP3,SM3_i,SP3_i,MU_S] = imm_smooth(MM,PP,MM_i,PP_i,MU,p_ij,mu_0j,ind,dims,A,Q,R,H,Y);
0240 
0241 % Calculate the MSEs
0242 MSE_KF1_1 = mean((X_r(1,:)-MM1(1,:)).^2);
0243 MSE_KF1_2 = mean((X_r(2,:)-MM1(2,:)).^2);
0244 MSE_KF1 = 1/2*(MSE_KF1_1 + MSE_KF1_2);
0245 
0246 MSE_KS1_1 = mean((X_r(1,:)-SM1(1,:)).^2);
0247 MSE_KS1_2 = mean((X_r(2,:)-SM1(2,:)).^2);
0248 MSE_KS1 = 1/2*(MSE_KS1_1 + MSE_KS1_2);
0249 
0250 MSE_KF2_1 = mean((X_r(1,:)-MM2(1,:)).^2);
0251 MSE_KF2_2 = mean((X_r(2,:)-MM2(2,:)).^2);
0252 MSE_KF2 = 1/2*(MSE_KF2_1 + MSE_KF2_2);
0253 
0254 MSE_KS2_1 = mean((X_r(1,:)-SM2(1,:)).^2);
0255 MSE_KS2_2 = mean((X_r(2,:)-SM2(2,:)).^2);
0256 MSE_KS2 = 1/2*(MSE_KS2_1 + MSE_KS2_2);
0257 
0258 MSE_IMM1 = mean((X_r(1,:)-MM(1,:)).^2);
0259 MSE_IMM2 = mean((X_r(2,:)-MM(2,:)).^2);
0260 MSE_IMM = 1/2*(MSE_IMM1 + MSE_IMM2);
0261 
0262 MSE_IMMS1 = mean((X_r(1,:)-SM3(1,:)).^2);
0263 MSE_IMMS2 = mean((X_r(2,:)-SM3(2,:)).^2);
0264 MSE_IMMS = 1/2*(MSE_IMMS1 + MSE_IMMS2);
0265 
0266 fprintf('Mean square errors of position estimates:\n');
0267 fprintf('KF1-RMSE = %.4f\n',MSE_KF1);
0268 fprintf('KS1-RMSE = %.4f\n',MSE_KS1);
0269 fprintf('KF2-RMSE = %.4f\n',MSE_KF2);
0270 fprintf('KS2-RMSE = %.4f\n',MSE_KS2);
0271 fprintf('IMM-RMSE = %.4f\n',MSE_IMM);
0272 fprintf('IMMS-RMSE = %.4f\n',MSE_IMMS);
0273 
0274 % Plot the final filtering and smoothing results
0275 if print_figures
0276     h = plot(Y(1,:),Y(2,:),'ko',...
0277              X_r(1,:),X_r(2,:),'g-',...         
0278              MM1(1,:),MM1(2,:),'r-',...
0279              SM1(1,:),SM1(2,:),'b-');
0280     legend('Measurement',...
0281            'True trajectory',...
0282            'Filtered',...
0283            'Smoothed');
0284     title('Estimates produced by Kalman filter using the model 1.');
0285     set(h,'markersize',2);
0286     set(h,'linewidth',0.5);
0287     set(gca,'FontSize',6);
0288     if save_figures
0289         print('-depsc','imm1.eps');
0290     end
0291     
0292     h = plot(Y(1,:),Y(2,:),'ko',...
0293              X_r(1,:),X_r(2,:),'g-',...         
0294              MM2(1,:),MM2(2,:),'r-',...
0295              SM2(1,:),SM2(2,:),'b-');
0296     legend('Measurement',...
0297            'True trajectory',...
0298            'Filtered',...
0299            'Smoothed');
0300     title('Estimates produced by Kalman filter using the model 2.');
0301     set(h,'markersize',2);
0302     set(h,'linewidth',0.5);
0303     set(gca,'FontSize',6);
0304     if save_figures
0305         print('-depsc','imm2.eps');
0306     end
0307     
0308     h = plot(Y(1,:),Y(2,:),'ko',...
0309              X_r(1,:),X_r(2,:),'g-',...         
0310              MM(1,:),MM(2,:),'r-',...
0311              SM2(1,:),SM2(2,:),'b-');
0312     legend('Measurement',...
0313            'True trajectory',...
0314            'Filtered',...
0315            'Smoothed');
0316     title('Estimates produced by IMM-filter.')
0317     set(h,'markersize',2);
0318     set(h,'linewidth',0.5);
0319     set(gca,'FontSize',6);
0320     if save_figures
0321         print('-depsc','imm3.eps');
0322     end
0323     
0324     h = plot(1:n,2-mstate,'g--',...
0325              1:n,MU(1,:)','r-',...  
0326              1:n,MU_S(1,:)','b-');         
0327     legend('True',...
0328            'Filtered',...
0329            'Smoothed');
0330     title('Probability of model 1');
0331     ylim([-0.1,1.1]);
0332     set(h,'markersize',2);
0333     set(h,'linewidth',0.5);
0334     set(gca,'FontSize',6);
0335     if save_figures
0336         print('-depsc','imm4.eps');
0337     end
0338 end

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