DT=0.01; %size of step
sc_ratio=0.0632;%pixel to cm
sc_ratio=0.0632;%pixel to cm
sgscale=0.03;% the ratio of subject's perception and visual noise

sigmaTU=0.0015;% system noise of the target
Rthresh=0.012;%threshold of the initiation of the movement


TPhist=zeros(80,6)*NaN;
TPhist2=[];
Sig_array=[20 40 60 80 100 120]*sc_ratio/100*sgscale;%set of noise


Sig_1=[3 5 6 5 5 5];% noise of pre-jump: this refers to Sig_array 
Sig_2=[5 5 5 3 5 6];% noise of post-jump


t_tgj=12;

colvec=['r','b','b','m','c','c'];


for c=1:6

% ***********  model definition ********************* 
delay_step=10;% time delay of measurement
B11=0,B12=0,B21=0,B22=0; % no force field
M=0.3; % mass
Tau=0.04; % time constant of muscle

A=[0 1 0 0 0 0;
    0 -B11/M  0 -B12/M 1/M 0;
    0      0   0     1  0 0;
    0 -B21/M   0 -B22/M 0 1/M;
    0  0   0   0 -1/Tau  0
    0  0   0   0    0 -1/Tau]; %system matrix of continuous system


C=[0 0 0 0 1/Tau 0; 0 0 0 0 0 1/Tau]';%for input

Ad=eye(6)+0.01*A;%discretization
Cd=0.01*C;
% target
Av=[Ad zeros(6,2);zeros(2,6) eye(2)];% adding target
Cv=[Cd;zeros(2,2)];
Bv=[1 0 0 0 0 0 0 0; 0 0 1 0 0 0 0 0; 0 0 0 0 0 0 1 0;  0 0 0 0 0 0 0 1]; ...
% observation matrix( position and target)


Atd=zeros(8+8*delay_step,8+8*delay_step);
Atd(1:8,1:8)=Av;%extending the system matrix using hidden state for
                %the delay
for i=1:delay_step
    Atd(8*i+1:8*i+8,8*(i-1)+1:8*i)=eye(8);
end


Ctd=[Cv;zeros(8*delay_step,2)];
Btd=[zeros(4,8*delay_step),Bv];



L=zeros(1,1,8+8*delay_step); %motor cost
L(1,1,:)=0.1^8;
L(1,2,:)=0;
L(2,1,:)=0;
L(2,2,:)=0.1^8;




sigmaU=0.02*sqrt(DT); % free parameter , motor noise

sigmaT1_offset=0.05;% initial uncertainty about target


sigmaT1=Sig_array(Sig_1(c))/sqrt(DT); % measurement noise of the
                                      % first target
sigmaT2=Sig_array(Sig_2(c))/sqrt(DT);% measurement noise of the
                                     % second target
sigmaXpre=0.0035;% uncertainty of the hand position
sigmaYpre=0.0035;%


sigmaX=0.01/sqrt(DT);%measurement noise of the hand position
sigmaY=0.01/sqrt(DT);

k_scale=1;

%%%%%%%%%%%%%% system noise %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Q0=zeros(8,8);
Qn=zeros(8,8);

Qn(5,5)=sigmaU^2;
Qn(6,6)=sigmaU^2;
Qn(7,7)=sigmaTU^2;
Qn(8,8)=sigmaTU^2;

Qe=zeros(8,8);
Qe(5,5)=sigmaU^2;
Qe(6,6)=sigmaU^2;
Qe(7,7)=0;
Qe(8,8)=0;

QM=zeros(8+8*delay_step,8+8*delay_step);
QM(1:8,1:8)=Qn;

QE=zeros(8+8*delay_step,8+8*delay_step);
QE(1:8,1:8)=Qe;

%%%%%%%%%%%%%% initial P: estimated uncertainty %%%%%%%%%%%%%%%%%%%%
Pv=zeros(8,8);Pv(1,1)=sigmaXpre^2;Pv(2,2)=0;Pv(3,3)=sigmaXpre;Pv(4,4)=0;Pv(5,5)=0;Pv(6,6)=0;
Pv(7,7)=(sigmaT1_offset)^2;Pv(8,8)=(sigmaT1_offset)^2;
P(:,:,1)=zeros(8*(delay_step+1),8*(delay_step+1));

P(1:8,1:8,1)=Pv;
P(:,:,1)=blkdiag(Pv,Pv,Pv,Pv,Pv,Pv,Pv,Pv,Pv,Pv,Pv);

%%%%%%%%%%%%  measurement noise %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% before jump
R0(:,:)=[sigmaX^2, 0, 0, 0;
    0,sigmaY^2,0 ,0;
    0 , 0, sigmaT1^2, 0;
    0 , 0, 0,sigmaT1^2];
for i=1:t_tgj+delay_step % delay
    R(:,:,i)=R0;
end





% after jump
R0(:,:)=[sigmaX^2, 0, 0, 0;
    0,sigmaY^2,0 ,0;
    0 , 0, sigmaT2^2, 0;
    0 , 0, 0,sigmaT2^2];
for i=t_tgj+delay_step:80
    R(:,:,i)=R0;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xi=[0 0 0 0 0 0 0 0.2]'; % initial state
xtd=zeros(8*(delay_step+1),1);
for i=1:delay_step+1
xtd(8*(i-1)+1:8*i,1)=xi;
end


xte(:,1)=xtd(:,1);
ytd(:,1)=[0 0 0 0.2]'; % initial measurement

Tw=0*ones(1,1:80);

MVT=50; % movement time is 500ms

for i=1:80 % shaping the accuracy cost function
  if i<MVT
    Tv=0.00005*[1 0 0 0 0 0 -1 0;
    0 0.0 0 0 0 0 0  0;
    0 0 0 0 0 0 0 0;
    0 0 0 0.0 0 0 0  0;
    0 0 0 0 0 0 0  0;
    0 0 0 0 0 0 0  0;
    -1 0 0 0 0 0 1 0;
    0 0 0 0 0 0 0 0];
  else
   Tv=[1 0 0 0 0 0 -1 0;
    0 0.0 0 0 0 0 0  0;
    0 0 1 0 0 0 0 -1;
    0 0 0 0.0 0 0 0  0;
    0 0 0 0 0 0 0  0;
    0 0 0 0 0 0 0  0;
    -1 0 0 0 0 0 1 0;
    0 0 -1 0 0 0 0 1];
   Tv=(0.001/(80-MVT)*(i-MVT-1)+0.00005)*Tv;
  end


Ttd(:,:,i)=zeros(8+8*delay_step,8+8*delay_step);
Ttd(1:8,1:8,i)=Tv;
end




Ae=Atd;
Ce=Ctd; % model for optimization
Aim=Atd;
Cim=Ctd; % model for kalman filter

W(:,:,80)=Ttd(:,:,80);


RT=200;





y_b=ytd(:,1);
clear P_b;
P_b(:,:,1)=P(:,:,1);
xte_b(:,1)=xtd(:,1);


k=0;
tRT=0;

while k<RT;
  k=k+1;
  K_b(:,:,k)=(P_b(:,:,k)*Btd')*inv(Btd*P_b(:,:,k)*Btd'+R(:,:,1));
  P_b(:,:,k)=(eye(8+delay_step*8)-K_b(:,:,k)*Btd)*P_b(:,:,k);
  xte_b(:,k)=xte_b(:,k)+K_b(:,:,k)*(y_b-Btd*xte_b(:,k)); 
  xte_b(:,k+1)=Aim*xte_b(:,k);
  P_b(:,:,k+1)=Aim*P_b(:,:,k)*Aim'+QM;
  if sqrt(P_b(7,7,k))<Rthresh& tRT==0;
    tRT=k;
    RT=tRT+10;
  end
  
  
end

TPhist(1:RT,c)=sqrt(squeeze(P_b(7,7,1:RT)));% record the uncertainty

P(:,:,1)=P_b(:,:,RT-1);


% optimal feedback controller
for i=1:79
      p=79-i+1;
      W(:,:,p)=Ae'*inv(eye(8+delay_step*8)+W(:,:,p+1)*Ce*inv(L(:,:,1))*Ce')*W(:,:,p+1)*Ae+Ttd(:,:,p);
  end
  for k=1:79
    %Kalman Gain
     
   
    K(:,:,k)=(P(:,:,k)*Btd')*inv(Btd*P(:,:,k)*Btd'+R(:,:,k));
   
    %correction  
    e(k)=norm(K(:,:,k));
    xte(:,k)=xte(:,k)+K(:,:,k)*(ytd(:,k)-Btd*xte(:,k)); % Kalman filter
    

    P(:,:,k)=(eye(8+delay_step*8)-K(:,:,k)*Btd)*P(:,:,k);
   
    %motor command
    u(:,k)=-1*inv(L(:,:,1)+Ce'*W(:,:,k+1)*Ce)*Ce'*W(:,:,k+1)*Ae*xte(:,k);
    G(:,:,k)=inv(L(:,:,1)+Ce'*W(:,:,k+1)*Ce)*Ce'*W(:,:,k+1)*Ae;
        
    % environment(forward simulation)
    xte(:,k+1)=Aim*xte(:,k)+Cim*u(:,k);
    xtd(:,k+1)=Atd*xtd(:,k)+Ce*u(:,k);
     %target jump  
    if k>t_tgj
      xtd(7,k+1)=0.0175;
      xtd(8,k+1)=0.2;  
    end 
       
    
    %measurement
    ytd(:,k+1)=Btd*xtd(:,k+1);
    
   
   P(:,:,k+1)=Aim*P(:,:,k)*Aim'+QM;
   
  end
  
  trj(c).xte=xte;
  trj(c).xtd=xtd;
  trj(c).u=u;
  % figure
  
  TPhist2(:,c)=[TPhist(RT,c);sqrt(squeeze(P(7,7,:)))];% storing uncertainty
  
  
 end

 figure
subplot(3,2,1);
hold on
plot([1-t_tgj:80-t_tgj]*0.01,trj(1).xte(7,1:80),'c', ...
     'LineWidth',1);
plot([1-t_tgj:80-t_tgj]*0.01,trj(2).xte(7,1:80),'k--', ...
     'LineWidth',1);
plot([1-t_tgj:80-t_tgj]*0.01,trj(3).xte(7,1:80),'m', ...
     'LineWidth',1);
plot([0 0.6],[0.0175 0.0175],'k--');
axis([-0.0 0.6 0 0.02])

subplot(3,2,2);
hold on
plot([1-t_tgj:80-t_tgj]*0.01,trj(4).xte(7,1:80),'r', ...
     'LineWidth',1);
plot([1-t_tgj:80-t_tgj]*0.01,trj(2).xte(7,1:80),'k--', ...
     'LineWidth',1);
plot([1-t_tgj:80-t_tgj]*0.01,trj(6).xte(7,1:80),'b', ...
     'LineWidth',1);
plot([0 0.6],[0.0175 0.0175],'k--');
axis([-0.0 0.6 0 0.02])

subplot(3,2,5)
hold on
plot([1-t_tgj:80-1-t_tgj]*0.01,(trj(1).xtd(2,2:end)-trj(1).xtd(2,1: ...
						  end-1))/0.01,'c','LineWidth',1);
plot([1-t_tgj:80-1-t_tgj]*0.01,(trj(2).xtd(2,2:end)-trj(2).xtd(2,1: ...
						  end-1))/0.01,'k--','LineWidth',1);
plot([1-t_tgj:80-1-t_tgj]*0.01,(trj(3).xtd(2,2:end)-trj(3).xtd(2,1: ...
						  end-1))/0.01, ...
     'm','LineWidth',1);

axis([-0.0 0.6 -1.5 1.5])
subplot(3,2,6)
hold on
plot([1-t_tgj:80-1-t_tgj]*0.01,(trj(4).xtd(2,2:end)-trj(4).xtd(2,1: ...
						  end-1))/0.01,'r','LineWidth',1);
plot([1-t_tgj:80-1-t_tgj]*0.01,(trj(2).xtd(2,2:end)-trj(2).xtd(2,1: ...
						  end-1))/0.01,'k--','LineWidth',1);
plot([1-t_tgj:80-1-t_tgj]*0.01,(trj(6).xtd(2,2:end)-trj(6).xtd(2,1: ...
						  end-1))/0.01,'b','LineWidth',1);
axis([-0.0 0.6 -1.5 1.5])



subplot(3,2,3)
hold on
plot([1-t_tgj:80-1-t_tgj]*0.01,trj(1).u(1,1:80-1),'c','LineWidth',1);
plot([1-t_tgj:80-1-t_tgj]*0.01,trj(2).u(1,1:80-1),'k--','LineWidth',1);
plot([1-t_tgj:80-1-t_tgj]*0.01,trj(3).u(1,1:80-1),'m','LineWidth',1);

axis([-0.0 0.6 -0.5 0.5])
subplot(3,2,4)
hold on
plot([1-t_tgj:80-1-t_tgj]*0.01,trj(4).u(1,1:80-1),'r','LineWidth',1);
plot([1-t_tgj:80-1-t_tgj]*0.01,trj(2).u(1,1:80-1),'k--','LineWidth',1);
plot([1-t_tgj:80-1-t_tgj]*0.01,trj(6).u(1,1:80-1),'b','LineWidth',1);
axis([-0 0.6 -0.5 0.5])




co=['r','g','b','m','c','k'];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
subplot(1,2,1)
for c=1:6
  plot(TPhist(1:45,c),co(c));
  hold on;
end

axis([1 40 0.005 0.04])
subplot(1,2,2)
  for c=1:6
    plot(TPhist2(2:79,c),co(c));
    hold on;

  end