clc;
clear;
close all;
%%
hq = 4 ; 
uq = sqrt(hq) ; 
A = -1/(2*sqrt(hq)) ;
B = 1 ;
Q = 1 ; R = 1 ; N = 0 ;
% K = lqr(A , B , Q , R  , N) ;
K = 3.75 ;
Ts = 0.001 ;
t = 0:Ts:10 ; 
Nt = numel(t) ; 
h = zeros(size(t)) ; h(1) = hq*rand ; 
u = zeros(size(t)) ; 
for  i = 1:Nt
    dh = h(i) - hq ;
    du = -K * dh ; 
    u(i) = uq + du ; 
    h(i+1) = h(i) + Ts * (u(i) - sqrt(h(i))) ; 
end
%% 
figure(1) ;
subplot(211) ; 
plot(t , hq*ones(size(t)) , 'r' , t , h(1:Nt) ,'b' , 'LineWidth' , 3) ; hold on 
grid on
title('h') ; legend('hq','h')

figure(1) ;
subplot(212) ; 
plot(t , u , 'LineWidth' , 3) ;  hold on
grid on
title('u') ;




