Home > src > demos > ekf_sine_demo > ekf_sine_demo.m

ekf_sine_demo

PURPOSE ^

Demonstration for EKF using a random sine signal model.

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

 Demonstration for EKF using a random sine signal model. 

 A Very simple demonstration for extended Kalman filter (EKF), which is
 used to track a random single-component sinusoid signal,
 which is modelled as x_k = a_k*sin(\theta_k), dtheta/dt = omega_k.
 The signal is also filtered with unscented Kalman filter (UKF) for
 comparison.

 Copyright (C) 2007 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 % Demonstration for EKF using a random sine signal model.
0002 %
0003 % A Very simple demonstration for extended Kalman filter (EKF), which is
0004 % used to track a random single-component sinusoid signal,
0005 % which is modelled as x_k = a_k*sin(\theta_k), dtheta/dt = omega_k.
0006 % The signal is also filtered with unscented Kalman filter (UKF) for
0007 % comparison.
0008 %
0009 % Copyright (C) 2007 Jouni Hartikainen
0010 %
0011 % This software is distributed under the GNU General Public
0012 % Licence (version 2 or later); please refer to the file
0013 % Licence.txt, included with the software, for details.
0014 
0015 clc;
0016 disp('Filtering the signal with EKF...');
0017 
0018 save_plots = 1;
0019 
0020 % Measurement model and it's derivative
0021 f_func = @ekf_sine_f;
0022 h_func = @ekf_sine_h;
0023 dh_dx_func = @ekf_sine_dh_dx;
0024 d2h_dx2_func = @ekf_sine_d2h_dx2;
0025 
0026 % Initial values for the signal.
0027 f = 0;
0028 w = 10;
0029 a = 1;
0030   
0031 % Number of samples and stepsize.
0032 d = 5;
0033 n = 500;
0034 dt = d/n;
0035 x = 1:n;
0036 
0037 % Check the derivative of the measurement function.
0038 der_check(h_func, dh_dx_func, 1, [f w a]');
0039 
0040 % Dynamic state transition matrix in continous-time domain.
0041 F = [0 1 0;
0042      0 0 0;
0043      0 0 0];
0044   
0045 % Noise effect matrix in continous-time domain.
0046 L = [0 0;
0047      1 0;
0048      0 1];
0049   
0050 % Spectral power density of the white noise.
0051 q1 = 0.2;
0052 q2 = 0.1;
0053 Qc = diag([q1 q2]);
0054   
0055 % Discretize the plant equation.
0056 [A,Q] = lti_disc(F,L,Qc,dt);
0057   
0058 % Generate the real signal.
0059 X = zeros(3, n);
0060 X(:,1) = [f w a]';
0061 for i = 2:n
0062    X(:,i) = A*X(:,i-1) + gauss_rnd([0 0 0]', Q);
0063 end  
0064   
0065 % Generate the observations with Gaussian noise.
0066 sd = 1;
0067 R = sd^2;
0068 
0069 Y = zeros(1,n);
0070 Y_real = feval(h_func,X);     
0071 Y = Y_real + gauss_rnd(0,R,n);
0072   
0073 plot(x,Y,'.',x,Y_real)
0074   
0075 % Initial guesses for the state mean and covariance.
0076 M = [f w a]';
0077 P = diag([3 3 3]);    
0078   
0079 % Reserve space for estimates.
0080 MM = zeros(size(M,1),size(Y,2));
0081 PP = zeros(size(M,1),size(M,1),size(Y,2));
0082 
0083 % Estimate with EKF
0084 for k=1:size(Y,2)
0085    [M,P] = ekf_predict1(M,P,A,Q);
0086    [M,P] = ekf_update1(M,P,Y(:,k),dh_dx_func,R*eye(1),h_func);
0087    MM(:,k)   = M;
0088    PP(:,:,k) = P;
0089 end
0090 
0091 % Initial guesses for the state mean and covariance.
0092 M = [f w a]';
0093 P = diag([3 3 3]);    
0094   
0095 % Reserve space for estimates.
0096 MM2 = zeros(size(M,1),size(Y,2));
0097 PP2 = zeros(size(M,1),size(M,1),size(Y,2));
0098 
0099 % Estimate with EKF
0100 for k=1:size(Y,2)
0101    [M,P] = ekf_predict1(M,P,A,Q);
0102    [M,P] = ekf_update2(M,P,Y(:,k),dh_dx_func,d2h_dx2_func,R*eye(1),h_func);
0103    MM2(:,k)   = M;
0104    PP2(:,:,k) = P;
0105 end
0106 
0107 clf; clc;
0108 disp('The filtering results using the 1st order EKF is now displayed')
0109 
0110 % Project the estimates to measurement space
0111 Y_m = feval(h_func, MM);
0112 Y_m2 = feval(h_func, MM2);
0113 plot(x,Y,'.', x,Y_real,'--',x,Y_m);
0114 legend('Measurements','Real signal', 'Filtered estimate');
0115 xlim([0 ceil(max(x))]);
0116 title('Estimating a random Sine signal with extended Kalman filter.');
0117 
0118 if save_plots
0119     print -dpsc demo2_f1.ps 
0120 end
0121 
0122 clc;
0123 disp('The filtering result using the 1st order EKF is now displayed.');
0124 fprintf('Smoothing the estimates using the RTS smoother...');
0125 
0126 [SM1,SP1] = erts_smooth1(MM,PP,A,Q);
0127 
0128 [SM2,SP2] = etf_smooth1(MM,PP,Y,A,Q,[],[],[],...
0129                          dh_dx_func,R*eye(1),h_func);  
0130 
0131 [SM1_2,SP1_2] = erts_smooth1(MM2,PP2,A,Q);
0132 
0133 [SM2_2,SP2_2] = etf_smooth1(MM2,PP2,Y,A,Q,[],[],[],...
0134                          dh_dx_func,R*eye(1),h_func);  
0135 
0136 fprintf('ready.\n');
0137 disp(' ');
0138 disp('Push any button to display the smoothing results.');
0139 pause
0140 
0141 Y_s1 = feval(h_func, SM1);
0142 plot(x,Y,'.', x, Y_real,'--',x,Y_s1);
0143 legend('Measurements','Real signal','Smoothed estimate');
0144 xlim([0 ceil(max(x))]);
0145 title(['Smoothing a random Sine signal with extended ',...
0146        'Kalman (RTS) smoother.']);
0147 
0148 if save_plots
0149     print -dpsc demo2_f2.ps
0150 end
0151 
0152 clc;
0153 disp('The smoothing results using the ERTS smoother is now displayed.');
0154 disp(' ');
0155 disp('Push any button to see the smoothing results of a ETF smoother.');
0156 pause
0157   
0158 Y_s2 = feval(h_func, SM2);
0159 plot(x,Y,'.', x, Y_real,'--',x,Y_s2);
0160 legend('Measurements','Real signal','Smoothed estimate');
0161 title(['Smoothing a random Sine signal with extended ',...
0162        'Kalman (Two Filter) smoother.']);
0163 xlim([0 ceil(max(x))]);
0164 
0165 if save_plots
0166     print -dpsc demo2_f3.ps
0167 end
0168 
0169 clc;
0170 disp('The smoothing results using the ETF smoother is now displayed.');
0171 
0172 
0173 Y_s1_2 = feval(h_func, SM1_2);
0174 Y_s2_2 = feval(h_func, SM2_2);
0175 % Errors.
0176 EMM_Y = sum((Y_m-Y_real).^2)/n;
0177 EMM_Y2 = sum((Y_m2-Y_real).^2)/n;
0178 ESM1_Y = sum((Y_s1-Y_real).^2)/n;
0179 ESM2_Y = sum((Y_s2-Y_real).^2)/n;
0180 ESM1_2_Y = sum((Y_s1_2-Y_real).^2)/n;
0181 ESM2_2_Y = sum((Y_s2_2-Y_real).^2)/n;
0182   
0183 disp(' ');
0184 fprintf('Filtering now with UKF...');
0185 
0186 % In the rest the signal is filtered with UKF for comparison.
0187 
0188 % Initial guesses for the state mean and covariance.
0189 M = [f w a]';
0190 P = diag([3 3 3]);    
0191   
0192 % Reserve space for estimates.
0193 U_MM = zeros(size(M,1),size(Y,2));
0194 U_PP = zeros(size(M,1),size(M,1),size(Y,2));
0195 
0196 % Estimate with UKF
0197 for k=1:size(Y,2)
0198    [M,P,X_s,w] = ukf_predict3(M,P,f_func,Q,R*eye(1),dt);
0199    [M,P] = ukf_update3(M,P,Y(:,k),h_func,R*eye(1),X_s,w,[]);
0200    U_MM(:,k)   = M;
0201    U_PP(:,:,k) = P;
0202 end
0203 [U_SM, U_SP] = urts_smooth1(U_MM,U_PP,f_func,Q,dt);
0204 
0205 fprintf('ready.\n')
0206 disp(' ');
0207 disp('Push any button to see the filtering results.');
0208 pause
0209 
0210 Y_m_u = feval(h_func, U_MM);
0211 plot(x,Y,'.', x, Y_real,'--',x,Y_m_u);
0212 legend('Measurements','Real signal', 'Filtered estimate');
0213 xlim([0 ceil(max(x))]);
0214 title('Estimating a random Sine signal with unscented Kalman filter.');
0215 
0216 clc;
0217 disp('The filtering results of a UKF are now displayed.')
0218 disp(' ');
0219 disp('Push any button to display the smoothing results.');
0220 pause
0221   
0222 Y_m_su = feval(h_func, U_SM);
0223 plot(x,Y,'.', x, Y_real,'--',x,Y_m_su);
0224 legend('Measurements','Real signal', 'Filtered estimate');
0225 xlim([0 ceil(max(x))]);
0226 title('Estimating a random Sine signal with unscented Kalman smoother (RTS).');
0227 
0228 clc;
0229 disp('The smoothing results of a ERTS smoother are now displayed');
0230 
0231 UKF_EMM_Y = sum((Y_m_u-Y_real).^2)/n;
0232 URTS_EMM_Y = sum((Y_m_su-Y_real).^2)/n;
0233 
0234 disp(' ');
0235 disp('Mean square errors of all estimates:');
0236 fprintf('EKF1-MSE = %.4f\n',sqrt(EMM_Y));
0237 fprintf('ERTS-MSE = %.4f\n',sqrt(ESM1_Y));
0238 fprintf('ETF-MSE = %.4f\n',sqrt(ESM2_Y));
0239 fprintf('EKF2-MSE = %.4f\n',sqrt(EMM_Y2));
0240 fprintf('ERTS2-MSE = %.4f\n',sqrt(ESM1_2_Y));
0241 fprintf('ETF2-MSE = %.4f\n',sqrt(ESM2_2_Y));
0242 fprintf('UKF-RMSE = %.4f\n',sqrt(UKF_EMM_Y));
0243 fprintf('URTS-RMSE = %.4f\n',sqrt(URTS_EMM_Y));
0244

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