Commit eb25908b authored by Per Skarin's avatar Per Skarin
Browse files

Import rate-switching MPC evaluation

parent 34173104
# cotc-rsmpc
Examples of rate switcing MPC
# Control-over-the-Cloud - Rate-Switching MPC
Evaluation of a rate switching, state and input constrained, MPC, derived from a continuous time optimal design. This code has been tested with [Matlab Simulink v 9.2 (R2018b)](https://www.mathworks.com/products/matlab.html) and [TrueTime 2.0](http://www.control.lth.se/research/tools-and-software/truetime).
### Primary files
- init.m Configures the example.
- rsmpc.slx Simulink model.
- tt\_rsmpc\_init.m RS-MPC init file, TrueTime.
- tt\_rsmpc\_main.m RS-MPC implementation, TrueTime.
- simrsmpc.m Runs simulation. See file header for options.
- freqinfo.m Finds freq. range from singular values.
File added
figure;
w = linspace(0.1, 1000, 10000);
[sv,w] = sigma(LQc,w);
svn = sv/max(sv);
larger = find(svn>0.71); % -3dB
smaller = find(svn<=0.71);
bf = [w(larger(end)) w(smaller(1))];
bsv = [svn(larger(end)) svn(smaller(1))];
step=(bsv(1)-0.71)/(bsv(1)-bsv(2));
bw = bf(1)+(bf(2)-bf(1))*step
sample_h=[0.15 0.6]/bw
sample_f=1./sample_h
%csvwrite('sigma.csv', [w svn']);
loglog(w, svn);
grid()
function [SysD,Q,R,N,K,S] = getdlq(SysC, Qc, Rc, h)
% Get a discrete time LQ, sampled system and cost matrices, from a continous definition
[SysD,Q,R,N,R1,Qconst] = lqgsamp(SysC,h,Qc,Rc,zeros(size(SysC.B)),zeros(size(SysC.A)));
SysD = modred(SysD, 4);
Q = Q(1:size(SysD.A,1),1:size(SysD.A,1));
N = N(1:size(SysD.A,1), 1:size(SysD.B,2));
[K,S] = dlqr(SysD.A, SysD.B, Q, R, N);
end
zetaVariant = 0;
f_hf = 30;
f_lf = f_hf/2;
h_hf = 1/f_hf;
h_lf = 1/f_lf;
N_lf = 10;
N_hf = 2*N_lf;
mpc_mode = 1; % 1 = A, 2 = B, 3 = C
rng(1);
SysC = sysmodel();
Qc = diag([1500 20 0]);
Rc = 1;
[Kc, Sc, Ec] = lqr(SysC, Qc, Rc);
LQc = ss(SysC.A-SysC.B*Kc, SysC.B, SysC.C, 0);
[SysD, Qd, Rd, Nd, Kd, Sd] = getdlq(SysC, Qc, Rc, h_hf);
t = 10;
xinit = [0 0 0];
simtime = h_hf * [0:1/h_hf * t] - 1/60;
simin_ref = [simtime; ones(1, length(simtime))*8]';
simin_drop = [simtime; zeros(1, length(simtime))]';
simin_au = [simtime; zeros(3,length(simtime))]';
experiment = {};
% Prepares an MPC configuration to use with quadprog
function [mpc] = init_mpc(h, SysC, Qc, Rc)
[SysD, Qd, Rd, Nd, Kd, Sd] = getdlq(SysC, Qc, Rc, h);
model = ss(SysD, h);
mpc.h = h;
mpc.A = model.A;
mpc.B = model.B;
mpc.C = model.C;
mpc.D = [];
mpc.Cx = repmat([0.55; 10; pi/4], 2, 1);
mpc.CX = [diag([1 1 1]); diag([-1 -1 -1])];
mpc.Cu = [10; 10];
mpc.CU = [1; -1];
mpc.Q = Qd;
mpc.R = Rd;
mpc.N = Nd;
mpc.Xf = [];
mpc.Qf = Sd;
mpc.K = Kd;
mpc.S = Sd;
function [sysd,Q1e,Q2e,Q12e,R1,Qconst] = lqgsamp(sysc,h,Q1c,Q2c,Q12c,R1,tau)
%
% [sysd,Q1,Q2,Q12,R1,Qconst] = lqgsamp(sysc,h,Q1c,Q2c,Q12c,R1c)
% [sysd,Q1,Q2,Q12,R1,Qconst] = lqgsamp(sysc,h,Q1c,Q2c,Q12c,R1c,tau)
%
% Sample a continous time LQ design
if nargin < 7
tau = 0;
end
[A,B,C] = ssdata(sysc);
totsize = size(A,2)+size(B,2);
a1 = size(A,1); % number of states
b2 = size(B,2); % number of inputs
inttau = max(0,ceil(tau/h-1)); % whole samples extra delay
fractau = tau - inttau*h; % fractional delay
% Sample the plant, the cost, and the noise
Q = [Q1c Q12c; Q12c' Q2c];
[Phi2,r,Q2] = calcc2d([A B;zeros(b2,a1+b2)],blkdiag(R1,zeros(b2)),Q,fractau);
Gamma2 = Phi2(1:a1,a1+1:a1+b2);
[X,r,Q1] = calcc2d([A B;zeros(b2,a1+b2)],blkdiag(R1,zeros(b2)),Q,h-fractau);
Gamma2 = X(1:a1,1:a1)*Gamma2;
Gamma1 = X(1:a1,a1+1:a1+b2);
[Phi,R1,Q,Qconst] = calcc2d([A B;zeros(b2,a1+b2)],blkdiag(R1,zeros(b2)),Q,h);
R1 = R1(1:a1,1:a1);
Phi = Phi(1:a1,1:a1);
% Build extended state-space model
Phie = [Phi Gamma2; zeros(b2,a1+b2)];
Gammae = [Gamma1; eye(b2)];
Ce = [C zeros(size(C,1),b2)];
Ge = [eye(a1);zeros(b2,a1)];
Q1e = Q2+Phi2(1:a1,:)'*Q1(1:a1,1:a1)*Phi2(1:a1,:);
Q2e = Q1(a1+1:end,a1+1:end);
Q12e = [Phi2(1:a1,:)'*Q1(1:a1,a1+1:end)];
% Add additional integer delays (if tau > h)
if inttau > 0
Phie = blkdiag([Phie Gammae],eye((inttau-1)*b2));
Gammae = zeros(size(Phie,1),b2);
Phie = [Phie; zeros(b2,size(Phie,2))];
Gammae = [Gammae; eye(b2)];
Ce = [Ce zeros(size(Ce,1),inttau*b2)];
Ge = [Ge; zeros(inttau*b2,size(Ge,2))];
Q1e = [Q1e Q12e; Q12e' Q2e];
Q1e = blkdiag(Q1e,zeros((inttau-1)*b2));
Q2e = zeros(size(Q2e));
Q12e = zeros(size(Q1e,1),size(Q2e,2));
end
R1 = blkdiag(R1,zeros(size(Phie)-size(A)));
sysd = ss(Phie,Gammae,Ce,0,h);
File added
File added
% Run quadprog optimizer
% mpc is the mpc configuration with the following fields:
% A,B System model matrices
% Q State cost
% Qf Terminal cost
% R Control signal cost
% N Cross-couping cost (may be [], which is typical in discrete design)
% CX,Cx State constraints
% CU,Uc Input constraints
% Xf Terminal constraints
% x is the current state estimate
% xref is the target state
% N is the prediction horizon
function [optx, fval, exitflag, output] = runquadprog(mpc, x, xref, Np, Nc)
ns = size(mpc.A, 1); % Number of states
ni = size(mpc.B, 2); % Number of inputs
dx = x-xref;
% Setup costs. f is linear costs which is not used.
H = blkdiag(kron(eye(Np-1), mpc.Q), mpc.Qf, kron(eye(Np), mpc.R));
f = zeros(length(H),1);
if isfield(mpc, 'N') == 1 && isempty(mpc.N) == 0
H(1:size(mpc.Q,1)*Np, size(mpc.Q,1)*Np+1:end) = kron(eye(Np), mpc.N);
H(size(mpc.Q,1)*Np+1:end, 1:size(mpc.Q,1)*Np) = kron(eye(Np), mpc.N');
end
% Setup create constraint inequality
A1 = kron(eye(Np), mpc.CX);
A4 = kron(eye(Np), mpc.CU);
A2 = zeros(size(A1,1), size(A4, 2));
A3 = zeros(size(A4,1), size(A1, 2));
A = [A1 A2; A3 A4];
if isfield(mpc, 'Xf') == 0 || isempty(mpc.Xf)
mpc.Xf = mpc.Cx - mpc.CX*xref;
end
b = [repmat(mpc.Cx - mpc.CX*xref, Np-1,1); mpc.Xf; repmat(mpc.Cu, Np, 1)];
% Setup equality constraints (the model)
Aeq1 = [zeros(ns,ns*Np); kron(eye(Np-1), -mpc.A) zeros(ns*(Np-1), ns)] + kron(eye(Np), eye(ns));
Aeq2def = [eye(Nc, Nc) zeros(Nc, Np-Nc); zeros(Np-Nc, Nc-1) ones(Np-Nc, 1) zeros(Np-Nc, Np-Nc)];
Aeq2 = kron(Aeq2def, -mpc.B);
Aeq = [Aeq1, Aeq2];
beq_ = mpc.A * dx;
beq = [beq_; zeros((Np-1)*ns, 1)];
% Set optimizer options
options = optimoptions('quadprog', 'Display', 'off', 'MaxIterations', 200, 'OptimalityTolerance', 1e-8);
x0 = dx;
[optx,fval,exitflag,output] = quadprog(H, f, A, b, Aeq, beq, [], [], x0, options);
function [experiment] = simcdc(name, mpcmode, zetaVariant)
% Runs named experiments. First run init.m.
%
% The named experiments are:
% 'singlestep' Performs a single step 0->0.50 and runs 5 seconds. Drops are random.
% 'repstep' Repeated step changes.
% 'largerepstep' Performs large steps close to the constraint. Note: Keeps u, does not use OL data.
% 'largerepstepol' Same as above but uses open loop when switching.
% 'disturb' A noisy disturbance is applied through the input u.
%
% mpc_mode is one of:
% 0: Local (LF) control only
% 1: Mode A
% 2: Mode B
% 3: Mode C
% 4: Disregard drop and run HF controller only (local HF control)
%
% zetaVariant is one of:
% 0: LQ
% 1: u(0)
% 2: Open loop, u(k-i)
% 3: u(1)
%
% After finished experiment use savesim(experiment) to store the result on disk.
% The result is stored in sim/{name}/{mode}/.
h_hf = evalin('base', 'h_hf');
refscale = 10/0.55;
rng(1);
assignin('base', 'zetaVariant', zetaVariant);
if strcmp(name, 'singlestep')
sp=0.35;
xinit = [-0.401 0 0]; % Small difference from reference allows Matlab to solve algebraic loop for LQc
T = h_hf * [0:1/h_hf * 2] - 1/60;
% Delay the reference so LQ doesn't kick in for all modes at the first step!
simin_ref = [T; ones(1, 5)*-0.4*refscale ones(1, length(T)-5) * sp*refscale]';
simin_drop = [T; randi(2, 1, length(T))-1]';
simin_au = [T; zeros(3,length(T))]';
stopTime=T(end);
elseif strcmp(name, 'repstep')
xinit = [-0.401 0 0]; % Small difference from reference allows Matlab to solve algebraic loop for LQc
T = h_hf * [0:1/h_hf * 2] - 1/60;
% Delay the reference so LQ doesn't kick in for all modes at the first step!
simin_ref = [ones(1, 5)*-0.4*refscale ones(1, length(T)-5) * 0.4*refscale];
simin_ref = kron(repmat([1 -1], 1, 90), simin_ref);
simin_drop = randi(2, 1, length(simin_ref))-1;
simin_au = zeros(3,length(simin_ref));
T = h_hf * [0:length(simin_ref)-1];
stopTime=T(end);
simin_ref = [T; simin_ref]';
simin_drop = [T; simin_drop]';
simin_au = [T; simin_au]';
elseif strcmp(name, 'largerepstep') || strcmp(name, 'largerepstepol')
xinit = [-0.501 0 0]; % Small difference from reference allows Matlab to solve algebraic loop for LQc
T = h_hf * [0:1/h_hf * 2] - 1/60;
% Delay the reference so LQ doesn't kick in for all modes at the first step!
simin_ref = [ones(1, 5)*-0.5*refscale ones(1, length(T)-5) * 0.5*refscale];
simin_ref = kron(repmat([1 -1], 1, 90), simin_ref);
simin_drop = randi(2, 1, length(simin_ref))-1;
simin_au = zeros(3,length(simin_ref));
T = h_hf * [0:length(simin_ref)-1];
stopTime=T(end);
simin_ref = [T; simin_ref]';
simin_drop = [T; simin_drop]';
simin_au = [T; simin_au]';
if strcmp(name, 'largerepstepol')
assignin('base', 'zetaVariant', 2);
end
% else
% assignin('base', 'zetaVariant', 1);
% end
elseif strcmp(name, 'disturb')
% assignin('base', 'zetaVariant', 2);
t = 360;
xinit = [0.541 0 0]; % Small difference from reference allows Matlab to solve algebraic loop for LQc
T = h_hf * [0:1/h_hf * t] - 1/60;
% Delay the reference so LQ doesn't kick in for all modes at the first step!
simin_ref = [ones(1, length(T))*0.54*refscale];
simin_drop = randi(2, 1, length(simin_ref))-1;
simin_au = 0 + 1.*randn(1, length(T));
T = h_hf * [0:length(simin_ref)-1];
stopTime=T(end);
simin_ref = [T; simin_ref]';
simin_drop = [T; simin_drop]';
simin_au = [T; simin_au; zeros(2, length(simin_au))]';
elseif strcmp(name, 'basic')
xinit = [-0.401 0 0]; % Small difference from reference allows Matlab to solve algebraic loop for LQc
stopTime = 5;
T = h_hf * [0:1/h_hf * stopTime] - 1/60;
ref=(10/0.55)*0.5;
simin_ref = [];
steplen=3;
for sec = 1:steplen:stopTime
l=min(steplen, stopTime-sec);
simin_ref = [simin_ref ref*ones(1, 1/h_hf*l)];
ref = -ref;
end
simin_ref = [simin_ref ones(1, length(T)-length(simin_ref))*simin_ref(end)];
simin_ref = [T; simin_ref]';
simin_drop = [T; randi(2, 1, length(T))-1]';
simin_au = [T; zeros(1,length(T))]';
else
ERROR
end
SimStopTime=sprintf('%d', stopTime);
assignin('base', 'xinit', xinit);
assignin('base', 'simin_drop', simin_drop);
assignin('base', 'simin_au', simin_au);
assignin('base', 'simin_ref', simin_ref);
assignin('base', 'mpc_mode', mpcmode);
simout = sim('rsmpc', 'ReturnWorkspaceOutputs', 'on', 'StopTime', SimStopTime);
experiment = {};
experiment.sim = simout;
modes = {'LF', 'A', 'B', 'C', 'HF'};
experiment.mpc_mode = modes{evalin('base', 'mpc_mode')+1};
experiment.zeta_variant = sprintf('%d', evalin('base', 'zetaVariant'));
experiment.name = name;
end
function [SysC] = sysmodel()
l = 1.1;
g = 9.80665;
g1 = 0.44;
g2 = 10/(pi/4);
g3 = g;
g4 = 5/7;
g5 = 10/(l/2);
g34 = -g3*g4;
A = [ 0 1 0; 0 0 g34; 0 0 0; ];
B = [ 0; 0; g1; ];
C = [ g5 0 0; 0 0 g2 ];
SysC = ss(A, B, C, []);
SysC.InputName= {'AngularVelocityV'};
SysC.OutputName= {'PositionV', 'AngleV'};
SysC.StateName= {'Position', 'Velocity', 'Angle'};
% Input args:
% 1: Frequency
% 2: Horizon
function tt_rsmpc_init(args)
ttInitKernel('prioFP');
SysC = sysmodel();
Qc = diag([args(7); args(8); args(9)]);
Rc = args(10);
data.mpcL = init_mpc(1.0/args(1), SysC, Qc, Rc);
data.mpcR = init_mpc(1.0/args(3), SysC, Qc, Rc);
data.NL = args(2);
data.NR = args(4);
data.u = 0;
data.Lu = 0;
data.pend_Lu = 0;
data.pend_j = -1;
data.Ru = zeros(1,3);
data.LQu = 0;
data.which = 0;
data.localExec = 0;
% Here are parameters from the paper algorithm
% NOTE: At this point j does not correspond. j in the paper is j+2 here.
% NOTE: At this point i does not correspond. i in the paper is i+1 here.
data.Delta = zeros(1,4)
data.sigma = 2;
data.applyi = -100;
data.applyj = -100;
data.r = -1
data.u_i = zeros(1,3); % Data from remote
data.flag_i = -2; % Flag from remote
data.flag_j = -2; % Flag from local
data.k = -1; % The HF time step
data.i = -100; % Remote data timestamp, (time when opt was requested)
data.j = -100; % Local data timestamp, (time when opt was requested)
data.local_run_at = -1; % A timestamp (k) when local mode shall be run
data.zetaVariant = args(5); % 0 = LQ, 1 = u(0), 2 = OpenLoop, 3 = u(1)
data.mode = args(6) % 1=A, 2=B, 3=C
if data.mode == 0 || data.mode == 4
data.zetaVariant = 0; % Always use LQ backup for these to not make things confusing
end
ttCreatePeriodicTask('rsmpc', 0, data.mpcR.h, 'tt_rsmpc_main', data);
ttSetPriority(1, 'rsmpc');
end
function [exectime, data] = tt_switching_main(segment, data)
exectime = 0;
switch segment
case 1 % Step time and read input
data.k = data.k + 1;
sp = ttAnalogIn(1);
pos = ttAnalogIn(2);
vel = ttAnalogIn(3);
ang = ttAnalogIn(4);
data.xk = [pos vel ang]';
data.xref = [sp 0 0]';
data.drop = ttAnalogIn(5);
if data.mode == 4
data.drop = 0;
end
data.Delta = zeros(1,4); % Selection vector
case 2 % Update remote response. Mode 0 is local only. Flag_i checks feasibility. Drop=0 means network was ok.
if data.flag_i == 1 % Feasible result
ttAnalogOut(4, 1);
else
ttAnalogOut(4, 0);
end
if data.mode > 0 && data.flag_i == 1 && data.drop == 0
data.Delta(1) = 1;
data.i = data.k-1; % Set the time of start for successful HF optimization.
data.applyi = data.k;
data.Ru = data.u_i;
if data.mode == 1 || data.mode == 3 % Stop local execution
data.local_run_at = -1;
if data.mode == 1
data.r = data.k + 3;
else
data.r = data.k + 2;
end
else
data.r = data.k + mod(data.k,data.sigma)+data.sigma;
end
elseif data.drop == 0 && data.flag_i ~= 1
disp(sprintf('HF infeasible, k=%d', data.k));
end
case 3 % Update local response and determine recovery
if data.pend_j > -1 && data.pend_j == data.k-2
if data.flag_j == 1 % Feasible
ttAnalogOut(5, 1);
data.j = data.pend_j;
data.applyj = data.k;
data.Lu = data.pend_Lu;
else
ttAnalogOut(5, 0);
end
end
if data.applyj-data.applyi >= data.sigma && data.k - data.applyj < data.sigma
data.Delta(2) = 1;
end
DeltaBar = 0;
if data.k ~= 0 && data.k < data.r % What if there is no initial networked response?
DeltaBar = 1;
end
data.Delta(3) = DeltaBar*(1-sum(data.Delta(1,1:2)));
data.Delta(4) = (1-sum(data.Delta(1,1:3)));
if sum(data.Delta) == 0 || sum(data.Delta) > 1
ME = MException('SwitchingMPC:DeltaError', 'Invalid Delta vector %i%i%i%i', data.Delta);
throw(ME)
end
case 4 % Select and apply control signal
% HF selector function \Delta_\alpha
deltaAlpha = zeros(1,3);
if data.i > -1 && data.k-data.i <= length(deltaAlpha)
deltaAlpha(data.k-data.i) = 1;
end
deltaB = zeros(1,2);
if data.j > -1 && data.k-data.j > 1 && data.k-data.j-1 <= length(deltaB)
deltaB(data.k-data.j-1) = 1;
end
deltaG = (1-sum(deltaAlpha(1:2)))*deltaB(1) + (1-sum(deltaAlpha(2:3)))*deltaB(2);
deltaOne = deltaAlpha(1);
deltaB = 0;
if deltaOne == 1 % Remote closed loop control
data.u = data.Ru(1);
data.which = 2;
elseif deltaG == 1
data.u = data.Lu;
data.which = 1;
elseif deltaAlpha(2) == 1
if data.zetaVariant == 2
data.u = data.Ru(2);
else
data.u = data.Ru(1);
end
data.which = 1.75;
elseif deltaAlpha(3) == 1
if data.zetaVariant == 2
data.u = data.Ru(3);
else
data.u = data.Ru(1);
end
data.which = 1.5;
else
data.u = data.mpcR.K*(data.xref-data.xk); % Calculate HF LQ for recovery mode
data.which = 0;
end
zeta_which = 0;
% The paper logic
if data.zetaVariant == 0 % LQ control
zeta_1 = data.mpcR.K*(data.xref-data.xk);
zeta_which = 0;
elseif data.zetaVariant == 1 % Hold control signal
zeta_1 = data.Ru(1);
zeta_which = 2;
elseif data.zetaVariant == 2 % Open loop control
if data.k - (data.i+1) < length(data.Ru)
zeta_1 = data.Ru(data.k-data.i); % Note: i is not according to definition
zeta_which = 2-0.25*(data.k-(data.i+1));
else
zeta_1 = 0;
zeta_which = -1;
end
else % Hold during MPC_2 execution
if data.mode == 1 % Mode A
% Applies one step open loop since MPC_2 is started in this time step
zeta_1 = data.Ru(2);
zeta_which = 1.5;
elseif data.mode == 2 % Mode B
if data.k == data.local_run_at
zeta_1 = data.Ru(2);
zeta_which = 1.5;
else
zeta_1 = data.Ru(1);
zeta_which = 1.75;
end
else % Mode C
% Holds control signal since MPC_2 was started in previous time step
zeta_1 = data.Ru(1);
zeta_which = 1.75;
end
end
zeta_2 = data.mpcR.K*(data.xref-data.xk);
data.zeta2cost = 0.5*(data.xref-data.xk)'*data.mpcR.S*(data.xref-data.xk);
selections = [data.Ru(1) data.Lu zeta_1 zeta_2];
newu = data.Delta*selections';
if data.Delta(1) == 1
data.which = 2;
elseif data.Delta(2) == 1
data.which = 1;
elseif data.Delta(3) == 1
data.which = zeta_which;
else
data.which = 0;
end
data.u = newu;
ttAnalogOut(1, data.u);
ttAnalogOut(2, data.which);
if data.mode == 1 && data.local_run_at == -1 % Mode A, shut down local on remote response
data.localExec = 0;
end
case 5
% Always run the HF controller
x = data.mpcR.A*data.xk + data.mpcR.B*data.u; % HF state prediction
[optx, fval, exitflag, output] = runquadprog(data.mpcR, x, data.xref, data.NR, data.NR);
if exitflag == 1
data.u_i = optx(data.NR*3+1:data.NR*3+3); % Three steps of control signal from HF
end
data.flag_i = exitflag;
case 6
if data.mode == 1 && data.drop == 1 % Mode A, start Local MPC when drop occurs