function sout = minmaxfc_pointMass(xinit,xfinal,simdata)
% sout = minmaxfc_pointMass(xinit,xfinal,simdata)
%
% Calculates a trajectory with initial condition, final target and
% parameters defined in the input structure simdata.
% XINIT: Initial State
% XFINAL: Target State
% Input data structure must contain:
% SIMDATA.delta Discretization step
% .delay Hard temporal delay in the closed loop system
% .pert 1x2 vector with step force magnitude along x and y axes
% .time Time horizon
% .gamma 1x2 with Parameter for optimal disturbance
% rejection level. The second entry (1 or 0) indicates
% whether the routine should optimize this value.
% .nStep Number of time steps
% .noise 1x2 vector of scaling parameters for noise matrices
% (default: [1 1])
% .ralpha matrix with one row per state variable and one column per time
% step with the cost of the corresponding state and time
% .nsimu number of simulation runs.
%
%
% SOUT: output data structure with the following fields:
% sout.L Series of optimal robust control gains
% .C Series of optimal LQG control gains
% .x State - Robust control
% .xest State Estimate - Robust control
% .z State - LQG
% .zest State Estimate - LQG
% .u Series of Control Vector - Robust control
% .v Series of Control Vector - LQG
% .minlambda Minimum eigen value (optimized or used). Must be > 0.
% .cost 1x2 vector with movement cost (1: Robust, 2: LQG)
% .gammaopt Optimal or used gamma parameter
%
%
%
% Uses: > AugRobustControl
% > extLQG
%
% Writtent by F. Crevecoeur - Spet. 6, 2019
% Used in: Robust control in human reaching movements: a model free
% strategy to compensate for unpredictable disturbances.
% Crevecoeur F., Scott S. H., Cluff T.
% DOI: https://doi.org/10.1523/JNEUROSCI.0770-19.2019
delta = simdata.delta;
delay = simdata.delay;
gamma = simdata.gamma(1);
ralpha = simdata.ralpha;
nStep = simdata.nStep;
statedim = size(xinit,1);
% Mapping all final targets on 0 to ensure positive definiteness of the
% cost matrices
xinit = xinit-xfinal;
% System Matrices
k = .1;
tau = .066;
A = [0 0 1 0 0 0 0 0;0 0 0 1 0 0 0 0;0 0 -k 0 1 0 1 0;0 0 -0 -k 0 1 0 1;...
0 0 0 0 -1/tau 0 0 0;0 0 0 0 0 -1/tau 0 0;0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0];
B = [0 0;0 0;0 0;0 0;1/tau 0;0 1/tau;0 0;0 0];
Aest = A;
DA = (A-Aest)*delta; % Used when there is a model error
A = eye(size(A))+delta*A;
Aest = eye(size(Aest))+delta*Aest;
B = delta*B;
% Observability Matrix
H = eye(size(A,1));
E = eye(8,1)'; %See Basar and Tamer, pp. 171
% Definition of the cost matrices:
Q = zeros(size(A,1),size(A,2),nStep);
M = Q;
TM = Q;
Id = eye(statedim);
%Filling in the cost matrices
for j = 1:nStep
for i = 1:statedim
Q(:,:,j) = Q(:,:,j) + ralpha(i,j)*Id(:,i)*Id(:,i)';
end
end
% Augment the System to Include the Feedback Delay
A0 = A;
DA0 = DA;
Aest0 = Aest;
B0 = B;
Q0 = Q;
H0 = H;
[A,DA,B,Q,H] = AugRobustControl(A0,DA0,B0,Q0,H0,delay,delta);
[Aest,~,~,~,~] = AugRobustControl(Aest0,DA0,B0,Q0,H0,delay,delta);
%Signal Dependent Noise
nc = size(B,2);
Csdn = zeros(size(B,1),nc,nc);
for i = 1:nc
Csdn(:,i,i) = .1*B(:,i);
end
M = Q;
TM = Q;
D = zeros(size(A));
D(1:8,1:8) = eye(8);
%--------------------------------------------------------------------------
% Implementing the backwards recursions
M(:,:,end) = Q(:,:,end);
SLQG = M;
L = zeros(size(B,2),size(A,1),nStep-1); % Optimal Minimax Gains
C = L; % Optimal LQG Gains
Lambda = zeros(size(A,1),size(A,2),nStep-1);
% Optimization of gamma
minlambda = zeros(nStep-1,1);
gammaK = 0.5;
reduceStep = 1;
positive = false;
relGamma = 1;
% Does the routine have to compute gamma opt?
if simdata.gamma(2)
while (relGamma > .001 || ~positive)
for k = nStep-1:-1:1
% Minimax Feedback Control
TM(:,:,k) = gamma^2*eye(size(A))-D'*M(:,:,k+1)*D;
minlambda(k) = min(eig(TM(:,:,k)));
Lambda(:,:,k) = eye(size(Aest))+(B*B'-gamma^-2*(D*D'))*M(:,:,k+1);
M(:,:,k) = Q(:,:,k)+Aest'*(M(:,:,k+1)^-1+B*B'-gamma^-2*D*D')^-1*Aest;
L(:,:,k) = B'*M(:,:,k+1)*Lambda(:,:,k)^-1*Aest;
end
oldGamma = gamma;
if min(real(minlambda)) >= 0
gamma = (1-gammaK)*gamma;
relGamma = (oldGamma-gamma)/oldGamma;
positive = true;
elseif min(real(minlambda)) < 0
gamma = (1-gammaK)^-1*gamma;
reduceStep = reduceStep + 0.5;
relGamma = -(oldGamma-gamma)/oldGamma;
gammaK = gammaK^reduceStep;
positive = false;
end
end
gamma = oldGamma;
elseif ~simdata.gamma(2)
for k = nStep-1:-1:1
% Minimax Feedback Control
TM(:,:,k) = gamma^2*eye(size(A))-D'*M(:,:,k+1)*D;
minlambda(k) = min(eig(TM(:,:,k)));
Lambda(:,:,k) = eye(size(Aest))+(B*B'-gamma^-2*(D*D'))*M(:,:,k+1);
M(:,:,k) = Q(:,:,k)+Aest'*(M(:,:,k+1)^-1+B*B'-gamma^-2*D*D')^-1*Aest;
L(:,:,k) = B'*M(:,:,k+1)*Lambda(:,:,k)^-1*Aest;
end
end
%--------------------------------------------------------------------------
statedim = size(A,1);
%Forward Simulation of the System Trajectory
h = max(0,floor(delay/delta))+1;
currentX = kron(ones(h,1),xinit);
currentXEst = currentX;
x = zeros(nStep,statedim);
xest = x;
x(1,:) = currentX(1:statedim)';
xest(1,:) = currentX(1:statedim)';
u = zeros(nStep-1,size(B,2)); % size(B,2) is the control dimension
w = zeros(size(currentX,1),1);
% isForceFieldON = simdata.forcefield;
% Parallel Simulation for LQG control
currentZ = currentX;
currentZEst = currentZ;
z = x;
zest = z;
v = u;
Oxi = 0.001*B*B';
Oxi(7:8,7:8) = Oxi(5:6,5:6);
Omega = eye(8)*Oxi(5,5)*simdata.noise(2);
%Parameters for State Estimation
Sigma = zeros(statedim,statedim,nStep);
Sigma(:,:,1) = eye(statedim)*10^-2;
SigmaK = Sigma;
pertX = false;
pertZ = false;
%--------------------------------------------------------------------------
% Extended LQG
RLQG = zeros(2,2,nStep-1);
for i = 1:nStep-1
RLQG(:,:,i) = eye(2);
end
Cstate = eye(statedim)*10^-2;
[C,Ke,~,~,~,~,~] = extLQG(Aest,B,Csdn,0*H,H,Q,RLQG,Oxi,Omega,0*A,Cstate,Cstate,currentZ);
%--------------------------------------------------------------------------
% Compute the total cost
cost = zeros(1,2);
for i = 1:nStep-1
if i == 14 % Time step correspondng to ~1/3 of the reach path
currentX(7:8) = simdata.pert;
currentZ(7:8) = simdata.pert;
end
sensoryNoise = mvnrnd(zeros(size(Omega,1),1),Omega)';
motorNoise = mvnrnd(zeros(size(Oxi,1),1),Oxi)';
motorNoise(7:8) = zeros(2,1);
%MINMAX HINFTY CONTROL ------------------------------------------------
%Riccati Equation for the State Estimator
Sigma(:,:,i+1) = Aest*(Sigma(:,:,i)^-1+H'*(E*E')^-1*H-gamma^-2*Q(:,:,i))^-1*Aest'+D*D';
%Feedback Eequation
yx = H*currentX + sensoryNoise;
%Minmax Simulation with State Estimator
u(i,:) = -B'*(M(:,:,i+1)^-1+B*B'-gamma^-2*(D*D'))^-1*Aest*... %Control
(eye(statedim)-gamma^-2*Sigma(:,:,i)*M(:,:,k))^-1*currentXEst;
% Cost Hinf
cost(1) = cost(1) + currentX'*Q(:,:,i)*currentX + u(i,:)*u(i,:)';
%Signal Dependent Noise - Robust Control
sdn = 0;
for isdn = 1:nc
sdn = sdn + normrnd(0,1)*Csdn(:,:,isdn)*u(i,:)';
end
% u(i,:) = u(i,:)*(1-pertX)
currentXEst = Aest*currentXEst + B*u(i,:)'+...
Aest*(Sigma(:,:,i)^-1+H'*(E*E')^-1*H-gamma^-2*Q(:,:,i))^-1*(gamma^-2*Q(:,:,i)*currentXEst+H'*(E*E')^-1*(yx-H*currentXEst));
% Minmax Simulation
wx = DA*currentX; % Non zero if there is a model error.
currentX = Aest*currentX + B*u(i,:)'+D*wx + motorNoise + sdn;
x(i+1,:) = currentX(1:statedim)';
xest(i+1,:) = currentXEst(1:statedim)';
%LQG CONTROL ----------------------------------------------------------
yz = H*currentZ + sensoryNoise;
v(i,:) = (-C(:,:,i)*currentZEst)';
K = Ke(:,:,i);
currentZEst = Aest*currentZEst + B*v(i,:)' + K*(yz-H*currentZEst);
% Cost LQG
cost(2) = cost(2) + currentZ'*Q(:,:,i)*currentZ + v(i,:)*v(i,:)';
%Signal Dependent Noise - LQG
sdn = 0;
for isdn = 1:nc
sdn = sdn + normrnd(0,1)*Csdn(:,:,isdn)*v(i,:)';
end
wz = DA*currentZ;
currentZ = Aest*currentZ + B*v(i,:)' + D*wz + motorNoise + sdn;
z(i+1,:) = currentZ(1:statedim)';
zest(i+1,:) = currentZEst(1:statedim)';
end
% Add the final cost
cost(1) = cost(1) + currentX'*Q(:,:,end)*currentX;
cost(2) = cost(2) + currentZ'*Q(:,:,end)*currentZ;
% Output structure
sout.L = L; % Series of optimal robust control gains
sout.C = C; % Series of optimal LQG control gains
sout.x = x(:,1:8); % State - Robust control
sout.xest = xest(:,1:8); % State Estimate - Robust control
sout.z = z(:,1:8); % State - LQG
sout.zest = zest(:,1:8); % State Estimate - LQG
sout.u = u; % Series of Control Vector - Robust design
sout.v = v; % Series of Control Vector - LQG
sout.minlambda = minlambda; % Min eigen value (optimized or used). Must be >0.
sout.cost = cost; % Movement cost (1: Robust, 2: LQG)
sout.gammaopt = gamma; % Optimal or used gamma parameter