Script MATLAB: Kalman filter diskrit

% MATLAB SCRIPT FOR DISCRETE KALMAN FILTER IN NOISY MEASUREMENT SIGNAL

clc

%% INPUT

y = signal(:,2);    % Signal

t = signal(:,1);    % Time vector

n = length(y);

%% ESTIMASI PARAMETER MODEL AUTO-REGRESSIVE ORDE 2

% y[k] = fi0 + fi1.y[k-1] + fi2.y[k-1]

x1 = 0;

x2 = 0;

x1(1) = 0;

x1(2) = y(1);

x2(1) = 0;

x2(2) = 0;

X(1,:) = [1 x1(1) x2(1)];

X(2,:) = [1 x1(2) x2(2)];

for k = 3:n

    x1(k)  = y(k-1);

    x2(k)  = y(k-2);

    X(k,:) = [1 x1(k) x2(k)];

end

fi = inv(X'*X)*X'*y; % fi0, fi1, fi2

%% DISCRETE KALMAN FILTER

% Define:

% x1[k] = y[k-1]

% x2[k] = y[k-2]

Q = eye(2)*0.001;

R = eye(1)*0.01;

A = [fi(2) fi(3);1 0];    % [fi(2) fi(3);1 0]

F = [fi(1) 0]';           % fi(1)

H = [fi(2) fi(3)];

xmin = [0 0]';

Pmin = 0.01*speye(2);

for i = 1:n

    x = A*xmin+F;

    P = A*Pmin*A'+Q;

    K = P*H'*inv(H*P*H'+R);

    x = x+K*(y(i)-H*x);

    P = (speye(2)-K*H)*P;

    zest(i) = H*x+fi(1);

    

    xmin = x;

    Pmin = P;

end

%% PLOT

figure

plot(t,zest,'LineWidth',1);

set(gca,'FontSize',12)

xlabel('Time (s)','FontSize',12);

ylabel('Voltage','FontSize',12);

axis([0 5 0 0.01])