Reaching movements with robust or stochastic optimal control models (Crevecoeur et al 2019)

 Download zip file 
Help downloading and running models
Accession:258846
"We explored the hypothesis that compensation for unmodelled disturbances was supported by a robust neural control strategy. We studied the predictions of stochastic optimal control (LQG) (Linear Quadratic Gaussian) (Todorov, 2005) and a robust control design that can equivalently be described as a “min-max” or worst-case strategy (Basar and Bernhard, 1991) applied to linear models of planar reaching movements. The robust controller displayed an increase in control gains, resulting in faster movements towards the target and more vigorous responses to perturbations. Our experimental results supported these predictions: the occurrence of unexpected force field disturbances evoked both faster movements and more vigorous responses to perturbations. Thus, the neural controller was more robust in the sense that the feedback responses reduced the impact of the perturbations (step and force field). Thus the compensation for disturbances involved a “model-free” component. ..."
Reference:
1 . Crevecoeur F, Scott SH, Cluff T (2019) Robust control in human reaching movements: a model-free strategy to compensate for unpredictable disturbances. J Neurosci [PubMed]
Model Information (Click on a link to find other models with that property)
Model Type:
Brain Region(s)/Organism:
Cell Type(s):
Channel(s):
Gap Junctions:
Receptor(s):
Gene(s):
Transmitter(s):
Simulation Environment: MATLAB;
Model Concept(s): Motor control;
Implementer(s): Crevecoeur, Frédéric ;
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    



Loading data, please wait...