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.