function t = plots_TC04_hfb_3RPE(t, parFEM, Z, U, syekf, sxekf, sxekfstd,... syukf, sxukf, sxukfstd,... syukfAug, sxukfAug, sxukfAugstd, method); % Conversion factor radians to degrees r2d = 180/pi; XL = t(end) + 5; % Plot time histories of measured and estimated observation variables and control inputs figure(1) subplot(811),plot(t,Z(:,1), t,syekf(:,1),'r'); grid;ylabel('V (m/s)'); title('EKF: Time histories of output variables (measured and estimated); input variables') subplot(812),plot(t,Z(:,2)*r2d, t,syekf(:,2)*r2d,'r');grid;ylabel('\alpha (°)'); subplot(813),plot(t,Z(:,3)*r2d, t,syekf(:,3)*r2d,'r');grid;ylabel('\theta (°)'); subplot(814),plot(t,Z(:,4)*r2d, t,syekf(:,4)*r2d,'r');grid;ylabel('q (°/s)'); subplot(815),plot(t,Z(:,5)*r2d, t,syekf(:,5)*r2d,'r');grid;ylabel('qp (°/s2)'); subplot(816),plot(t,Z(:,6), t,syekf(:,6),'r'); grid;ylabel('ax (m/s2)'); subplot(817),plot(t,Z(:,7), t,syekf(:,7),'r'); grid;ylabel('az (m/s2)'); subplot(818),plot(t,U(:,1)*r2d); grid;ylabel('{\delta_e} (°)'); xlabel('Time in sec'); % UKF: Plot time histories of measured and estimated observation variables and inputs figure(2) subplot(811),plot(t,Z(:,1), t,syukf(:,1),'r'); grid;ylabel('V (m/s)'); title('UKF: Time histories of output variables (measured and estimated); input variables') subplot(812),plot(t,Z(:,2)*r2d, t,syukf(:,2)*r2d,'r');grid;ylabel('\alpha (°)'); subplot(813),plot(t,Z(:,3)*r2d, t,syukf(:,3)*r2d,'r');grid;ylabel('\theta (°)'); subplot(814),plot(t,Z(:,4)*r2d, t,syukf(:,4)*r2d,'r');grid;ylabel('q (°/s)'); subplot(815),plot(t,Z(:,5)*r2d, t,syukf(:,5)*r2d,'r');grid;ylabel('qp (°/s2)'); subplot(816),plot(t,Z(:,6), t,syukf(:,6),'r'); grid;ylabel('ax (m/s2)'); subplot(817),plot(t,Z(:,7), t,syukf(:,7),'r'); grid;ylabel('az (m/s2)'); subplot(818),plot(t,U(:,1)*r2d); grid;ylabel('{\delta_e} (°)'); xlabel('Time in sec'); % UKFaugemted: Plot time histories of measured and estimated observation variables and inputs figure(3) subplot(811),plot(t,Z(:,1), t,syukfAug(:,1),'r'); grid;ylabel('V (m/s)'); title('UKFaug: Time histories of output variables (measured and estimated); input variables') subplot(812),plot(t,Z(:,2)*r2d, t,syukfAug(:,2)*r2d,'r');grid;ylabel('\alpha (°)'); subplot(813),plot(t,Z(:,3)*r2d, t,syukfAug(:,3)*r2d,'r');grid;ylabel('\theta (°)'); subplot(814),plot(t,Z(:,4)*r2d, t,syukfAug(:,4)*r2d,'r');grid;ylabel('q (°/s)'); subplot(815),plot(t,Z(:,5)*r2d, t,syukfAug(:,5)*r2d,'r');grid;ylabel('qp (°/s2)'); subplot(816),plot(t,Z(:,6), t,syukfAug(:,6),'r'); grid;ylabel('ax (m/s2)'); subplot(817),plot(t,Z(:,7), t,syukfAug(:,7),'r'); grid;ylabel('az (m/s2)'); subplot(818),plot(t,U(:,1)*r2d); grid;ylabel('{\delta_e} (°)'); xlabel('Time in sec'); disp(' ') disp('The time history plots for the model output from UKF and UKFaug show large') disp('deviations for the first data point. This is because, the initial p0 specified') disp('in mDEfCase04.m is too large for the system states or parameters.') disp('The states V, al, theta, q are in m/s, rad, rad, rad/s respectively. The initial') disp('p0 of ten for all them is not realistic.') disp('Same starting p0 is used for convenience and to enable comparison of performance') disp('starting from the same initial conditions.') disp('This problem, which is specific to UKF only, results from the Sigma points.') disp('The Sigma points are computed based on initially specified xa0 and p0.') disp('For the subsequent data points the Sigma points are generated using the updated') disp('covariances, which are computed by the UKF-algorithm, and are more realistic.') disp('For more details see Section 7.3.2 and 7.5.4.') % EKF: Plot time histories of estimated parameters figure(4) subplot(611),plot(t,sxekf(:,5),'k', t,sxekf(:,5)+sxekfstd(:,5),'r',... t,sxekf(:,5)-sxekfstd(:,5),'r',... [0 XL],[parFEM(1,1) parFEM(1,1)]);grid;ylabel('CD_0'); legend('x','x+\sigma','x-\sigma','FEM',1) title('EKF: Convergence of parameter estimates with error bounds') subplot(612),plot(t,sxekf(:,6),'k', t,sxekf(:,6)+sxekfstd(:,6),'r',... t,sxekf(:,6)-sxekfstd(:,6),'r',... [0 XL],[parFEM(1,2) parFEM(1,2)]);grid;ylabel('CD_V'); subplot(613),plot(t,sxekf(:,7),'k', t,sxekf(:,7)+sxekfstd(:,7),'r',... t,sxekf(:,7)-sxekfstd(:,7),'r',... [0 XL],[parFEM(1,3) parFEM(1,3)]);grid;ylabel('CD_{\alpha}'); subplot(614),plot(t,sxekf(:,8),'k', t,sxekf(:,8)+sxekfstd(:,8),'r',... t,sxekf(:,8)-sxekfstd(:,8),'r',... [0 XL],[parFEM(1,4) parFEM(1,4)]);grid;ylabel('CL_0'); subplot(615),plot(t,sxekf(:,9),'k', t,sxekf(:,9)+sxekfstd(:,9),'r',... t,sxekf(:,9)-sxekfstd(:,9),'r',... [0 XL],[parFEM(1,5) parFEM(1,5)]);grid;ylabel('CL_V'); subplot(616),plot(t,sxekf(:,10),'k', t,sxekf(:,10)+sxekfstd(:,10),'r',... t,sxekf(:,10)-sxekfstd(:,10),'r',... [0 XL],[parFEM(1,6) parFEM(1,6)]);grid;ylabel('CL_{\alpha}'); figure(5) subplot(511),plot(t,sxekf(:,11),'k', t,sxekf(:,11)+sxekfstd(:,11),'r',... t,sxekf(:,11)-sxekfstd(:,11),'r',... [0 XL],[parFEM(1,7) parFEM(1,7)]);grid;ylabel('Cm_0'); legend('x','x+\sigma','x-\sigma','FEM',1) title('EKF: Convergence of parameter estimates with error bounds') subplot(512),plot(t,sxekf(:,12),'k', t,sxekf(:,12)+sxekfstd(:,12),'r',... t,sxekf(:,12)-sxekfstd(:,12),'r',... [0 XL],[parFEM(1,8) parFEM(1,8)]);grid;ylabel('Cm_V'); subplot(513),plot(t,sxekf(:,13),'k', t,sxekf(:,13)+sxekfstd(:,13),'r',... t,sxekf(:,13)-sxekfstd(:,13),'r',... [0 XL],[parFEM(1,9) parFEM(1,9)]);grid;ylabel('Cm_{\alpha}'); subplot(514),plot(t,sxekf(:,14),'k', t,sxekf(:,14)+sxekfstd(:,14),'r',... t,sxekf(:,14)-sxekfstd(:,14),'r',... [0 XL],[parFEM(1,10) parFEM(1,10)]);grid;ylabel('Cm_q'); subplot(515),plot(t,sxekf(:,15),'k', t,sxekf(:,15)+sxekfstd(:,15),'r',... t,sxekf(:,15)-sxekfstd(:,15),'r',... [0 XL],[parFEM(1,11) parFEM(1,11)]);grid;ylabel('Cm_{\deltae}'); % UKF: Plot time histories of estimated parameters figure(6) subplot(611),plot(t,sxukf(:,5),'k', t,sxukf(:,5)+sxukfstd(:,5),'r',... t,sxukf(:,5)-sxukfstd(:,5),'r',... [0 XL],[parFEM(1,1) parFEM(1,1)]);grid;ylabel('CD_0'); legend('x','x+\sigma','x-\sigma','FEM',1) title('UKF: Convergence of parameter estimates with error bounds') subplot(612),plot(t,sxukf(:,6),'k', t,sxukf(:,6)+sxukfstd(:,6),'r',... t,sxukf(:,6)-sxukfstd(:,6),'r',... [0 XL],[parFEM(1,2) parFEM(1,2)]);grid;ylabel('CD_V'); subplot(613),plot(t,sxukf(:,7),'k', t,sxukf(:,7)+sxukfstd(:,7),'r',... t,sxukf(:,7)-sxukfstd(:,7),'r',... [0 XL],[parFEM(1,3) parFEM(1,3)]);grid;ylabel('CD_{\alpha}'); subplot(614),plot(t,sxukf(:,8),'k', t,sxukf(:,8)+sxukfstd(:,8),'r',... t,sxukf(:,8)-sxukfstd(:,8),'r',... [0 XL],[parFEM(1,4) parFEM(1,4)]);grid;ylabel('CL_0'); subplot(615),plot(t,sxukf(:,9),'k', t,sxukf(:,9)+sxukfstd(:,9),'r',... t,sxukf(:,9)-sxukfstd(:,9),'r',... [0 XL],[parFEM(1,5) parFEM(1,5)]);grid;ylabel('CL_V'); subplot(616),plot(t,sxukf(:,10),'k', t,sxukf(:,10)+sxukfstd(:,10),'r',... t,sxukf(:,10)-sxukfstd(:,10),'r',... [0 XL],[parFEM(1,6) parFEM(1,6)]);grid;ylabel('CL_{\alpha}'); figure(7) subplot(511),plot(t,sxukf(:,11),'k', t,sxukf(:,11)+sxukfstd(:,11),'r',... t,sxukf(:,11)-sxukfstd(:,11),'r',... [0 XL],[parFEM(1,7) parFEM(1,7)]);grid;ylabel('Cm_0'); legend('x','x+\sigma','x-\sigma','FEM',1) title('UKF: Convergence of parameter estimates with error bounds') subplot(512),plot(t,sxukf(:,12),'k', t,sxukf(:,12)+sxukfstd(:,12),'r',... t,sxukf(:,12)-sxukfstd(:,12),'r',... [0 XL],[parFEM(1,8) parFEM(1,8)]);grid;ylabel('Cm_V'); subplot(513),plot(t,sxukf(:,13),'k', t,sxukf(:,13)+sxukfstd(:,13),'r',... t,sxukf(:,13)-sxukfstd(:,13),'r',... [0 XL],[parFEM(1,9) parFEM(1,9)]);grid;ylabel('Cm_{\alpha}'); subplot(514),plot(t,sxukf(:,14),'k', t,sxukf(:,14)+sxukfstd(:,14),'r',... t,sxukf(:,14)-sxukfstd(:,14),'r',... [0 XL],[parFEM(1,10) parFEM(1,10)]);grid;ylabel('Cm_q'); subplot(515),plot(t,sxukf(:,15),'k', t,sxukf(:,15)+sxukfstd(:,15),'r',... t,sxukf(:,15)-sxukfstd(:,15),'r',... [0 XL],[parFEM(1,11) parFEM(1,11)]);grid;ylabel('Cm_{\deltae}'); % UKFaug: Plot time histories of estimated parameters figure(8) subplot(611),plot(t,sxukfAug(:,5),'k', t,sxukfAug(:,5)+sxukfAugstd(:,5),'r',... t,sxukfAug(:,5)-sxukfAugstd(:,5),'r',... [0 XL],[parFEM(1,1) parFEM(1,1)]);grid;ylabel('CD_0'); legend('x','x+\sigma','x-\sigma','FEM',1) title('UKFaugmented: Convergence of parameter estimates with error bounds') subplot(612),plot(t,sxukfAug(:,6),'k', t,sxukfAug(:,6)+sxukfAugstd(:,6),'r',... t,sxukfAug(:,6)-sxukfAugstd(:,6),'r',... [0 XL],[parFEM(1,2) parFEM(1,2)]);grid;ylabel('CD_V'); subplot(613),plot(t,sxukfAug(:,7),'k', t,sxukfAug(:,7)+sxukfAugstd(:,7),'r',... t,sxukfAug(:,7)-sxukfAugstd(:,7),'r',... [0 XL],[parFEM(1,3) parFEM(1,3)]);grid;ylabel('CD_{\alpha}'); subplot(614),plot(t,sxukfAug(:,8),'k', t,sxukfAug(:,8)+sxukfAugstd(:,8),'r',... t,sxukfAug(:,8)-sxukfAugstd(:,8),'r',... [0 XL],[parFEM(1,4) parFEM(1,4)]);grid;ylabel('CL_0'); subplot(615),plot(t,sxukfAug(:,9),'k', t,sxukfAug(:,9)+sxukfAugstd(:,9),'r',... t,sxukfAug(:,9)-sxukfAugstd(:,9),'r',... [0 XL],[parFEM(1,5) parFEM(1,5)]);grid;ylabel('CL_V'); subplot(616),plot(t,sxukfAug(:,10),'k', t,sxukfAug(:,10)+sxukfAugstd(:,10),'r',... t,sxukfAug(:,10)-sxukfAugstd(:,10),'r',... [0 XL],[parFEM(1,6) parFEM(1,6)]);grid;ylabel('CL_{\alpha}'); figure(9) subplot(511),plot(t,sxukfAug(:,11),'k', t,sxukfAug(:,11)+sxukfAugstd(:,11),'r',... t,sxukfAug(:,11)-sxukfAugstd(:,11),'r',... [0 XL],[parFEM(1,7) parFEM(1,7)]);grid;ylabel('Cm_0'); legend('x','x+\sigma','x-\sigma','FEM',1) title('UKFaugmented: Convergence of parameter estimates with error bounds') subplot(512),plot(t,sxukfAug(:,12),'k', t,sxukfAug(:,12)+sxukfAugstd(:,12),'r',... t,sxukfAug(:,12)-sxukfAugstd(:,12),'r',... [0 XL],[parFEM(1,8) parFEM(1,8)]);grid;ylabel('Cm_V'); subplot(513),plot(t,sxukfAug(:,13),'k', t,sxukfAug(:,13)+sxukfAugstd(:,13),'r',... t,sxukfAug(:,13)-sxukfAugstd(:,13),'r',... [0 XL],[parFEM(1,9) parFEM(1,9)]);grid;ylabel('Cm_{\alpha}'); subplot(514),plot(t,sxukfAug(:,14),'k', t,sxukfAug(:,14)+sxukfAugstd(:,14),'r',... t,sxukfAug(:,14)-sxukfAugstd(:,14),'r',... [0 XL],[parFEM(1,10) parFEM(1,10)]);grid;ylabel('Cm_q'); subplot(515),plot(t,sxukfAug(:,15),'k', t,sxukfAug(:,15)+sxukfAugstd(:,15),'r',... t,sxukfAug(:,15)-sxukfAugstd(:,15),'r',... [0 XL],[parFEM(1,11) parFEM(1,11)]);grid;ylabel('Cm_{\deltae}'); % Plot time histories of estimated parameters figure(10) subplot(321),plot(t,sxekf(:,5),'b', t,sxukf(:,5),'r',t,sxukfAug(:,5),'m',... [0 XL],[parFEM(1,1) parFEM(1,1)]);grid;... legend('CD_0 EKF',' UKF',' UKFaug',' FEM',1); title('EKF, UKF, UKFaug: Convergence of parameter estimates') subplot(323),plot(t,sxekf(:,6),'b', t,sxukf(:,6),'r',t,sxukfAug(:,6),'m',... [0 XL],[parFEM(1,2) parFEM(1,2)]);grid;... legend('CD_V EKF',' UKF',' UKFaug',' FEM',1); subplot(325),plot(t,sxekf(:,7),'b', t,sxukf(:,7),'r',t,sxukfAug(:,7),'m',... [0 XL],[parFEM(1,3) parFEM(1,3)]);grid;... legend('CD_{\alpha} EKF',' UKF',' UKFaug',' FEM',1);... xlabel('Time in sec') subplot(322),plot(t,sxekf(:,8),'b', t,sxukf(:,8),'r',t,sxukfAug(:,8),'m',... [0 XL],[parFEM(1,4) parFEM(1,4)]);grid;... legend('CL_0 EKF',' UKF',' UKFaug',' FEM',1); subplot(324),plot(t,sxekf(:,9),'b', t,sxukf(:,9),'r',t,sxukfAug(:,9),'m',... [0 XL],[parFEM(1,5) parFEM(1,5)]);grid;... legend('CL_V EKF',' UKF',' UKFaug',' FEM',1); subplot(326),plot(t,sxekf(:,10),'b', t,sxukf(:,10),'r',t,sxukfAug(:,10),'m',... [0 XL],[parFEM(1,6) parFEM(1,6)]);grid;... legend('CL_{\alpha} EKF',' UKF',' UKFaug',' FEM',1);... xlabel('Time in sec') figure(11) subplot(321),plot(t,sxekf(:,11),'b', t,sxukf(:,11),'r',t,sxukfAug(:,11),'m',... [0 XL],[parFEM(1,7) parFEM(1,7)]);grid;... legend('Cm_0 EKF',' UKF',' UKFaug',' FEM',1); title('EKF, UKF, UKFaug: Convergence of parameter estimates') subplot(323),plot(t,sxekf(:,12),'b', t,sxukf(:,12),'r',t,sxukfAug(:,12),'m',... [0 XL],[parFEM(1,8) parFEM(1,8)]);grid;... legend('Cm_V EKF',' UKF',' UKFaug',' FEM',1); subplot(325),plot(t,sxekf(:,13),'b', t,sxukf(:,13),'r',t,sxukfAug(:,13),'m',... [0 XL],[parFEM(1,9) parFEM(1,9)]);grid;... legend('Cm_{\alpha} EKF',' UKF',' UKFaug',' FEM',1);... xlabel('Time in sec') subplot(322),plot(t,sxekf(:,14),'b', t,sxukf(:,14),'r',t,sxukfAug(:,14),'m',... [0 XL],[parFEM(1,10) parFEM(1,10)]);grid;... legend('Cm_q EKF',' UKF',' UKFaug',' FEM',1); subplot(324),plot(t,sxekf(:,15),'b', t,sxukf(:,15),'r',t,sxukfAug(:,15),'m',... [0 XL],[parFEM(1,11) parFEM(1,11)]);grid;... legend('Cm_{\deltae} EKF',' UKF',' UKFaug',' FEM',1); xlabel('Time in sec') % Figure 7.6 of chapter 7: figure(12) subplot(321),plot(t,sxekf(:,8),'b', t,sxukf(:,8),'r',t,sxukfAug(:,8),'m',... [0 XL],[parFEM(1,4) parFEM(1,4)]);grid;... legend('CL_0 EKF',' UKF',' UKFaug',' FEM',1); title('EKF, UKF, UKFaug: Convergence of parameter estimates') subplot(323),plot(t,sxekf(:,11),'b', t,sxukf(:,11),'r',t,sxukfAug(:,11),'m',... [0 XL],[parFEM(1,7) parFEM(1,7)]);grid;... legend('Cm_0 EKF',' UKF',' UKFaug',' FEM',1); subplot(325),plot(t,sxekf(:,14),'b', t,sxukf(:,14),'r',t,sxukfAug(:,14),'m',... [0 XL],[parFEM(1,10) parFEM(1,10)]);grid;... legend('Cm_q EKF',' UKF',' UKFaug',' FEM',1); xlabel('Time in sec') subplot(322),plot(t,sxekf(:,10),'b', t,sxukf(:,10),'r',t,sxukfAug(:,10),'m',... [0 XL],[parFEM(1,6) parFEM(1,6)]);grid;... legend('CL_{\alpha} EKF',' UKF',' UKFaug',' FEM',1);... subplot(324),plot(t,sxekf(:,13),'b', t,sxukf(:,13),'r',t,sxukfAug(:,13),'m',... [0 XL],[parFEM(1,9) parFEM(1,9)]);grid;... legend('Cm_{\alpha} EKF',' UKF',' UKFaug',' FEM',1);... subplot(326),plot(t,sxekf(:,15),'b', t,sxukf(:,15),'r',t,sxukfAug(:,15),'m',... [0 XL],[parFEM(1,11) parFEM(1,11)]);grid;... legend('Cm_{\deltae} EKF',' UKF',' UKFaug',' FEM',1); xlabel('Time in sec')