% Model of optimal feedback controller with state estimation using delayed feedback
% Generates Figure 4
% from 'Extrinsic and Intrinsic Dynamics in Movement Intermittency'
% by Susilaradeya et al.

clear all
close all
figure
col=['b' 'r'];
dt=0.01;                                    % Model time-step
tau_int=0.26;                               % Intrinsic time delay
trans_mat=[1 dt;0  1];                      % Transition matrix
model1=ss(trans_mat,[0 0;0 1],[1 0],0,dt);  % Model for kalman filter
model2=ss(trans_mat,[0;1],[1 0],0,dt);      % Model for lqr
k_PI=lqr(model2,[1 0;0 0],1)                % lqr gains
H_PI=k_PI(2)/(1+k_PI(2));                   % Approximate PI controller transfer function
sigma=250;                                  % Ratio of process/measurement noise

all_freq=[0.1:0.1:6];                       % Plot responses up to 6 Hz
pert_w=[10:10:50];                          % Highlight 1..5 Hz perturbation frequencies

for cond=1:2
    tau_ext=(cond-1)*0.2;                   % Extrinsic time delay of 0ms or 200 ms
    Q=[0 0;0 dt^2]*sigma^2;                 % Noise covariance matrix

% Calculate state estimator dynamics
    
    [k,l,p,m]=kalman(model1,Q,1,'delayed')
    
% Adjust output to compensate for intrinsic delay
    
    C=k.C;
    C(1,2)=C(1,2)+tau_int;                 
    k.C=C;
    
% Calculate state estimator transfer function
    
    [mag_resp,phase_resp,w]=bode(k,all_freq*2*pi); 
    H_yz=squeeze(mag_resp)'.*exp(i*deg2rad(squeeze(phase_resp)'));
    
% Calculate force transfer function (incorporating intrinsic and extrinsic delays
    
    H_force=exp(-i*w*(tau_int+tau_ext)).*H_PI.*H_yz(:,1);
    
% Calculate cursor transfer function 
    
    H_cursor=1-H_force;

% Plot cursor amplitude response    
    
    subplot(3,1,1)
    hold on
    plot(w/(2*pi),abs(H_cursor),col(cond))
    plot(w(pert_w)/(2*pi),abs(H_cursor(pert_w)),[col(cond) 'o'])
    axis([0 6 0 1.5])
    title('Cursor amplitude response')

% Plot force amplitude response        
    
    subplot(3,1,2)
    hold on
    plot(w/(2*pi),0.05*cond+abs(H_force),col(cond));
    plot(w(pert_w)/(2*pi),0.05*cond+abs(H_force(pert_w)),[col(cond) 'o']);
    axis([0 6 0 1.5])
    title('Force amplitude response')

% Plot force phase delay    
    
    subplot(3,1,3)
    hold on
    phase_delay=1000*(unwrap(-angle(H_force))./w-tau_ext);
    plot(w/(2*pi),5*cond+phase_delay,col(cond))
    plot(w(pert_w)/(2*pi),5*cond+phase_delay(pert_w),[col(cond) 'o'])
    axis([0 6 100 400])
    title('Phase delay (ms)')
    xlabel('Frequency (Hz)')
    legend('0ms delay','','200ms delay','')
end