Home > src > demos > eimm_demo > botm_demo.m

botm_demo

PURPOSE ^

EIMM_DEMO2 Bearings Only Tracking of a Manouvering Target demonstration

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

EIMM_DEMO2 Bearings Only Tracking of a Manouvering Target 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 non-linear bearings only model.
 See BOT-demo or the documentation for more details.
 
 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_DEMO2 Bearings Only Tracking of a Manouvering Target 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 non-linear bearings only model.
0008 % See BOT-demo or the documentation for more details.
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 = 1;
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 for model 1 using EKF
0077 EKF_q1 = .05;
0078 EKF_Qc1 = diag([EKF_q1 EKF_q1]);
0079 
0080 % Discretization of the continous-time system.
0081 [EKF_A1,EKF_Q1] = lti_disc(F{1},L{1},EKF_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 hdims = 2;
0101 
0102 %mu_ip = [0.90 0.05 0.05];
0103 mu_ip = [0.95 0.05];
0104 mu_0j = mu_ip;
0105 %p_ij = [0.65 0.35;
0106 %        0.10 0.90];
0107 
0108 p_ij = [0.90 0.10;
0109         0.10 0.90];
0110 
0111 % Number of data points
0112 n = 200;
0113 
0114 % Space for real states and modes
0115 X_r = zeros(dims,n);
0116 mstate = zeros(1,n);
0117 
0118 %%%%%%% Creation of trajectory %%%%%%%
0119 
0120 % Start with constant velocity 1 toward right
0121 mstate(1:40) = 1;
0122 X_r(:,1) = [0 0 1 0 0]';
0123 
0124 % At 4s make a turn left with rate 1
0125 mstate(41:90) = 2;
0126 X_r(5,40) = 1;
0127 
0128 % At 9s move straight for 2 seconds
0129 mstate(91:110) = 1;
0130 
0131 % At 11s commence another turn right with rate -1
0132 mstate(111:160) = 2;
0133 X_r(5,110) = -1;
0134 
0135 % At 16s move straight for 4 seconds
0136 mstate(161:200) = 1;
0137 
0138 % Generate object state values
0139 for i = 2:n
0140    st = mstate(i);
0141    if isstr(a_func{st}) | strcmp(class(a_func{st}),'function_handle')
0142        X_r(ind{st},i) = feval(a_func{st},X_r(ind{st},i-1),a_param{st});
0143    else 
0144        X_r(ind{st},i) = A{st}*X_r(ind{st},i-1);
0145    end
0146 end
0147 
0148 % Positions of sensors
0149 % $$$ S1 = [-0.5; 3];
0150 % $$$ S2 = [-0.5;-3];
0151 % $$$ S3 = [   7;-3];
0152 % $$$ S4 = [   7; 3];
0153 
0154 S1 = [-0.5; 3.5];
0155 S2 = [-0.5;-3.5];
0156 S3 = [   7;-3.5];
0157 S4 = [   7; 3.5];
0158 
0159 s = [S1 S2 S3 S4];
0160 
0161 % Handles to measurement models
0162 H{1} = @bot_dh_dx;
0163 H{2} = @bot_dh_dx;
0164 
0165 h_func{1} = @bot_h;
0166 h_func{2} = @bot_h;
0167 
0168 h_param{1} = s;
0169 h_param{2} = s;
0170 
0171 % Standard deviation
0172 sd = 0.1*ones(1,size(s,2));
0173 R = {};
0174 R{1} = diag(sd.^2);
0175 R{2} = R{1};
0176 % Generate measurements
0177 Y = bot_h(X_r,s);
0178 % Add noise
0179 for i = 1:size(Y,2)
0180     st = mstate(i);
0181     for j = 1:size(Y,1)
0182         Y(j,i) = Y(j,i) + sqrt(R{st}(j,j)) * randn; 
0183     end
0184 end
0185 
0186 % Print the trajectory
0187 if print_figures
0188     h = plot(X_r(1,:),X_r(2,:),'-g',...
0189              s(1,:),s(2,:),'k^',... 
0190              X_r(1,1),X_r(2,1),'ro','MarkerSize',12);    
0191     legend('Real trajectory',...
0192            'Positions of sensors',...
0193            'Starting position', 'Location', 'North');
0194     set(h,'markersize',5);
0195     set(h,'linewidth',0.5);
0196     set(gca,'FontSize',8);
0197     xlim([-1 7.5]) 
0198     ylim([-4 4]) 
0199     if save_figures
0200         print('-depsc','eimm2_trajectory.eps');
0201     end
0202     pause
0203 end
0204 
0205 
0206 m = [0 0 0 -1 0]';
0207 P = diag([10.1 10.1 1.1 1.1 1]);
0208 
0209 %% Space for the estimates.
0210 
0211 % EKF with model 1
0212 EKF_MM = zeros(size(A{1},1), size(Y,2));
0213 EKF_PP = zeros(size(A{1},1), size(A{1},1), size(Y,2));
0214 
0215 % UKF with model 1
0216 UKF_MM = zeros(size(A{1},1), size(Y,2));
0217 UKF_PP = zeros(size(A{1},1), size(A{1},1), size(Y,2));
0218 
0219 % EKF based IMM
0220 EIMM_MM = zeros(size(m,1), size(Y,2));
0221 EIMM_PP = zeros(size(m,1), size(m,1), size(Y,2));
0222 EIMM_MM_i = cell(nmodels,n);
0223 EIMM_PP_i = cell(nmodels,n);
0224 EIMM_MU = zeros(nmodels,size(Y,2));
0225 
0226 % UKF based IMM
0227 UIMM_MM = zeros(size(m,1), size(Y,2));
0228 UIMM_PP = zeros(size(m,1), size(m,1), size(Y,2));
0229 UIMM_MM_i = cell(nmodels,n);
0230 UIMM_PP_i = cell(nmodels,n);
0231 UIMM_MU = zeros(nmodels,size(Y,2));
0232 
0233 %%% Initial estimates %%%
0234 
0235 % EKF with model 1
0236 EKF_M = [0 0 0 -1]';
0237 EKF_P = diag([1.1 1.1 0.1 0.1]);
0238 
0239 % UKF with model 1
0240 UKF_M = [0 0 0 -1]';
0241 UKF_P = diag([1.1 1.1 0.1 0.1]);
0242 
0243 % EKF based IMM
0244 x_ip1{1} = [0 0 1 0]';
0245 x_ip1{2} = [0 0 1 0 0]';
0246 mu_ip1 = mu_ip;
0247 
0248 P_ip1{1} = diag([0.1 0.1 0.1 0.1]);
0249 P_ip1{2} = diag([0.1 0.1 0.1 0.1 1]);
0250 
0251 % UKF based IMM
0252 x_ip2{1} = [0 0 1 0]';
0253 x_ip2{2} = [0 0 1 0 0]';
0254 mu_ip2 = mu_ip;
0255 
0256 P_ip2{1} = diag([0.1 0.1 0.1 0.1]);
0257 P_ip2{2} = diag([0.1 0.1 0.1 0.1 1]);
0258 
0259 
0260 % Filtering steps.
0261 for i = 1:size(Y,2)
0262     % EKF with model 1
0263     [EKF_M,EKF_P] = kf_predict(EKF_M,EKF_P,EKF_A1,EKF_Q1);
0264     [EKF_M,EKF_P] = ekf_update1(EKF_M,EKF_P,Y(:,i),H{1},R{1},h_func{1},[],h_param{1});
0265     
0266     EKF_MM(:,i)   = EKF_M;
0267     EKF_PP(:,:,i) = EKF_P;
0268 
0269     % UKF with model 2
0270     [UKF_M,UKF_P] = kf_predict(UKF_M,UKF_P,EKF_A1,EKF_Q1);
0271     [UKF_M,UKF_P] = ukf_update1(UKF_M,UKF_P,Y(:,i),h_func{1},R{1},h_param{1});
0272     
0273     UKF_MM(:,i)   = UKF_M;
0274     UKF_PP(:,:,i) = UKF_P;
0275     
0276     % EKF based IMM
0277     [x_p1,P_p1,c_j1] = eimm_predict(x_ip1,P_ip1,mu_ip1,p_ij,ind,dims,A,a_func,a_param,Q);
0278     [x_ip1,P_ip1,mu_ip1,m1,P1] = eimm_update(x_p1,P_p1,c_j1,ind,dims,Y(:,i),H,h_func,R,h_param);
0279     EIMM_MM(:,i)   = m1;
0280     EIMM_PP(:,:,i) = P1;
0281     EIMM_MU(:,i)   = mu_ip1';
0282     EIMM_MM_i(:,i) = x_ip1';
0283     EIMM_PP_i(:,i) = P_ip1';
0284 
0285     % UKF based IMM
0286     [x_p2,P_p2,c_j2] = uimm_predict(x_ip2,P_ip2,mu_ip2,p_ij,ind,dims,A,a_func,a_param,Q);
0287     [x_ip2,P_ip2,mu_ip2,m2,P2] = uimm_update(x_p2,P_p2,c_j2,ind,dims,Y(:,i),H,h_func,R,h_param);
0288     
0289     UIMM_MM(:,i)   = m2;
0290     UIMM_PP(:,:,i) = P2;
0291     UIMM_MU(:,i)   = mu_ip2';
0292     UIMM_MM_i(:,i) = x_ip2';
0293     UIMM_PP_i(:,i) = P_ip2';
0294     
0295     % Plot the estimates so far
0296     if print_figures
0297         plot(EKF_MM(1,1:i),EKF_MM(2,1:i),'y-',...
0298              EIMM_MM(1,1:i),EIMM_MM(2,1:i),'r-',...
0299              UIMM_MM(1,1:i),UIMM_MM(2,1:i),'b-',...
0300              X_r(1,1:i),X_r(2,1:i),'g-');
0301         
0302         % Measurement directions
0303         hold on
0304         for k = 1:size(s,2)
0305             len = sqrt(sum((X_r(1:2,i)-s(:,k)).^2,1));
0306             dx = len*cos(Y(k,i));
0307             dy = len*sin(Y(k,i));
0308             
0309             plot([s(1,k);s(1,k)+dx], [s(2,k);s(2,k)+dy], 'k--')
0310         end
0311         plot(s(1,:),s(2,:),'k^')
0312         xlim([-1 7.5]) 
0313         ylim([-4 4]) 
0314         hold off
0315         drawnow
0316     end
0317 end
0318 
0319 % Smooth with EKF based IMM smoother
0320 [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,h_func,h_param,Y);
0321 
0322 % Smooth with UKF based IMM smoother
0323 [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,h_func,h_param,Y);
0324 
0325 % Smooth the EKF estimates with RTS smoother using model 1
0326 [SM1,SP1] = rts_smooth(EKF_MM,EKF_PP,EKF_A1,EKF_Q1);
0327 
0328 % Smooth the UKF estimates with RTS smoother using model 1
0329 [SM2,SP2] = rts_smooth(UKF_MM,UKF_PP,EKF_A1,EKF_Q1);
0330 
0331 % Calculate the MSEs
0332 MSE_EKF1_1 = mean((X_r(1,:)-EKF_MM(1,:)).^2);
0333 MSE_EKF1_2 = mean((X_r(2,:)-EKF_MM(2,:)).^2);
0334 MSE_EKF1 = 1/2*(MSE_EKF1_1 + MSE_EKF1_2);
0335 
0336 MSE_EKS1_1 = mean((X_r(1,:)-SM1(1,:)).^2);
0337 MSE_EKS1_2 = mean((X_r(2,:)-SM1(2,:)).^2);
0338 MSE_EKS1 = 1/2*(MSE_EKS1_1 + MSE_EKS1_2);
0339 
0340 % Calculate the MSEs
0341 MSE_UKF1_1 = mean((X_r(1,:)-UKF_MM(1,:)).^2);
0342 MSE_UKF1_2 = mean((X_r(2,:)-UKF_MM(2,:)).^2);
0343 MSE_UKF1 = 1/2*(MSE_UKF1_1 + MSE_UKF1_2);
0344 
0345 MSE_UKS1_1 = mean((X_r(1,:)-SM2(1,:)).^2);
0346 MSE_UKS1_2 = mean((X_r(2,:)-SM2(2,:)).^2);
0347 MSE_UKS1 = 1/2*(MSE_UKS1_1 + MSE_UKS1_2);
0348 
0349 MSE_EIMM1 = mean((X_r(1,:)-EIMM_MM(1,:)).^2);
0350 MSE_EIMM2 = mean((X_r(2,:)-EIMM_MM(2,:)).^2);
0351 MSE_EIMM = 1/2*(MSE_EIMM1 + MSE_EIMM2);
0352 
0353 MSE_EIMMS1 = mean((X_r(1,:)-SMI_1(1,:)).^2);
0354 MSE_EIMMS2 = mean((X_r(2,:)-SMI_1(2,:)).^2);
0355 MSE_EIMMS = 1/2*(MSE_EIMMS1 + MSE_EIMMS2);
0356 
0357 MSE_UIMM1 = mean((X_r(1,:)-UIMM_MM(1,:)).^2);
0358 MSE_UIMM2 = mean((X_r(2,:)-UIMM_MM(2,:)).^2);
0359 MSE_UIMM = 1/2*(MSE_UIMM1 + MSE_UIMM2);
0360 
0361 MSE_UIMMS1 = mean((X_r(1,:)-SMI_2(1,:)).^2);
0362 MSE_UIMMS2 = mean((X_r(2,:)-SMI_2(2,:)).^2);
0363 MSE_UIMMS = 1/2*(MSE_UIMMS1 + MSE_UIMMS2);
0364 
0365 
0366 fprintf('Mean square errors of position estimates:\n');
0367 fprintf('EKF1-RMSE = %.4f\n',MSE_EKF1);
0368 fprintf('EKS1-RMSE = %.4f\n',MSE_EKS1);
0369 fprintf('UKF1-RMSE = %.4f\n',MSE_UKF1);
0370 fprintf('UKS1-RMSE = %.4f\n',MSE_UKS1);
0371 fprintf('EIMM-RMSE = %.4f\n',MSE_EIMM);
0372 fprintf('EIMMS-RMSE = %.4f\n',MSE_EIMMS);
0373 fprintf('UIMM-RMSE = %.4f\n',MSE_UIMM);
0374 fprintf('UIMMS-RMSE = %.4f\n',MSE_UIMMS);
0375 
0376 
0377 % Plot the final filtering and smoothing results
0378 if print_figures
0379     h = plot(X_r(1,:),X_r(2,:),'g-',...         
0380              EKF_MM(1,:),EKF_MM(2,:),'-k',...
0381              SM1(1,:),SM1(2,:),'--k',...             
0382              EIMM_MM(1,:),EIMM_MM(2,:),'-r',...             
0383              SMI_1(1,:),SMI_1(2,:),'--r',...
0384              UIMM_MM(1,:),UIMM_MM(2,:),'-b',...
0385              SMI_2(1,:),SMI_2(2,:),'--b');
0386     legend('True trajectory',...
0387            'EKF',...
0388            'ERTS',...
0389            'IMM-EKF',...
0390            'IMM-EKS',...
0391            'IMM-UKF',...           
0392            'IMM-UKS');
0393     %title('Estimates produced by IMM-filter.')
0394     set(h,'markersize',2);
0395     set(h,'linewidth',0.5);
0396     set(gca,'FontSize',8);
0397     xlim([-1 7.5]) 
0398     ylim([-4 4]) 
0399     if save_figures
0400         print('-depsc','eimm2_1.eps');
0401     end
0402     pause
0403 
0404     
0405     % Determine the real model probabilities
0406     p_models = zeros(nmodels,n);
0407     I1 = find(mstate == 1);
0408     p_models(1,I1) = 1;
0409     I2 = find(mstate == 2);
0410     p_models(2,I2) = 1;
0411  
0412     % Plot model 2 probability for filters
0413     h = plot(1:n, p_models(2,:),'g--',...
0414              1:n,EIMM_MU(2,:)','-r',...
0415              1:n,MU_S_1(2,:)','--r',...
0416              1:n,UIMM_MU(2,:)','-b',...
0417              1:n,MU_S_2(2,:)','--b');
0418     %1:n,MU_S_2(2,:)','b-');
0419     legend('True',...
0420            'IMM-EKF',...
0421            'IMM-EKS',...
0422            'IMM-UKF',...
0423            'IMM-UKS');
0424     %title('Probability of model 2');
0425     ylim([-0.1,1.1]);
0426     set(h,'markersize',2);
0427     set(h,'linewidth',0.5);
0428     set(gca,'FontSize',8);
0429     if save_figures
0430         print('-depsc','eimm2_2.eps');
0431     end
0432     pause
0433     
0434     % Collect the turn rate estimates
0435     EMM_t = zeros(5,n);
0436     EMM_s = zeros(5,n);
0437     UMM_t = zeros(5,n);
0438     UMM_s = zeros(5,n); 
0439     for i = 1:n
0440         EMM_t(:,i) = EIMM_MM_i{2,i};
0441         EMM_s(:,i) = SMI_i_1{2,i};
0442         UMM_t(:,i) = UIMM_MM_i{2,i};
0443         UMM_s(:,i) = SMI_i_2{2,i};
0444     end
0445     
0446     % Plot the filtered turn rates
0447     h = plot(1:n, X_r(5,:),'-g',...
0448              1:n,EMM_t(5,:),'-r',...     
0449              1:n,EMM_s(5,:),'--r',...     
0450              1:n,UMM_t(5,:),'-b',...             
0451              1:n,UMM_s(5,:),'--b');
0452     %title('Turn rate estimates')
0453     legend('True',...
0454            'IMM-EKF',...
0455            'IMM-EKS',...
0456            'IMM-UKF',...
0457            'IMM-UKS');
0458     set(h,'markersize',2);
0459     set(h,'linewidth',0.5);
0460     set(gca,'FontSize',8);
0461     if save_figures
0462         print('-depsc','eimm2_3.eps');
0463     end
0464   
0465 end
0466

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