DEMO of Kalman Filter (Optimal Recursive Data Algorithem)

% Kalman Filter (Optimal Recursive Data Algorithm)
% Script was programmed based on Tutorial Video from DR_CAN
% https://www.bilibili.com/video/BV1ez4y1X7eR/?spm_id_from=333.788.top_right_bar_window_history.content.click&vd_source=c14840b02ae091da3dff0ef4c7069a6e


close all
clear all
clc

num_samples = 500;
moving_avg_window = 15;
total_sum = 0;


real_value = 50;
kk = zeros(num_samples, 1);
Zk = zeros(num_samples, 1);
x_hat = zeros(num_samples, 1);
e_EST = zeros(num_samples, 1);
kk = zeros(num_samples, 1);
e_MAG = zeros(num_samples, 1);
moving_avg = zeros(num_samples, 1);
moving_avg_buffer = zeros(moving_avg_window, 1);
total_avg = zeros(num_samples, 1);

Zk(1) = 50;
x_hat(1) = 40;
e_MAG(1) = 3;
e_EST(1) = 5;
meg_error = 5; % +/-5

index = 1:num_samples;
index = index';

jj = 1;

for ii = 2:num_samples
    if rand >= 0.5 
        sig = 1;
    else
        sig = -1;
    end
    
    % Kalman Optimal Recursive Data Algorithem
    Zk(ii) = real_value + meg_error * rand() * sig;                 % measurement with random error
    e_MAG(ii) = meg_error;                                          % measurement error set as max error
    kk(ii) = e_EST(ii-1) / ( e_EST(ii-1) + e_MAG(ii) );             % Kalman factor
    x_hat(ii) = x_hat(ii-1) + kk(ii) * ( Zk(ii) - x_hat(ii-1) );    % estimation based on Kalman algorigthem
    e_EST(ii) = (1-kk(ii))*e_EST(ii-1);                             % estimation error ???
    
    % simplified moving average filer
    moving_avg_buffer( jj ) = Zk(ii);
    if ii <= moving_avg_window
        moving_avg(ii) = sum(moving_avg_buffer)/jj;
        jj = jj + 1;
    else
        jj = 1;
        moving_avg(ii) = sum(moving_avg_buffer)/moving_avg_window;
    end
    
    % total average
    total_sum = total_sum + Zk(ii);
    total_avg(ii) = total_sum / ii;
    
end


figure(1)

subplot(2,1,1);
plot(index,Zk,'y-', index,x_hat,'r-', index,moving_avg,'b-', index,total_avg,'g-');
legend('Measurement','Kalman estimation','moving average','total average');
ylim([real_value-meg_error real_value+meg_error]);
title('data overview');
xlabel('sample data points');
ylabel('sample data values');

subplot(2,1,2);
plot(index,Zk-real_value,'y-', index,x_hat-real_value,'r-', index,moving_avg-real_value,'b-', index,total_avg-real_value,'g-');
legend('Error of Measurement','Error of Kalman estimation','Error of moving average','Error of total average');
ylim([-0.5 0.5]);
%ylim([-meg_error meg_error]);
title('data error overview');
xlabel('sample data points');
ylabel('sample error data values');