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. 

Extended Kalman Filter

In the code box below I provide code for an Extended Kalman filter to model a sine wave. This is a mashup of code from a couple of toolboxes I have found online, namely learning-the-extended-kalman-filter and EKF/UKF Tollbox for Matlab/Octave. The modelled states are the phase, angular frequency and amplitude of the sine wave and the measurement is the ( noisy ) sine wave value itself.

clear all ;
1;

function x_n = ekf_sine_f( x )
% Dynamical model function for the sine signal.
% Takes the input vector of phase, angular frequency in radians and 
% amplitude and calculates the next values of the states given the 
% current vector.

% transition matrix
A = [ 1 1 0 ;   % phase in Rads
      0 1 0 ;   % angular_freq_rads
      0 0 1 ] ; % amplitude
      
x_n = A * x ; 
x_n( 1 ) = mod( x_n( 1 ) , 2 * pi ) ;

endfunction

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

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

endfunction

function [ z , A ] = jacobian_transition( fun , x )
z = fun( x ) ;
A = [ 1 1 0 ;
      0 1 0 ;
      0 0 1 ] ;
endfunction

function [ z , A ] = jacobian_measurement( fun , x )
z = fun( x ) ;
A = [ x( 3 ) * cos( x( 1 ) ) 0 sin( x( 1 ) ) ] ;
endfunction

%%%%%%%%%%%%%%%%%%% Generate a sine wave %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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 ) ;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
f = @ekf_sine_f ;
h = @ekf_sine_h ; 

n = 3 ;        % number of states - phase, angular frequency and amplitude
Q = eye( n ) ; % matrix for covariance of process

Q( 1 , 1 ) = var( real_phase ) ;
Q( 2 , 2 ) = var( angular_freq_rads ) ;
Q( 3 , 3 ) = var( sqrt(2) * sqrt( mean( my_sine_noisy.^2 ) ) ) ;
Q = 1 .* Q ; % scale the covariance matrix, a tunable factor ? 

R = var( gen_sine .- my_sine_noisy ) ; % measurement noise variance

% initial state
s = [ real_phase( 6 ) ;                                        % phase ( Rads )
      mean( angular_freq_rads( 1 : 20 ) ) ;                    % angular_freq_rads
      sqrt(2) * sqrt( mean( my_sine_noisy( 1 : 20 ).^2 ) ) ] ; % amplitude               
    
P = eye( n ) ;  % initial state covariance

% allocate memory
xV = zeros( n , N ) ;  % record estmates of states        
sV = ones( 1 , N ) ;   % real amplitudes 
pV1 = zeros( n , N ) ; % projection measurments
pV2 = zeros( n , N ) ; % projection measurments
pV3 = zeros( n , N ) ; % projection measurments

for k = 7 : N
 
z = my_sine_noisy( k ) ; % noisy measurement of sine

% do ekf
[ x1 , A ] = jacobian_transition( f , s ) ;   % nonlinear update and linearisation at current state
P = A * P * A' + Q ;                          % partial update
[ z1 , H ] = jacobian_measurement( h , x1 ) ; % nonlinear measurement and linearisation
P12 = P * H' ;                                % cross covariance
R = chol( H * P12 + R ) ;                     % Cholesky factorisation
U = P12 / R ;                                 % K = U / R' ; Faster because of back substitution
s = x1 + U * ( R' \ ( z - z1 ) ) ;            % Back substitution to get state update
P = P - U * U' ;                              % Covariance update, U * U' = P12 / R / R' * P12' = K * P12  
 
xV( : , k ) = s ;                             % save estimated updated states
pV1( : , k ) = s ; pV1( 1 , k ) = pV1( 1 , k ) + pV1( 2 , k ) ;   % for plotting
pV2( : , k ) = s ; pV2( 1 , k ) = pV2( 1 , k ) + 2*pV2( 2 , k ) ; % for plotting
pV3( : , k ) = s ; pV3( 1 , k ) = pV3( 1 , k ) + 3*pV3( 2 , k ) ; % for plotting
 
endfor

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

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

figure(1) ; subplot(3,1,3) ; plot( sV , 'k', 'linewidth' , 1 , xV(3,:), 'r' , 'linewidth' , 2 ) ; grid minor on ;
title( 'Amplitude State' ) ; legend( 'Actual 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 ) ; 
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 , sine_1 , 'r' , 'linewidth' , 2 , sine_2 , 'g' , 'linewidth' , 2 , sine_3 , 'b' , 'linewidth' , 2 ) ;
title( 'Plot of the actual sine wave vs projected estimated sines' ) ; 
legend( 'Actual' , 'EKF Projected 1' , 'EKF Projected 2' , 'EKF Projected 3' ) ;

Typical output plots are the states

the filtered estimate

and shifted, future projections using the current estimated states

The code is well commented so I won't talk further about it in this post as this is part of ongoing work. A fuller discussion in due course.