Home > src > demos > eimm_demo > ct_demo.m

ct_demo

PURPOSE ^

EIMM_DEMO1 Coordinated turn model demonstration

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

EIMM_DEMO1 Coordinated turn model demonstration 
 
 Simple demonstration for non-linear IMM using the following models:
  1. Standard Wiener process velocity model
  2. Coordinated turn 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 %EIMM_DEMO1 Coordinated turn model demonstration
0002 %
0003 % Simple demonstration for non-linear IMM using the following models:
0004 %  1. Standard Wiener process velocity model
0005 %  2. Coordinated turn 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 print_figures = 1;
0017 save_figures = 0;
0018 
0019 % Dimensionality of the state space
0020 dims = 5;
0021 
0022 % The number of models in use
0023 nmodels = 2;
0024 
0025 % Step size
0026 dt = 0.1;
0027 
0028 % Space for function handles and parameters
0029 a_func = {};
0030 ia_func = {};
0031 a_param = {};
0032 h_func = {};
0033 h_param = {};
0034 dh_dx_func = {};
0035 
0036 a_func{1} = [];
0037 a_func{2} = @f_turn;
0038 ia_func{1} = [];
0039 ia_func{2} = @f_turn_inv;
0040 a_param{1} = [];
0041 a_param{2} = {dt};
0042 
0043 
0044 % Space for model parameters
0045 ind = cell(1,nmodels);
0046 F   = cell(1,nmodels);
0047 L   = cell(1,nmodels);
0048 Qc  = cell(1,nmodels);
0049 A   = cell(1,nmodels);
0050 Q   = cell(1,nmodels);
0051 H   = cell(1,nmodels);
0052 R   = cell(1,nmodels);
0053 
0054 % Index vector of model 1
0055 ind{1} = [1 2 3 4]';
0056 
0057 % Transition matrix for the continous-time velocity model.
0058 F{1} = [0 0 1 0;
0059         0 0 0 1;
0060         0 0 0 0;
0061         0 0 0 0];
0062 
0063 % Noise effect matrix for the continous-time system.
0064 L{1} = [0 0;
0065         0 0;
0066         1 0;
0067         0 1];
0068 
0069 % Process noise variance
0070 q1 = 0.01;
0071 Qc{1} = diag([q1 q1]);
0072 
0073 % Discretization of the continous-time system.
0074 [A{1},Q{1}] = lti_disc(F{1},L{1},Qc{1},dt);
0075 
0076 % Process noise variance
0077 KF_q1 = .05;
0078 KF_Qc1 = diag([KF_q1 KF_q1]);
0079 
0080 % Discretization of the continous-time system.
0081 [KF_A1,KF_Q1] = lti_disc(F{1},L{1},KF_Qc1,dt);
0082 
0083 
0084 %%% Specification of the turning model
0085 
0086 % System components. 5th parameter is the turning rate
0087 ind{2} = [1 2 3 4 5]';
0088 
0089 % Derivative of the dynamic function
0090 A{2} = @f_turn_dx;
0091 % Process noise for the turning rate
0092 Qc{2} = 0.15;
0093 
0094 % Noise effect matrix
0095 L{2} = [0 0 0 0 1]';
0096 
0097 % Process noise covariance
0098 Q{2} = L{2}*Qc{2}*L{2}'*dt;
0099 
0100 % Check the derivatives
0101 %der_check(a_func{2}, A{2}, 1, [1 1 1 1 0]',{dt});
0102 
0103 
0104 % Measurement models.
0105 H{1} = [1 0 0 0;
0106         0 1 0 0];
0107 
0108 H{2} = [1 0 0 0 0;
0109         0 1 0 0 0];
0110 
0111 hdims = 2;
0112 
0113 %mu_ip = [0.90 0.05 0.05];
0114 mu_ip = [0.95 0.05];
0115 mu_0j = mu_ip;
0116 %p_ij = [0.65 0.35;
0117 %        0.10 0.90];
0118 
0119 p_ij = [0.90 0.10;
0120         0.10 0.90];
0121 
0122 % Number of data points
0123 n = 200;
0124 
0125 % Space for real states and modes
0126 X_r = zeros(dims,n);
0127 mstate = zeros(1,n);
0128 
0129 %%%%%%% Creation of trajectory %%%%%%%
0130 
0131 % Start with constant velocity 1 toward right
0132 mstate(1:40) = 1;
0133 X_r(:,1) = [0 0 1 0 0]';
0134 
0135 % At 4s make a turn left with rate 1
0136 mstate(41:90) = 2;
0137 X_r(5,40) = 1;
0138 
0139 % At 9s move straight for 2 seconds
0140 mstate(91:110) = 1;
0141 
0142 % At 11s commence another turn right with rate -1
0143 mstate(111:160) = 2;
0144 X_r(5,110) = -1;
0145 
0146 % At 16s move straight for 4 seconds
0147 mstate(161:200) = 1;
0148 
0149 % Generate object state values
0150 for i = 2:n
0151    st = mstate(i);
0152    if isstr(a_func{st}) | strcmp(class(a_func{st}),'function_handle')
0153        X_r(ind{st},i) = feval(a_func{st},X_r(ind{st},i-1),a_param{st});
0154    else 
0155        X_r(ind{st},i) = A{st}*X_r(ind{st},i-1);
0156    end
0157 end
0158 
0159 % Noise variances of the measurement models
0160 
0161 % Model 1
0162 r1 = .05;
0163 r2 = .05;
0164 R{1} = diag([r1 r2]);
0165 
0166 % Model 2
0167 r1 = .05;
0168 r2 = .05;
0169 R{2} = diag([r1 r2]);
0170 
0171 % Generate the measurements.
0172 Y = zeros(hdims,n);
0173 for i = 1:n
0174     Y(:,i) = H{mstate(i)}*X_r(ind{mstate(i)},i) + gauss_rnd(zeros(size(Y,1),1), R{mstate(i)});
0175 end
0176 
0177 ac = floor(n/2)+1:floor(n/2)+2;
0178 clf; clc;
0179 %fprintf('Filtering with KF...');
0180 
0181 if print_figures
0182     h = plot(X_r(1,:),X_r(2,:),'-g',...
0183              Y(1,:),Y(2,:),'.',... 
0184              X_r(1,1),X_r(2,1),'ro','MarkerSize',12);    
0185     legend('Real trajectory',...
0186            'Measurements',...
0187            'Starting position');
0188     set(h,'markersize',5);
0189     set(h,'linewidth',0.5);
0190     set(gca,'FontSize',8);
0191     xlim([-1 7.5]) 
0192     ylim([-3.5 3.5]) 
0193     %title('Trajectory of the object');
0194     if save_figures
0195         print('-dpsc','eimm1_trajectory.ps');
0196     end
0197     pause
0198 end
0199 
0200 
0201 m = [0 0 0 -1 0]';
0202 P = diag([10.1 10.1 1.1 1.1 1]);
0203 
0204 %% Space for the estimates.
0205 
0206 % KF with model 1
0207 KF_MM = zeros(size(A{1},1), size(Y,2));
0208 KF_PP = zeros(size(A{1},1), size(A{1},1), size(Y,2));
0209 
0210 % IMM
0211 IMM1_MM = zeros(size(m,1), size(Y,2));
0212 IMM1_PP = zeros(size(m,1), size(m,1), size(Y,2));
0213 IMM1_MM_i = cell(nmodels,n);
0214 IMM1_PP_i = cell(nmodels,n);
0215 IMM1_MU = zeros(nmodels,size(Y,2));
0216 
0217 %%% Initial estimates %%%
0218 
0219 % KF with model 1
0220 KF_M = [0 0 0 -1]';
0221 KF_P = diag([1.1 1.1 0.1 0.1]);
0222 
0223 % EKF based IMM
0224 x_ip1{1} = [0 0 1 0]';
0225 x_ip1{2} = [0 0 1 0 0]';
0226 mu_ip1 = mu_ip;
0227 
0228 P_ip1{1} = diag([0.1 0.1 0.1 0.1]);
0229 P_ip1{2} = diag([0.1 0.1 0.1 0.1 1]);
0230 
0231 % UKF based IMM
0232 x_ip2{1} = [0 0 1 0]';
0233 x_ip2{2} = [0 0 1 0 0]';
0234 mu_ip2 = mu_ip;
0235 
0236 P_ip2{1} = diag([0.1 0.1 0.1 0.1]);
0237 P_ip2{2} = diag([0.1 0.1 0.1 0.1 1]);
0238 
0239 
0240 % Filtering steps.
0241 for i = 1:size(Y,2)
0242     % KF with model 1
0243     [KF_M,KF_P] = kf_predict(KF_M,KF_P,KF_A1,KF_Q1);
0244     [KF_M,KF_P] = kf_update(KF_M,KF_P,Y(:,i),H{1},R{1});
0245     KF_MM(:,i)   = KF_M;
0246     KF_PP(:,:,i) = KF_P;
0247     
0248     % EKF based IMM
0249     [x_p1,P_p1,c_j1] = eimm_predict(x_ip1,P_ip1,mu_ip1,p_ij,ind,dims,A,a_func,a_param,Q);
0250     [x_ip1,P_ip1,mu_ip1,m1,P1] = imm_update(x_p1,P_p1,c_j1,ind,dims,Y(:,i),H,R);
0251 
0252     EIMM_MM(:,i)   = m1;
0253     EIMM_PP(:,:,i) = P1;
0254     EIMM_MU(:,i)   = mu_ip1';
0255     EIMM_MM_i(:,i) = x_ip1';
0256     EIMM_PP_i(:,i) = P_ip1';
0257     
0258     % UKF based IMM
0259     [x_p2,P_p2,c_j2] = uimm_predict(x_ip2,P_ip2,mu_ip2,p_ij,ind,dims,A,a_func,a_param,Q);    
0260     [x_ip2,P_ip2,mu_ip2,m2,P2] = imm_update(x_p2,P_p2,c_j2,ind,dims,Y(:,i),H,R);
0261 
0262     UIMM_MM(:,i)   = m2;
0263     UIMM_PP(:,:,i) = P2;
0264     UIMM_MU(:,i)   = mu_ip2';
0265     UIMM_MM_i(:,i) = x_ip2';
0266     UIMM_PP_i(:,i) = P_ip2';
0267 
0268     if print_figures
0269         % Plot the estimates obtained so far
0270         plot(Y(1,1:i),Y(2,1:i),'k.',...
0271              KF_MM(1,1:i),KF_MM(2,1:i),'y-',...
0272              EIMM_MM(1,1:i),EIMM_MM(2,1:i),'r-',...
0273              UIMM_MM(1,1:i),UIMM_MM(2,1:i),'b-',...         
0274              X_r(1,1:i),X_r(2,1:i),'g-');
0275         xlim([-1 7.5]) 
0276         ylim([-3.5 3.5]) 
0277         
0278         drawnow
0279     end
0280 end
0281 
0282 % Smooth with EKF based IMM
0283 [SMI_1,SPI_1,SMI_i_1,SPI_i_1,MU_S_1] = eimm_smooth(EIMM_MM,EIMM_PP,EIMM_MM_i,EIMM_PP_i,EIMM_MU,p_ij,mu_0j,ind,dims,A,ia_func,a_param,Q,R,H,[],[],Y);
0284 
0285 % Smooth with UKF based IMM
0286 [SMI_2,SPI_2,SMI_i_2,SPI_i_2,MU_S_2] = uimm_smooth(UIMM_MM,UIMM_PP,UIMM_MM_i,UIMM_PP_i,UIMM_MU,p_ij,mu_0j,ind,dims,A,ia_func,a_param,Q,R,H,[],[],Y);
0287 
0288 % Smooth with KF
0289 [SM1,SP1] = erts_smooth1(KF_MM,KF_PP,KF_A1,KF_Q1);
0290 
0291 % Calculate the MSEs
0292 MSE_KF1_1 = mean((X_r(1,:)-KF_MM(1,:)).^2);
0293 MSE_KF1_2 = mean((X_r(2,:)-KF_MM(2,:)).^2);
0294 MSE_KF1 = 1/2*(MSE_KF1_1 + MSE_KF1_2);
0295 
0296 MSE_KS1_1 = mean((X_r(1,:)-SM1(1,:)).^2);
0297 MSE_KS1_2 = mean((X_r(2,:)-SM1(2,:)).^2);
0298 MSE_KS1 = 1/2*(MSE_KS1_1 + MSE_KS1_2);
0299 
0300 MSE_EIMM1 = mean((X_r(1,:)-EIMM_MM(1,:)).^2);
0301 MSE_EIMM2 = mean((X_r(2,:)-EIMM_MM(2,:)).^2);
0302 MSE_EIMM = 1/2*(MSE_EIMM1 + MSE_EIMM2);
0303 
0304 MSE_EIMMS1 = mean((X_r(1,:)-SMI_1(1,:)).^2);
0305 MSE_EIMMS2 = mean((X_r(2,:)-SMI_1(2,:)).^2);
0306 MSE_EIMMS = 1/2*(MSE_EIMMS1 + MSE_EIMMS2);
0307 
0308 MSE_UIMM1 = mean((X_r(1,:)-UIMM_MM(1,:)).^2);
0309 MSE_UIMM2 = mean((X_r(2,:)-UIMM_MM(2,:)).^2);
0310 MSE_UIMM = 1/2*(MSE_UIMM1 + MSE_UIMM2);
0311 
0312 MSE_UIMMS1 = mean((X_r(1,:)-SMI_2(1,:)).^2);
0313 MSE_UIMMS2 = mean((X_r(2,:)-SMI_2(2,:)).^2);
0314 MSE_UIMMS = 1/2*(MSE_UIMMS1 + MSE_UIMMS2);
0315 
0316 
0317 fprintf('Mean square errors of position estimates:\n');
0318 fprintf('KF1-MSE = %.4f\n',MSE_KF1);
0319 fprintf('KS1-MSE = %.4f\n',MSE_KS1);
0320 fprintf('EIMM-MSE = %.4f\n',MSE_EIMM);
0321 fprintf('EIMMS-MSE = %.4f\n',MSE_EIMMS);
0322 fprintf('UIMM-MSE = %.4f\n',MSE_UIMM);
0323 fprintf('UIMMS-MSE = %.4f\n',MSE_UIMMS);
0324 
0325 
0326 % Plot the final filtering and smoothing results
0327 if print_figures
0328     h = plot(X_r(1,:),X_r(2,:),'g-',...         
0329              KF_MM(1,:),KF_MM(2,:),'-k',...
0330              SM1(1,:),SM1(2,:),'--k',...
0331              EIMM_MM(1,:),EIMM_MM(2,:),'-r',...             
0332              SMI_1(1,:),SMI_1(2,:),'--r',...
0333              UIMM_MM(1,:),UIMM_MM(2,:),'-b',...
0334              SMI_2(1,:),SMI_2(2,:),'--b');
0335     legend('True trajectory',...
0336            'KF',...
0337            'RTS',...
0338            'IMM-EKF',...
0339            'IMM-EKS',...
0340            'IMM-UKF',...
0341            'IMM-UKS');
0342     %title('Estimates produced by Kalman filter using the model 1.');
0343     set(h,'markersize',2);
0344     set(h,'linewidth',0.5);
0345     set(gca,'FontSize',8);
0346     xlim([-1 7.5]) 
0347     ylim([-3.5 3.5]) 
0348     if save_figures
0349         print('-dpsc','eimm1_1.ps');
0350     end
0351     pause
0352     
0353     % Determine the real model probabilities
0354     p_models = zeros(nmodels,n);
0355     I1 = find(mstate == 1);
0356     p_models(1,I1) = 1;
0357     I2 = find(mstate == 2);
0358     p_models(2,I2) = 1;
0359 
0360     % Plot model 2 probability for each step
0361     h = plot(1:n, p_models(2,:),'g--',...
0362              1:n,EIMM_MU(2,:)','-r',...
0363              1:n,MU_S_1(2,:)','--r',...
0364              1:n,UIMM_MU(2,:)','-b',...
0365              1:n,MU_S_2(2,:)','--b');
0366     %1:n,MU_S_2(2,:)','b-');
0367     legend('True',...
0368            'IMM-EKF',...
0369            'IMM-EKS',...
0370            'IMM-UKF',...
0371            'IMM-UKS');
0372     %title('Probability of model 2');
0373     ylim([-0.1,1.1]);
0374     set(h,'markersize',2);
0375     set(h,'linewidth',0.5);
0376     set(gca,'FontSize',8);
0377     if save_figures
0378         print('-dpsc','eimm1_2.ps');
0379     end
0380     pause
0381 
0382     % Collect the turn rate estimates
0383     EMM_t = zeros(5,n);
0384     EMM_s = zeros(5,n);
0385     UMM_t = zeros(5,n);
0386     UMM_s = zeros(5,n);
0387 
0388     for i = 1:n
0389         EMM_t(:,i) = EIMM_MM_i{2,i};
0390         EMM_s(:,i) = SMI_i_1{2,i};
0391         UMM_t(:,i) = UIMM_MM_i{2,i};
0392         UMM_s(:,i) = SMI_i_2{2,i};
0393     end
0394     
0395     % Plot the filtered turn rates
0396     h = plot(1:n, X_r(5,:),'-g',...
0397              1:n,EMM_t(5,:),'-r',...     
0398              1:n,EMM_s(5,:),'--r',...     
0399              1:n,UMM_t(5,:),'-b',...             
0400              1:n,UMM_s(5,:),'--b');
0401     %title('Turn rate estimates')
0402     legend('True',...
0403            'IMM-EKF',...
0404            'IMM-EKS',...
0405            'IMM-UKF',...
0406            'IMM-UKS');
0407     set(h,'markersize',2);
0408     set(h,'linewidth',0.5);
0409     set(gca,'FontSize',8);    
0410     if save_figures
0411         print('-dpsc','eimm1_3.ps');
0412     end
0413 
0414 end
0415

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