Di bawah ini adalah kode alternatif untuk Filter Kalman yang diperluas untuk gelombang sinus, yang memiliki 4 keadaan: nilai gelombang sinus, fase, frekuensi sudut dan amplitudo serta pengukurannya. Saya merasa perlu menerapkan versi ini karena saya tidak dapat menyesuaikan versi sebelumnya kode untuk menerima dan mengukur status tambahan tanpa Dekomposisi kolestetika fungsi chol() keluar dan memberikan kesalahan tentang input yang bukan matriks positif pasti.
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' ) ;Pengukuran untuk fase adalah keluaran dari indikator gelombang sinus fungsi, periode keluaran dari periodogram autokorelasi fungsi dan amplitudo akar kuadrat 2 dikalikan dengan Akar rata-rata kuadrat gelombang sinus selama periode periodogram autokorelasi yang diukur.
Seperti biasa, kode tersebut diberi banyak komentar. Akan segera diulas lebih lanjut.
