Friday 31 May 2019

Extended Kalman Filter, Alternative Version

Below is alternative code for an Extended Kalman filter for a sine wave, which has 4 states: the sine wave value, the phase, the angular frequency and amplitude and measurements thereof. I have found it necessary to implement this version because I couldn't adjust my earlier version code to accept and measure the additional states without the Cholesky decomposition function chol() exiting and giving errors about the input not being a positive definite matrix.
clear all ;
1 ;

function Y = ekf_sine_h( x )
% Measurement model function for the sine signal.
% Takes the state input vector x of sine, phase, angular frequency and amplitude and 
% calculates the current value of the sine given the state vector values.

f = x( 2 , : ) ;    % phase value in radians
a = x( 4 , : ) ;    % amplitude
Y = a .* sin( f ) ; % the sine value

endfunction

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
pkg load statistics ;
load all_snr ;
load all_hmm_periods_daily ;

N = 250 ; % total dynamic steps i.e.length of data to generate
[ gen_period , gen_states ] = hmmgenerate( N , transprobest , outprobest ) ;
gen_period = gen_period .+ hmm_min_period_add ;
angular_freq_rads = ( 2 * pi ) ./ gen_period ;
real_phase = cumsum( angular_freq_rads ) ; real_phase = mod( real_phase , 2 * pi ) ;
gen_sine = sin( real_phase ) ;

noise_val = mean( [ all_snr(:,1) ; all_snr(:,2) ] ) ;
my_sine_noisy = awgn( gen_sine , noise_val ) ;

[ ~ , ~ , ~ , ~ , ~ , ~ , phase ] = sinewave_indicator( my_sine_noisy ) ;
phase = deg2rad( phase' ) ;
period = autocorrelation_periodogram( my_sine_noisy ) ;
measured_angular_frequency = ( 2 * pi ) ./ period' ;
measured_angular_frequency( 1 : 50 ) = 2 * pi / 20 ;

% for for amplitude
measured_amplitude = ones( 1 , N ) ;
for ii = 50 : N
  measured_amplitude( ii ) = sqrt( 2 ) * sqrt( mean( my_sine_noisy( ii - period(ii) : ii ).^2 ) ) ;  
endfor

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h = @ekf_sine_h ; 

n = 4 ; % number of states - sine value, phase, angular frequency and amplitude

% matrix for covariance of process
Q = eye( n ) ; 
Q( 1 , 1 ) = var( my_sine_noisy ) ;
Q( 2 , 2 ) = var( phase( 50 : end ) ) ;
Q( 3 , 3 ) = var( measured_angular_frequency( 50 : end ) ) ;
Q( 4 , 4 ) = var( measured_amplitude( 50 : end ) ) ;
% scale the covariance matrix, a tunable factor?
Q = 0.001 .* Q ; 

% measurement noise variance matrix
R = zeros( 4 , 4 ) ; 
R( 1 , 1 ) = var( gen_sine .- my_sine_noisy ) ;                                               % the sine values
R( 2 , 2 ) = var( real_phase( 50 : end ) .- phase( 50 : end ) ) ;                             % the phase values
R( 3 , 3 ) = var( angular_freq_rads( 50 : end ) .- measured_angular_frequency( 50 : end ) ) ; % angular frequency values
R( 4 , 4 ) = var( measured_amplitude( 50 : end ) .- 1 ) ;                                     % amplitude values

% initial state from noisy measurements
s = [ my_sine_noisy( 50 ) ;              % sine
      phase( 50 ) ;                      % phase ( Rads )
      measured_angular_frequency( 50 ) ; % angular_freq_rads
      measured_amplitude( 50 ) ] ;       % amplitude               
 
% initial state covariance 
P = eye( n ) ; 
 
% allocate memory
xV = zeros( n , N ) ;  % record estmates of states        
pV1 = zeros( n , N ) ; % projection measurments
pV2 = zeros( n , N ) ; % projection measurments
pV3 = zeros( n , N ) ; % projection measurments

% basic Jacobian of state transition matrix 
A = [ 0 0 0 0 ;   % sine value
      0 1 1 0 ;   % phase
      0 0 1 0 ;   % angular frequency
      0 0 0 1 ] ; % amplitude
      
H = eye( n ) ; % measurement matrix
      
for k = 51 : N

% do ekf
% nonlinear update x and linearisation at current state s
x = s ;
x( 2 ) = x( 2 ) + x( 3 ) ;        % advance phase value by angular frequency
x( 2 ) = mod( x( 2 ) , 2 * pi ) ; % limit phase values to range 0 --- 2 * pi
x( 1 ) = x( 4 ) * sin( x( 2 ) ) ; % sine value calculation

% update the 1st row of the jacobian matrix at state vector s values
A( 1 , : ) = [ 0 s( 4 ) * cos( s( 2 ) ) 0 sin( s( 2 ) ) ] ;

P = A * P * A' + Q ; % state transition model update of covariance matrix P

measurement_residual = [ my_sine_noisy( k ) - x( 1 ) ;              % sine residual
                         phase( k ) - x( 2 ) ;                      % phase residual
                         measured_angular_frequency( k ) - x( 3 ) ; % angular frequency residual
                         measured_amplitude( k ) - x( 4 ) ] ;       % amplitude residual
                         
innovation_residual_covariance = H * P * H' + R ;
kalman_gain = P * H' / innovation_residual_covariance ;

% update the state vector s with kalman_gain 
s = x + kalman_gain * measurement_residual ;

% some reality based post hoc adjustments
s( 2 ) = abs( s( 2 ) ) ; % no negative phase values
s( 3 ) = abs( s( 3 ) ) ; % no negative angular frequencies
s( 4 ) = abs( s( 4 ) ) ; % no negative amplitudes

% update the state covariance matrix P
% NOTE
% The Joseph formula is given by P+ = ( I − KH ) P− ( I − KH )' + KRK', where I is the identity matrix,
% K is the gain, H is the measurement mapping matrix, R is the measurement noise covariance matrix, 
% and P−, P+ are the pre and post measurement update estimation error covariance matrices, respectively.  
% The optimal linear unbiased estimator (equivalently the optimal linear minimum mean square error estimator)  
% or Kalman filter often utilizes simplified covariance update equations such as P+ = (I−KH)P− and P+ = P− −K(HP−H'+R)K'.  
% While these alternative formulations require fewer computations than the Joseph formula, they are only valid 
% when K is chosen as the optimal Kalman gain. In engineering applications, situations arise where the optimal 
% Kalman gain is not utilized and the Joseph formula must be employed to update the estimation error covariance.  
% Two examples of such a scenario are underweighting measurements and considering states. 
% Even when the optimal gain is used, the Joseph formulation is still preferable because it possesses 
% greater numerical accuracy than the simplified equations.
P = ( eye( n ) - kalman_gain * H ) * P * ( eye( n ) - kalman_gain * H )' + kalman_gain * R * kalman_gain' ;
 
xV( : , k ) = s ;                                                 % save estimated updated states
pV1( : , k ) = s ; pV1( 2 , k ) = pV1( 2 , k ) + pV1( 3 , k ) ;   % for plotting projections
pV2( : , k ) = s ; pV2( 2 , k ) = pV2( 2 , k ) + 2*pV2( 3 , k ) ; % for plotting projections
pV3( : , k ) = s ; pV3( 2 , k ) = pV3( 2 , k ) + 3*pV3( 3 , k ) ; % for plotting projections
 
endfor

% Plotting
figure(1) ; subplot(3,1,1) ; plot( real_phase , 'k', 'linewidth' , 2 , phase , 'b' , 'linewidth' , 2 , xV( 2 , : ) , 'r' , 'linewidth' , 2 ) ; grid minor on ;
title( 'Phase State' ) ; legend( 'Actual Phase State' , 'Measured Phase State' , 'Estimated Phase Sate' ) ; ylabel( 'Radians' ) ;

figure(1) ; subplot(3,1,2) ; plot(angular_freq_rads, 'k', 'linewidth' , 2 , measured_angular_frequency , 'b' , 'linewidth' , 2 , xV( 3 , : ) , 'r' , 'linewidth' , 2 ) ; 
grid minor on ; 
title( 'Angular Frequency State in Radians' ) ; legend( 'Actual Angular Frequency' , 'Measured Angular Frequency' , 'Estimated Angular Frequency' ) ;

figure(1) ; subplot(3,1,3) ; plot( ones(1,N) , 'k', 'linewidth' , 1 , measured_amplitude , 'b' , 'linewidth' , 2 , xV( 4 , : ) , 'r' , 'linewidth' , 2 ) ; grid minor on ;
title( 'Amplitude State' ) ; legend( 'Actual Amplitude' , 'Measured Amplitude' , 'Estimated Amplitude' ) ;

sine_est = h( xV ) ;
sine_1 = h( pV1 ) ; sine_1 = shift( sine_1 , 1 ) ;
sine_2 = h( pV2 ) ; sine_2 = shift( sine_2 , 2 ) ;
sine_3 = h( pV3 ) ; sine_3 = shift( sine_3 , 3 ) ;

figure(2) ; plot( gen_sine , 'k' , 'linewidth' , 2 , my_sine_noisy , '*b' , 'linewidth' , 2 , sine_est , 'r' , 'linewidth' , 2 ) ; grid minor on ;
title( 'Underlying and its Estimate' ) ; legend( 'Real Underlying Sine' , 'Noisy Measured Sine' , 'EKF Estimate of Real Underlying Sine' ) ;

figure(3) ; plot( gen_sine , 'k' , 'linewidth' , 2 , my_sine_noisy , '*b' , 'linewidth' , 2 , sine_1 , 'r' , 'linewidth' , 2 , sine_2 , 'g' , 'linewidth' , 2 , ...
sine_3 , 'b' , 'linewidth' , 2 ) ; grid minor on ;
title( 'Plot of the actual sine wave vs projected estimated sines' ) ; 
legend( 'Actual' , 'Noisy Measured Sine' , 'EKF Projected 1' , 'EKF Projected 2' , 'EKF Projected 3' ) ;
The measurement for the phase is an output of the sinewave indicator function, the period an output of the autocorrelation periodogram function and the amplitude the square root of 2 multiplied by the Root mean square of the sine wave over the autocorrelation periodogram measured period.

As usual the code is liberally commented. More soon. 

No comments: