Home > src > demos > reentry_demo > reentry_demo.m

reentry_demo

PURPOSE ^

% Discrete-time reentry dynamics demonstration with non-linear filters

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

% Discrete-time reentry dynamics demonstration with non-linear filters

  Description:
    In this example various different non-linear filters and smoothers are
    applied to reentry tracking problem . The filters used in this 
    demonstration are:
      * Extended Kalman filter
      * Unscented Kalman filter
      * Gauss-Hermite Kalman filter (degree 3)
      * Cubature Kalman filter
    Additionally, the corresponding smoother results are also presented.

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

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

  Author:
    Copyright (C) 2006 Simo Särkkä
                  2007 Jouni Hartikainen
                  2010 Arno Solin

  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 %% Discrete-time reentry dynamics demonstration with non-linear filters
0002 %
0003 %  Description:
0004 %    In this example various different non-linear filters and smoothers are
0005 %    applied to reentry tracking problem . The filters used in this
0006 %    demonstration are:
0007 %      * Extended Kalman filter
0008 %      * Unscented Kalman filter
0009 %      * Gauss-Hermite Kalman filter (degree 3)
0010 %      * Cubature Kalman filter
0011 %    Additionally, the corresponding smoother results are also presented.
0012 %
0013 %  References:
0014 %    Refer to the Toolbox documentation for details on the model.
0015 %
0016 %  See also:
0017 %    ukf_predict1, ukf_update1, urts_smooth1,
0018 %    ekf_predict1, ekf_update1, erts_smooth1, ghkf_predict, ghkf_update,
0019 %    ghrts_smooth, ckf_predict, ckf_update, crts_smooth
0020 %
0021 %  Author:
0022 %    Copyright (C) 2006 Simo Särkkä
0023 %                  2007 Jouni Hartikainen
0024 %                  2010 Arno Solin
0025 %
0026 %  Licence:
0027 %    This software is distributed under the GNU General Public
0028 %    Licence (version 2 or later); please refer to the file
0029 %    Licence.txt, included with the software, for details.
0030 
0031 %% Set up model parameters
0032 
0033   reentry_param;
0034   make_reentry_data;
0035 
0036   silent = 0;
0037 
0038   % Handles to dynamic and measurement model functions,
0039   % and to their derivatives.
0040   func_f  = @reentry_f;
0041   func_if = @reentry_if;
0042   func_df = @reentry_df_dx;
0043   func_h  = @reentry_h;
0044   func_dh = @reentry_dh_dx;
0045   
0046   % Initial values and space for EKF
0047   m = m0;
0048   P = P0;
0049   Q = L*Qc*L'*dt;
0050   
0051   MM_EKF = zeros(size(m,1),size(Y,2));
0052   PP_EKF = zeros(size(m,1),size(m,1),size(Y,2));
0053   VV_EKF = zeros(size(m,1),size(Y,2));
0054   EE_EKF = zeros(size(m,1),size(Y,2));
0055   
0056   % Check derivatives (should be OK)
0057   %der_check(func_a, func_da, 1, m0, {dt,b0,H0,Gm0,R0});
0058   %der_check(func_h, func_dh, 1, m0, {xr,yr});
0059 
0060   clf; clc; 
0061   disp(['This is a demonstration for tracking a reentry vehicle ',...
0062         'using 1st order EKF and augmented UKF.'])
0063   disp(' ');
0064   
0065   
0066 %% Extended Kalman filter
0067   
0068   fprintf('Running EKF...'); 
0069   % Filtering with EKF
0070   for k=1:size(Y,2)
0071     [m,P] = ekf_predict1(m,P,func_df,Q,func_f,[],{dt,b0,H0,Gm0,R0});
0072     [m,P] = ekf_update1(m,P,Y(:,k),func_dh,diag([vr va]),func_h,[],{xr,yr});
0073     MM_EKF(:,k) = m;
0074     PP_EKF(:,:,k) = P;
0075     VV_EKF(:,k) = diag(P);
0076     EE_EKF(:,k) = (X(:,k) - m).^2;
0077   end
0078 
0079   %
0080   % Calculate RMSE
0081   %
0082   ekf_rmse = sqrt(mean(sum((X(1:2,:)-MM_EKF(1:2,:)).^2)));
0083   ME_EKF = squeeze(PP_EKF(1,1,:)+PP_EKF(2,2,:));
0084   fprintf('Done!\n')
0085 
0086   fprintf('Running smoothers...');
0087   %
0088   % Smoother 1
0089   %
0090   [SM_ERTS,SP_ERTS] = erts_smooth1(MM_EKF,PP_EKF,func_df,Qc*dt,func_f,L,...
0091                           {dt,b0,H0,Gm0,R0});
0092   eks_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_ERTS(1:2,:)).^2)));
0093   ME_ERTS = squeeze(SP_ERTS(1,1,:)+SP_ERTS(2,2,:));
0094 
0095 
0096   SV_ERTS = zeros(size(m,1),size(Y,2));
0097   SE_ERTS = zeros(size(m,1),size(Y,2));
0098   for k=1:size(Y,2)
0099     SV_ERTS(:,k) = diag(SP_ERTS(:,:,k));
0100     SE_ERTS(:,k) = (X(:,k) - SM_ERTS(:,k)).^2;
0101   end
0102 
0103   %
0104   % Smoother 2
0105   %
0106   [SM_ETF,SP_ETF] = etf_smooth1(MM_EKF,PP_EKF,Y,...
0107     func_df,Qc*dt,func_if,L,{dt,b0,H0,Gm0,R0},...
0108     func_dh,diag([vr va]),func_h,[],{xr,yr});
0109   
0110   eks_rmse2 = sqrt(mean(sum((X(1:2,:)-SM_ETF(1:2,:)).^2)));
0111   ME_ETF = squeeze(SP_ETF(1,1,:)+SP_ETF(2,2,:));
0112   fprintf('Done!\n');
0113   
0114   
0115 %% Unscented Kalman filter
0116 
0117   fprintf('Running UKF...'); 
0118 
0119   % Initial values and space for (augmented) UKF
0120   m = m0;
0121   P = P0;
0122   MM_UKF = zeros(size(m,1),size(Y,2));
0123   PP_UKF = zeros(size(m,1),size(m,1),size(Y,2));
0124   VV_UKF = zeros(size(m,1),size(Y,2));
0125   EE_UKF = zeros(size(m,1),size(Y,2));
0126 
0127   % Filtering with UKF
0128   for k=1:size(Y,2)
0129     % Non-augmented UKF
0130     %[m,P] = ukf_predict1(m,P,func_a,Q,{dt,b0,H0,Gm0,R0});
0131 
0132     % Augmented UKF with separate sigma points
0133     %[m,P] = ukf_predict2(m,P,func_f,Qc*dt,{dt,b0,H0,Gm0,R0,L});
0134     
0135     %[m,P] = ukf_update2(m,P,Y(:,k),func_h,diag([vr va]),{xr,yr});
0136     
0137     % Augmented UKF with same sigma points for predict and update steps
0138     [m,P,X_s,w] = ukf_predict3(m,P,func_f,Qc*dt,diag([vr va]),d_param);  
0139     [m,P] = ukf_update3(m,P,Y(:,k),func_h,diag([vr va]),X_s,w,h_param,h_param);    
0140 
0141     MM_UKF(:,k) = m;
0142     PP_UKF(:,:,k) = P;
0143     VV_UKF(:,k) = diag(P);
0144     EE_UKF(:,k) = (X(:,k) - m).^2;
0145   end
0146 
0147   %
0148   % Calculate RMSE of UKF
0149   %
0150   ukf_rmse = sqrt(mean(sum((X(1:2,:)-MM_UKF(1:2,:)).^2)));
0151   ME_UKF = squeeze(PP_UKF(1,1,:)+PP_UKF(2,2,:));
0152   fprintf('Done!\n');
0153   
0154   fprintf('Running smoothers...');
0155   %
0156   % Smoother 1
0157   %
0158   [SM_URTS,SP_URTS] = urts_smooth1(MM_UKF,PP_UKF,func_f,Q,d_param);
0159   uks_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_URTS(1:2,:)).^2)));
0160   ME_URTS = squeeze(SP_URTS(1,1,:)+SP_URTS(2,2,:));
0161 
0162 
0163   SV_URTS = zeros(size(m,1),size(Y,2));
0164   SE_URTS = zeros(size(m,1),size(Y,2));
0165   for k=1:size(Y,2)
0166     SV_URTS(:,k) = diag(SP_URTS(:,:,k));
0167     SE_URTS(:,k) = (X(:,k) - SM_URTS(:,k)).^2;
0168   end
0169   
0170   [SM_URTSb,SP_URTSb] = urts_smooth2(MM_UKF,PP_UKF,func_f,Qc*dt,d_param);
0171   uks_rmse1b = sqrt(mean(sum((X(1:2,:)-SM_URTSb(1:2,:)).^2)));
0172   ME_URTSb = squeeze(SP_URTSb(1,1,:)+SP_URTSb(2,2,:));
0173 
0174   %
0175   % Smoother 2
0176   %
0177   [SM_UTF,SP_UTF] = utf_smooth1(MM_UKF,PP_UKF,Y,...
0178     func_if,Qc*dt,d_param,...
0179     func_h,diag([vr va]),h_param);
0180   
0181   uks_rmse2 = sqrt(mean(sum((X(1:2,:)-SM_UTF(1:2,:)).^2)));
0182   ME_UTF = squeeze(SP_UTF(1,1,:)+SP_UTF(2,2,:));
0183 
0184   fprintf('Done!\n');
0185 
0186   
0187 %% Gauss-Hermite Kalman filter
0188   
0189   fprintf('Running GHKF...'); 
0190   
0191   % Initial values and space for GHKF
0192   m = m0;
0193   P = P0;
0194   MM_GHKF = zeros(size(m,1),size(Y,2));
0195   PP_GHKF = zeros(size(m,1),size(m,1),size(Y,2));
0196   VV_GHKF = zeros(size(m,1),size(Y,2));
0197   EE_GHKF = zeros(size(m,1),size(Y,2));
0198   
0199   % Filtering with GHKF
0200   for k=1:size(Y,2)
0201     [m,P] = ghkf_predict(m,P,func_f,Q,{dt,b0,H0,Gm0,R0},3);
0202     [m,P] = ghkf_update(m,P,Y(:,k),func_h,diag([vr va]),{xr,yr},3);
0203     MM_GHKF(:,k) = m;
0204     PP_GHKF(:,:,k) = P;
0205     VV_GHKF(:,k) = diag(P);
0206     EE_GHKF(:,k) = (X(:,k) - m).^2;
0207   end
0208 
0209   %
0210   % Calculate RMSE
0211   %
0212   ghkf_rmse = sqrt(mean(sum((X(1:2,:)-MM_GHKF(1:2,:)).^2)));
0213   ME_GHKF = squeeze(PP_GHKF(1,1,:)+PP_GHKF(2,2,:));
0214   fprintf('Done!\n')
0215 
0216   fprintf('Running smoother...');
0217   %
0218   % Smoother
0219   %
0220   [SM_GHRTS,SP_GHRTS] = ghrts_smooth(MM_GHKF,PP_GHKF,func_f,Q,...
0221                           {dt,b0,H0,Gm0,R0},3);
0222   ghrts_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_GHRTS(1:2,:)).^2)));
0223   ME_GHRTS = squeeze(SP_GHRTS(1,1,:)+SP_GHRTS(2,2,:));
0224 
0225 
0226   SV_GHRTS = zeros(size(m,1),size(Y,2));
0227   SE_GHRTS = zeros(size(m,1),size(Y,2));
0228   for k=1:size(Y,2)
0229     SV_GHRTS(:,k) = diag(SP_GHRTS(:,:,k));
0230     SE_GHRTS(:,k) = (X(:,k) - SM_GHRTS(:,k)).^2;
0231   end
0232 
0233   fprintf('Done!\n');
0234 
0235   
0236 %% Cubature Kalman filter
0237   
0238   fprintf('Running CKF...'); 
0239   
0240   % Initial values and space for CKF
0241   m = m0;
0242   P = P0;
0243   MM_CKF = zeros(size(m,1),size(Y,2));
0244   PP_CKF = zeros(size(m,1),size(m,1),size(Y,2));
0245   VV_CKF = zeros(size(m,1),size(Y,2));
0246   EE_CKF = zeros(size(m,1),size(Y,2));
0247   
0248   % Filtering with CKF
0249   for k=1:size(Y,2)
0250     [m,P] = ckf_predict(m,P,func_f,Q,{dt,b0,H0,Gm0,R0});
0251     [m,P] = ckf_update(m,P,Y(:,k),func_h,diag([vr va]),{xr,yr});
0252     MM_CKF(:,k) = m;
0253     PP_CKF(:,:,k) = P;
0254     VV_CKF(:,k) = diag(P);
0255     EE_CKF(:,k) = (X(:,k) - m).^2;
0256   end
0257 
0258   %
0259   % Calculate RMSE
0260   %
0261   ckf_rmse = sqrt(mean(sum((X(1:2,:)-MM_CKF(1:2,:)).^2)));
0262   ME_CKF = squeeze(PP_CKF(1,1,:)+PP_CKF(2,2,:));
0263   fprintf('Done!\n')
0264 
0265   fprintf('Running smoother...');
0266   %
0267   % Smoother
0268   %
0269   [SM_CRTS,SP_CRTS] = crts_smooth(MM_CKF,PP_CKF,func_f,Q,...
0270                           {dt,b0,H0,Gm0,R0});
0271   crts_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_CRTS(1:2,:)).^2)));
0272   ME_CRTS = squeeze(SP_CRTS(1,1,:)+SP_CRTS(2,2,:));
0273 
0274 
0275   SV_CRTS = zeros(size(m,1),size(Y,2));
0276   SE_CRTS = zeros(size(m,1),size(Y,2));
0277   for k=1:size(Y,2)
0278     SV_CRTS(:,k) = diag(SP_CRTS(:,:,k));
0279     SE_CRTS(:,k) = (X(:,k) - SM_CRTS(:,k)).^2;
0280   end
0281 
0282   fprintf('Done!\n');
0283   
0284 %% Visualize methods
0285   
0286   if ~silent
0287     aa = 0.02*(-1:0.1:4);
0288     cx = R0 * cos(aa);
0289     cy = R0 * sin(aa);
0290     % Plot EKF estimate
0291     plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
0292          MM_EKF(1,:),MM_EKF(2,:),'k--');
0293     legend('Radar','Earth','True','Estimate');
0294     title('Filtering result with EKF');
0295     disp(' ');
0296     disp('Filtering result with EKF is now displayed.');
0297     disp(' ');
0298     disp('<press any key to see the estimation error of x_1>');    
0299     pause;
0300   
0301     % Error for x_1 with EKF
0302     semilogy(T,EE_EKF(1,:),'g-',T,VV_EKF(1,:),'b--',...
0303          T,SE_ERTS(1,:),'r-',T,SV_ERTS(1,:),'k--');
0304     legend('EKF-RMSE','EKF-STDE',...
0305        'ERTS-RMSE1','ERTS-STDE1');  
0306     title('RMSE of estimating x_1 with EKF and ERTS')
0307     clc;
0308     disp('RMSE of estimating x_1 with EKF and ERTS is now displayed.');
0309     disp(' ');
0310     disp('<press any key to see the estimation error of x_5>');
0311     pause  
0312     
0313     % Error for x_5 with EKF
0314     semilogy(T,EE_EKF(5,:),'g-',T,VV_EKF(5,:),'b--',...
0315          T,SE_ERTS(5,:),'r-',T,SV_ERTS(5,:),'k--');
0316     legend('EKF-RMSE','EKF-STDE',...
0317        'ERTS-RMSE1','ERTS-STDE1');
0318     title('RMSE of estimating x_5 with EKF and ERTS')
0319     clc;
0320     disp('RMSE of estimating x_5 with EKF and ERTS is now displayed.');
0321     disp(' ');
0322     disp('<press any key to see the estimation error of x_5>');
0323     pause  
0324     
0325     % Plot UKF estimate
0326     plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
0327          MM_UKF(1,:),MM_UKF(2,:),'k--');
0328     legend('Radar','Earth','True','Estimate');
0329     clc;
0330     disp('Filtering result with UKF is now displayed.');
0331     disp(' ');
0332     disp('<press any key to see the estimation error of x_1>');    
0333     pause;
0334   
0335     % Error for x_1 with UKF
0336     semilogy(T,EE_UKF(1,:),'g-',T,VV_UKF(1,:),'b--',...
0337              T,SE_URTS(1,:),'r-',T,SV_URTS(1,:),'k--');
0338     legend('UKF-RMSE','UKF-STDE','URTS-RMSE','URTS-STDE');
0339     title('RMSE of estimating x_1 with UKF and URTS')    
0340     clc;
0341     disp('RMSE of estimating x_1 with EKF and ERTS is now displayed.');
0342     disp(' ');
0343     disp('<press any key to see the estimation error of x_5>');
0344     pause;
0345   
0346     % Error for x_5 with UKF
0347     semilogy(T,EE_UKF(5,:),'g-',T,VV_UKF(5,:),'b--',...
0348              T,SE_URTS(5,:),'r-',T,SV_URTS(5,:),'k--');
0349     legend('UKF-RMSE','UKF-STDE','URTS-RMSE','URTS-STDE');
0350     title('RMSE of estimating x_5 with UKF and URTS')
0351     clc;
0352     disp('RMSE of estimating x_5 with EKF and ERTS is now displayed.');
0353     
0354     
0355     % Plot GHKF estimate
0356     plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
0357          MM_GHKF(1,:),MM_GHKF(2,:),'k--');
0358     legend('Radar','Earth','True','Estimate');
0359     clc;
0360     disp('Filtering result with GHKF is now displayed.');
0361     disp(' ');
0362     disp('<press any key to see the estimation error of x_1>');    
0363     pause;
0364   
0365     % Error for x_1 with GHKF
0366     semilogy(T,EE_UKF(1,:),'g-',T,VV_UKF(1,:),'b--',...
0367              T,SE_URTS(1,:),'r-',T,SV_URTS(1,:),'k--');
0368     legend('GHKF-RMSE','GHKF-STDE','GHRTS-RMSE','GHRTS-STDE');
0369     title('RMSE of estimating x_1 with GHKF and GHRTS')    
0370     clc;
0371     disp('RMSE of estimating x_1 with GHKF and GHRTS is now displayed.');
0372     disp(' ');
0373     disp('<press any key to see the estimation error of x_5>');
0374     pause;
0375   
0376     % Error for x_5 with GHKF
0377     semilogy(T,EE_GHKF(5,:),'g-',T,VV_GHKF(5,:),'b--',...
0378              T,SE_GHRTS(5,:),'r-',T,SV_GHRTS(5,:),'k--');
0379     legend('GHKF-RMSE','GHKF-STDE','GHRTS-RMSE','GHRTS-STDE');
0380     title('RMSE of estimating x_5 with GHKF and GHRTS')
0381     clc;
0382     disp('RMSE of estimating x_5 with EKF and GHRTS is now displayed.');
0383     
0384     
0385     % Plot CKF estimate
0386     plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
0387          MM_CKF(1,:),MM_CKF(2,:),'k--');
0388     legend('Radar','Earth','True','Estimate');
0389     clc;
0390     disp('Filtering result with CKF is now displayed.');
0391     disp(' ');
0392     disp('<press any key to see the estimation error of x_1>');    
0393     pause;
0394   
0395     % Error for x_1 with CKF
0396     semilogy(T,EE_UKF(1,:),'g-',T,VV_UKF(1,:),'b--',...
0397              T,SE_URTS(1,:),'r-',T,SV_URTS(1,:),'k--');
0398     legend('CKF-RMSE','CKF-STDE','CRTS-RMSE','CRTS-STDE');
0399     title('RMSE of estimating x_1 with CKF and CRTS')    
0400     clc;
0401     disp('RMSE of estimating x_1 with CKF and CRTS is now displayed.');
0402     disp(' ');
0403     disp('<press any key to see the estimation error of x_5>');
0404     pause;
0405   
0406     % Error for x_5 with CKF
0407     semilogy(T,EE_CKF(5,:),'g-',T,VV_CKF(5,:),'b--',...
0408              T,SE_CRTS(5,:),'r-',T,SV_CRTS(5,:),'k--');
0409     legend('CKF-RMSE','CKF-STDE','CRTS-RMSE','CRTS-STDE');
0410     title('RMSE of estimating x_5 with CKF and CRTS')
0411     clc;
0412     disp('RMSE of estimating x_5 with EKF and CRTS is now displayed.');
0413 
0414     
0415   end
0416 
0417   
0418 %% Show RMSE errors for each method
0419   
0420   disp(' ');
0421   disp('RMS errors:');
0422   fprintf('EKF-RMSE   = %.6f [%.6f]\n',ekf_rmse,sqrt(mean(ME_EKF)));
0423   fprintf('ERTS-RMSE  = %.6f [%.6f]\n',eks_rmse1,sqrt(mean(ME_ERTS)));
0424   fprintf('ETF-RMSE   = %.6f [%.6f]\n',eks_rmse2,sqrt(mean(ME_ETF)));
0425   fprintf('UKF-RMSE   = %.6f [%.6f]\n',ukf_rmse,sqrt(mean(ME_UKF)));
0426   fprintf('URTS1-RMSE = %.6f [%.6f]\n',uks_rmse1,sqrt(mean(ME_URTS)));
0427   fprintf('URTS2-RMSE = %.6f [%.6f]\n',uks_rmse1b,sqrt(mean(ME_URTSb)));
0428   fprintf('UTF-RMSE   = %.6f [%.6f]\n',uks_rmse2,sqrt(mean(ME_UTF)));
0429 
0430   % Cubature methods
0431   fprintf('GHKF-RMSE  = %.6f [%.6f]\n',ghkf_rmse,sqrt(mean(ME_GHKF)));
0432   fprintf('GHRTS-RMSE = %.6f [%.6f]\n',ghrts_rmse1,sqrt(mean(ME_GHRTS)));
0433   fprintf('CKF-RMSE   = %.6f [%.6f]\n',ckf_rmse,sqrt(mean(ME_CKF)));
0434   fprintf('CRTS-RMSE  = %.6f [%.6f]\n',crts_rmse1,sqrt(mean(ME_CRTS)));
0435   
0436   
0437

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