% LKB photon box: QND measurement of photon
% September 2015
% pierre.rouchon@mines-paristech.fr
%
clear all;
%%  system parameter
T=1; omega=2*pi/T;
%% PID gain
Omega0=omega;xi=1/sqrt(2);epsilon=1/5;
Kp=Omega0^2;
Kd=2*xi*sqrt(omega^2+Omega0^2);
Ki=epsilon*(omega^2+Omega0^2).^(3/2);
%%  measurement operators
dt=T/100; t=(0:dt:7*T); Nt=length(t);
x1=0;x2=0;% initial system state 
Int=0;% initial controller state
Y=zeros(Nt,1);Yc=zeros(Nt,1);U=zeros(Nt,1);W=zeros(Nt,1);
%% simulation loop
for ii=1:Nt;
   w= 2*pi*sin(omega*t(ii)); % unknown perturbation
   
   yc=(1+sign(t(ii)-4*T))/2; % set point
   y=x1;ydot=x2; % measurement

   err=y-yc; errdot=ydot; % output error
   u=-(Kp*y+Kd*errdot+Ki*Int);  % PID controller 
   
   % numerical integration (explicit Euler scheme)
   x1dot=x2;  x2dot=-omega^2*x1+u+w;
   x1=x1+dt*x1dot; x2=x2+dt*x2dot; % system
   
   Int=Int+dt*err; % controller
   
   Y(ii)=y;Yc(ii)=yc;U(ii)=u;W(ii)=w;
end
%% graphics
figure(1),clf,
subplot(211);
plot(t,Y,'k-',t,Yc,'k--','linewidth',2);
legend('y','y_c');
subplot(212);
plot(t,U,'k-',t,W,'k--','linewidth',2);
legend('control u','perturbation w');
xlabel('time');



