Home > src > demos > ungm_demo > ungm_demo.m

ungm_demo

PURPOSE ^

% Demonstration of univariate nonstationary growth model (UNGM)

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

% Demonstration of univariate nonstationary growth model (UNGM) 

  Description:
    In this example various different non-linear filters and smoothers are
    applied to the univariate nonstationary growth model (UNGM). The 
    filters used in this demonstration are:
      * Extended Kalman filter
      * Unscented Kalman filter (augmented and non-augmented forms)
      * Bootstrap filter
      * Gauss-Hermite Kalman filter (degree 10)
      * Cubature Kalman filter
    Additionally, the corresponding Rauch-Tung-Striebel smoother results 
    are also presented.

  References:
    Refer to the Toolbox documentation for details on the model.

  See also:
    ukf_predict1, ukf_update1, ukf_predict3, ukf_update3, urts_smooth1,
    ekf_predict1, ekf_update1, erts_smooth1, ghkf_predict, ghkf_update,
    ghrts_smooth, ckf_predict, ckf_update, crts_smooth

  Author:
    Copyright (C) 2007 Jouni Hartikainen,
    Updated by Arno Solin (2010).

  Licence:
    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 of univariate nonstationary growth model (UNGM)
0002 %
0003 %  Description:
0004 %    In this example various different non-linear filters and smoothers are
0005 %    applied to the univariate nonstationary growth model (UNGM). The
0006 %    filters used in this demonstration are:
0007 %      * Extended Kalman filter
0008 %      * Unscented Kalman filter (augmented and non-augmented forms)
0009 %      * Bootstrap filter
0010 %      * Gauss-Hermite Kalman filter (degree 10)
0011 %      * Cubature Kalman filter
0012 %    Additionally, the corresponding Rauch-Tung-Striebel smoother results
0013 %    are also presented.
0014 %
0015 %  References:
0016 %    Refer to the Toolbox documentation for details on the model.
0017 %
0018 %  See also:
0019 %    ukf_predict1, ukf_update1, ukf_predict3, ukf_update3, urts_smooth1,
0020 %    ekf_predict1, ekf_update1, erts_smooth1, ghkf_predict, ghkf_update,
0021 %    ghrts_smooth, ckf_predict, ckf_update, crts_smooth
0022 %
0023 %  Author:
0024 %    Copyright (C) 2007 Jouni Hartikainen,
0025 %    Updated by Arno Solin (2010).
0026 %
0027 %  Licence:
0028 %    This software is distributed under the GNU General Public
0029 %    Licence (version 2 or later); please refer to the file
0030 %    Licence.txt, included with the software, for details.
0031 
0032 %% Set up the model parameters
0033 
0034 silent = 0;
0035 
0036 clf; clc;
0037 disp('This is a demonstration for EKF and UKF using the UNGM model.');
0038 disp(' ');
0039 
0040 % Handles to dynamic and measurement model functions,
0041 % and their derivatives
0042 f_func = @ungm_f;
0043 h_func = @ungm_h;
0044 df_func = @ungm_df_dx;
0045 dh_func = @ungm_dh_dx;
0046 d2f_func = @ungm_d2f_dx2;
0047 d2h_func = @ungm_d2h_dx2;
0048 
0049 % Number of samples
0050 n = 500;
0051 
0052 % Initial state and covariance
0053 x_0 = .1;
0054 P_0 = 1;
0055 
0056 % Space for measurements
0057 Y = zeros(1,n);
0058 
0059 % Strengths of perturbations
0060 u_n = 1;
0061 v_n = 1;
0062 
0063 fprintf('Generating real states and measurements...');
0064 % Generate the true states with process noise
0065 X = zeros(1,n);
0066 X(1) = ungm_f(x_0,1) + gauss_rnd(0,u_n,1);
0067 for i = 2:n
0068     X(i) = feval(f_func,X(i-1),i) + gauss_rnd(0,u_n,1);
0069 end
0070     
0071 % Generate the observations with measurement noise
0072 for i = 1:n
0073     Y(i) = feval(h_func,X(i)) + gauss_rnd(0,v_n,1);
0074 end
0075 
0076 Y_r = feval(h_func,X);
0077 
0078 fprintf('Done!\n');
0079 
0080 % Parameters for dynamic model. Used by smoothers.
0081 params = cell(size(Y,2));
0082 for i = 1:size(Y,2)
0083    params{i} = i+1; 
0084 end
0085 
0086 
0087 %% Run the various filters and smoothers
0088 
0089 fprintf('Filtering with UKF1...');
0090 
0091 % Initial values and space for non-augmented UKF (UKF1)
0092 M = x_0;
0093 P = P_0;
0094 MM_UKF1 = zeros(size(M,1),size(Y,2));
0095 PP_UKF1 = zeros(size(M,1),size(M,1),size(Y,2));
0096 
0097 % Filtering loop for UKF1
0098 for k = 1:size(Y,2)
0099    [M,P] = ukf_predict1(M,P,f_func,u_n,k);
0100    [M,P] = ukf_update1(M,P,Y(:,k),h_func,v_n);
0101    MM_UKF1(:,k)   = M;
0102    PP_UKF1(:,:,k) = P;    
0103 end
0104 [MMS_URTS1, PPS_URTS1] = urts_smooth1(MM_UKF1,PP_UKF1,f_func,u_n,params,[],[],[],[],0);
0105 
0106 % MSE for UKF1
0107 UKF1_MSE = sum((X-MM_UKF1).^2)/n;
0108 UKS1_MSE = sum((X-MMS_URTS1).^2)/n;
0109 
0110 fprintf('Done!\n');
0111 fprintf('Filtering with UKF2...');
0112 
0113 % Initial valuesand space for augmented UKF (UKF2)
0114 M = x_0;
0115 P = P_0;
0116 MM_UKF2 = zeros(size(M,1),size(Y,2));
0117 PP_UKF2 = zeros(size(M,1),size(M,1),size(Y,2));
0118 
0119 % Filtering loop for UKF2
0120 for k = 1:size(Y,2)
0121    [M,P,X_s,w] = ukf_predict3(M,P,f_func,u_n,v_n,k);
0122    [M,P] = ukf_update3(M,P,Y(:,k),h_func,v_n,X_s,w,[]);
0123    MM_UKF2(:,k)   = M;
0124    PP_UKF2(:,:,k) = P;    
0125 end
0126 
0127 [MMS_URTS2, PPS_URTS2] = urts_smooth1(MM_UKF2,PP_UKF2,f_func,u_n,params,[],[],[],[],0);
0128 
0129 % MSE for UKF2
0130 UKF2_MSE = sum((X-MM_UKF2).^2)/n;
0131 UKS2_MSE = sum((X-MMS_URTS2).^2)/n;
0132 
0133 fprintf('Done!\n');
0134 
0135 fprintf('Filtering with EKF...');
0136 
0137 % Filtering with EKF
0138 M = x_0;
0139 P = P_0;
0140 
0141 MM_EKF = zeros(size(M,1),size(Y,2));
0142 PP_EKF = zeros(size(M,1),size(M,1),size(Y,2));
0143 
0144 % Filtering loop for EKF
0145 for k = 1:size(Y,2)
0146    [M,P] = ekf_predict1(M,P,df_func,u_n,f_func,[],k);
0147    [M,P] = ekf_update1(M,P,Y(:,k),dh_func,v_n,h_func,[],[]);
0148    MM_EKF(:,k)   = M;
0149    PP_EKF(:,:,k) = P;    
0150 end
0151 
0152 [MMS_ERTS,PPS_ERTS] = erts_smooth1(MM_EKF,PP_EKF,df_func,u_n,f_func,[],params,0);
0153 
0154 EKF_MSE = sum((X-MM_EKF).^2)/n;
0155 ERTS_MSE = sum((X-MMS_ERTS).^2)/n;
0156 
0157 fprintf('Done!\n');
0158 
0159 fprintf('Filtering with bootstrap filter...');
0160 
0161 % Filtering with bootstrap filter
0162 M = x_0;
0163 P = P_0;
0164 n_particles = 1000;
0165 SX = gauss_rnd(M,P,n_particles);
0166 
0167 MM_BS = zeros(size(M,1),size(Y,2));
0168 PP_BS = zeros(size(M,1),size(M,1),size(Y,2));
0169 
0170 % Filtering loop for bootstrap filter
0171 for k = 1:size(Y,2)
0172    SX = ungm_f(SX,k) + gauss_rnd(0,1,size(SX,2));
0173    W  = gauss_pdf(Y(:,k),ungm_h(SX,k),1);
0174    ind = resampstr(W);
0175    SX = SX(:,ind);
0176    M = mean(SX);
0177    P = var(SX);
0178    MM_BS(:,k)   = M;
0179    PP_BS(:,:,k) = P;    
0180 end
0181 
0182 BS_MSE = sum((X-MM_BS).^2)/n;
0183 
0184 fprintf('Done!\n');
0185 
0186 fprintf('Filtering with Gauss-Hermite Kalman filter...');
0187 
0188 % Filtering with GHKF
0189 M = x_0;
0190 P = P_0;
0191 
0192 MM_GHKF = zeros(size(M,1),size(Y,2));
0193 PP_GHKF = zeros(size(M,1),size(M,1),size(Y,2));
0194 
0195 % Filtering loop for GHKF
0196 for k = 1:size(Y,2)
0197    [M,P] = ghkf_predict(M,P,f_func,u_n,k);
0198    [M,P] = ghkf_update(M,P,Y(:,k),h_func,v_n);
0199    MM_GHKF(:,k)   = M;
0200    PP_GHKF(:,:,k) = P;    
0201 end
0202 [MMS_GHRTS, PPS_GHRTS] = ghrts_smooth(MM_GHKF,PP_GHKF,f_func,u_n,params,[],0);
0203 
0204 GHKF_MSE = sum((X-MM_GHKF).^2)/n;
0205 GHRTS_MSE = sum((X-MMS_GHRTS).^2)/n;
0206 
0207 fprintf('Done!\n');
0208 
0209 fprintf('Filtering with Cubature Kalman filter...');
0210 
0211 % Filtering with CKF
0212 M = x_0;
0213 P = P_0;
0214 
0215 MM_CKF = zeros(size(M,1),size(Y,2));
0216 PP_CKF = zeros(size(M,1),size(M,1),size(Y,2));
0217 
0218 % Filtering loop for CKF
0219 for k = 1:size(Y,2)
0220    [M,P] = ckf_predict(M,P,f_func,u_n,k);
0221    [M,P] = ckf_update(M,P,Y(:,k),h_func,v_n);
0222    MM_CKF(:,k)   = M;
0223    PP_CKF(:,:,k) = P;    
0224 end
0225 [MMS_CRTS, PPS_CRTS] = crts_smooth(MM_CKF,PP_CKF,f_func,u_n,params,0);
0226 
0227 CKF_MSE = sum((X-MM_CKF).^2)/n;
0228 CRTS_MSE = sum((X-MMS_CRTS).^2)/n;
0229 
0230 fprintf('Done!\n');
0231 
0232 
0233 %% Visualize results
0234 
0235 if ~silent
0236   subplot(3,1,1);
0237   plot(1:100,X(1:100),'-kx',1:100,MM_UKF2(1:100),'--bo')
0238   title('UKF2 filtering result');
0239   xlim([0 100]);
0240   ylim([-20 20]);
0241   legend('Real signal', 'UKF2 filtered estimate');
0242 
0243   subplot(3,1,2);
0244   plot(1:100,X(1:100),'-kx',1:100,MM_EKF(1:100),'--bo')
0245   title('EKF filtering result');
0246   xlim([0 100]);
0247   ylim([-20 20]);
0248   legend('Real signal', 'EKF filtered estimate');
0249 
0250   subplot(3,1,3);
0251   plot(1:100,X(1:100),'-kx',1:100,MM_BS(1:100),'--bo')
0252   title('Bootstrap filtering result')
0253   xlim([0 100]);
0254   ylim([-20 20]);
0255   legend('Real signal','Filtered estimates')
0256   % Uncomment if you want to print images
0257   %print -dpsc ungm_states.ps
0258 
0259   disp(' ');
0260   disp(['First 100 values of the filtering results with UKF2, EKF and BS are now displayed.',...
0261         'Notice how the BS gives clearly better performance than UKF2 and EKF.']);
0262   disp(' ');
0263   disp('<press any key to see the estimation error of UKF2, EKF and BS>');
0264   
0265   pause;
0266 
0267   subplot(3,1,1);
0268   E = X-MM_UKF2;
0269   PP_UKF2 = squeeze(PP_UKF2);
0270   plot(1:100,E(1:100),'-kx',1:100,3*sqrt(PP_UKF2(1:100))','--r',1:100,-3*sqrt(PP_UKF2(1:100))','--r');
0271   title('UKF2 error');
0272   legend('Estimation error of UKF2','3\sigma interval')
0273 
0274   subplot(3,1,2);
0275   E = X-MM_EKF;
0276   PP_EKF = squeeze(PP_EKF);
0277   plot(1:100,E(1:100),'-kx',1:100,3*sqrt(PP_EKF(1:100))','--r',1:100,-3*sqrt(PP_EKF(1:100))','--r');
0278   title('EKF error');
0279   legend('Estimation error of EKF','3\sigma interval');
0280 
0281   subplot(3,1,3);
0282   E = X-MM_BS;
0283   PP_BS = squeeze(PP_BS);
0284   plot(1:100,E(1:100),'-kx',1:100,3*sqrt(PP_BS(1:100))','--r',1:100,-3*sqrt(PP_BS(1:100))','--r');
0285   title('Bootstrap filtering error');
0286   legend('Estimation error with BS','3\sigma interval')
0287   % Uncomment if you want to print images
0288   %print -dpsc ungm_c_errors.ps
0289   clc;
0290   disp(['The estimation errors of UKF2, EKF and BS are now displayed.',...
0291         'The superiority of BS can easily be seen also from this.']);
0292   disp(' ');
0293   disp('<press any key to see the filtering results of UKF1 and UKF2>');  
0294   pause;  
0295   
0296   subplot(2,1,1);
0297   plot(1:100,X(1:100),'-kx',1:100,MM_UKF1(1:100),'--bo')
0298   title('UKF1 filtering result');
0299   xlim([0 100]);
0300   ylim([-20 20]);
0301   legend('Real signal', 'UKF1 filtered estimate');
0302   
0303   subplot(2,1,2);
0304   plot(1:100,X(1:100),'-kx',1:100,MM_UKF2(1:100),'--bo')
0305   title('UKF2 filtering result');
0306   xlim([0 100]);
0307   ylim([-20 20]);
0308   legend('Real signal', 'UKF2 filtered estimate');
0309   % Uncomment if you want to print images
0310   %print -dpsc ungm_ukf_comp.ps
0311   clc;
0312   disp(['First 100 values of the filtering results with UKF1 and UKF2 are now displayed. ',...
0313        'Clearly the non-augmented UKF is not applicable to this problem.']);
0314   disp(' ');
0315   disp('<press any key to see the estimation errors of UKF1 and UKF2>');
0316   
0317   pause
0318 
0319   subplot(2,1,1);
0320   E = X-MM_UKF1;
0321   PP_UKF1 = squeeze(PP_UKF1);
0322   plot(1:100,E(1:100),'-kx',1:100,3*sqrt(PP_UKF1(1:100))','--r',1:100,-3*sqrt(PP_UKF1(1:100))','--r');
0323   title('UKF1 error');
0324   legend('Estimation error of UKF1','3\sigma interval')
0325 
0326   subplot(2,1,2);
0327   E = X-MM_UKF2;
0328   PP_UKF2 = squeeze(PP_UKF2);
0329   plot(1:100,E(1:100),'-kx',1:100,3*sqrt(PP_UKF2(1:100))','--r',1:100,-3*sqrt(PP_UKF2(1:100))','--r');
0330   title('UKF2 error');
0331   legend('Estimation error of UKF2','3\sigma interval')
0332   % Uncomment if you want to print images
0333   %print -dpsc ungm_ukf_comp_error.ps
0334   clc;
0335   disp('The absolute estimation errors of UKF1 and UKF2 are now displayed.');
0336   disp(' ');
0337   disp('<press any key to see the filtering results of GHKF and CKF>');
0338   pause
0339   
0340   subplot(2,1,1);
0341   plot(1:100,X(1:100),'-kx',1:100,MM_GHKF(1:100),'--bo')
0342   title('GHKF filtering result');
0343   xlim([0 100]);
0344   ylim([-20 20]);
0345   legend('Real signal', 'GHKF filtered estimate');
0346   
0347   subplot(2,1,2);
0348   plot(1:100,X(1:100),'-kx',1:100,MM_CKF(1:100),'--bo')
0349   title('CKF filtering result');
0350   xlim([0 100]);
0351   ylim([-20 20]);
0352   legend('Real signal', 'CKF filtered estimate');
0353   % Uncomment if you want to print images
0354   %print -dpsc ungm_ukf_comp.ps
0355   clc;
0356   disp(['First 100 values of the filtering results with GHKF (10 th degree) and CKF are now displayed. ',...
0357        'Clearly the non-augmented UKF is not applicable to this problem.']);
0358   disp(' ');
0359   disp('<press any key to see MSE values for each method>');
0360   
0361   pause
0362   
0363 end
0364 
0365 disp('MS errors:');
0366 fprintf('UKF1-MSE  = %.4f\n',UKF1_MSE);
0367 fprintf('UKS1-MSE  = %.4f\n',UKS1_MSE);
0368 fprintf('UKF2-MSE  = %.4f\n',UKF2_MSE);
0369 fprintf('UKS2-MSE  = %.4f\n',UKS2_MSE);
0370 fprintf('EKF-MSE   = %.4f\n',EKF_MSE);
0371 fprintf('ERTS-MSE  = %.4f\n',ERTS_MSE);
0372 fprintf('BS-MSE    = %.4f\n',BS_MSE);
0373 % CKF and GHKF methods
0374 fprintf('GHKF-MSE  = %.4f\n',GHKF_MSE);
0375 fprintf('GHRTS-MSE = %.4f\n',GHRTS_MSE);
0376 fprintf('CKF-MSE   = %.4f\n',CKF_MSE);
0377 fprintf('CRTS-MSE  = %.4f\n',CRTS_MSE);

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