function estimate = observer(u,y,A,B,Bd,C,L)
% observer.m
%
% Discrete observer for estimation of states and disturbances
% Used by the single layer MPC that is designed from scratch
% Martin Kvernland

persistent x_hat d_hat

if (isempty(x_hat))
    x_hat = zeros(size(A,1),1);
    d_hat = zeros(size(Bd,2),1);
else
    A_tilde = [A Bd; zeros(size(Bd,2),size(A,2)) eye(size(Bd,2))];
    B_tilde = [B; zeros(size(Bd,2),size(B,2))];
    C_tilde = [C zeros(size(C,1),size(Bd,2))];
    estimate = (A_tilde - L * C_tilde) * [x_hat; d_hat] + B_tilde * u + L * y;
    x_hat = estimate(1:size(A,1));
    d_hat = estimate((size(A,1) + 1):(size(A,1) + size(Bd,2)));
end

x_hat_est = x_hat;
d_hat_est = d_hat;

estimate = [
    x_hat_est
    d_hat_est
    ];
