clear all;

tSim = 25;
dt = 0.01;
t = 0:dt:tSim;
tt = numel(t);

% dwa okresy sygnalu prostokatnego jako wejscie
u = square(t,1/tSim*2,50);

% parametry ukladu
R = 20;
L = 10;
C1 = 0.01;

% macierze ukladu
A = [-R/L -1/L;
    1/C1 0];
B = [1/L;
    0];
C = [1, 0;
    0, 1];
    
q = [-1/L;
    0];

% symulacja ukladu ciaglego z uzyciem RK4
x_0 = [0;0];
x_c = zeros(2,tt);
y_c = zeros(2,tt);
ss_hndl = @(A,B,state,u) (A * state + B * u);
out_hndl = @(C,state) (C * state);
x_c(:,1) = x_0;
y_c(:,1) = out_hndl(C,x_0);
for i = 2:tt
    k1 = dt * ss_hndl(A,B,x_c(:,i-1), u(i));
    k2 = dt * ss_hndl(A,B,x_c(:,i-1) + 0.5 * k1, u(i));
    k3 = dt * ss_hndl(A,B,x_c(:,i-1) + 0.5 * k2, u(i));
    k4 = dt * ss_hndl(A,B,x_c(:,i-1) + k3, u(i));
    x_c(:,i) = x_c(:,i-1) + 1 / 6 * (k1 + 2 * k2 + 2 * k3 + k4);
    y_c(:,i) = out_hndl(C,x_c(:,i));
end

% dyskretyzacja
I = diag(ones(size(A,1),1));
%eat = expm(A * dt);
%eat = A + I * dt;
eat = (I + 0.5 * A * dt) * (I - 0.5 * A * dt)^-1;
A_d = eat;
B_d = A^-1 * (eat - I) * B;
C_d = C;

% symulacja ukladu dyskretnego
x_d = zeros(size(A,1),tt);
y_d = zeros(size(A,1),tt);
for i = 1:tt
    if i == 1
        x_d(:,i) = x_0;
    else
        x_d(:,i) = A_d * x_d(:,i-1) + B_d * u(i);
    end
    y_d(:,i) = C_d * x_d(:,i);
end

% porownanie ukladu ciaglego i dyskretnego
figure;
subplot(2,1,1);
plot(t,y_c(1,:),'r',t,y_d(1,:),'b'); grid on;
title('i');
legend('ciagly','dyskretny');
subplot(2,1,2);
plot(t,y_c(2,:),'r',t,y_d(2,:),'b'); grid on;
title('Uc');
legend('ciagly','dyskretny');

% zaklocenia wyjscia w ukladzie dyskretnym
sigma_uc = 0.1;
y_d(2,:) = y_d(2,:) + randn(1,tt) * sigma_uc;

% FK
x_p = zeros(2,tt);
x_prio = zeros(2,1);
x_post = [0; 0];
P_prio = zeros(2,2);
P_post = zeros(2,2);

A_k = A_d;
B_k = B_d;
H_k = [0,1];
q_k = [A_k(1,2);0];
W = 0.01;
R = sigma_uc^2;

xHat = zeros(2,tt);
xPost = zeros(2,1);
Ppost = zeros(2,2);
for i = 1:tt
    [xPost,Ppost] = KalmanFilter(A_k,B_k,H_k,q_k,W,R,u(i),y_d(2,i),xPost,Ppost,dt);
    xHat(:,i) = xPost;   
end

% wyniki dzialania FK
figure;
subplot(2,1,1);
plot(t,x_d(1,:),'g',t,xHat(1,:),'b'); grid on;
title('i');
legend('wzor','estymata');
subplot(2,1,2);
plot(t,y_d(2,:),'r',t,x_d(2,:),'g',t,xHat(2,:),'b'); grid on;
title('uc');
legend('pomiary','wzor','estymata');