diff --git a/README.md b/README.md
index dd93048788ce2d675a333bfc276530fb2806e234..b14e865906b50a1d8cdb872e5b30f04e413b78e0 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,10 @@
-# 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.
diff --git a/execinfo.slx b/execinfo.slx
new file mode 100644
index 0000000000000000000000000000000000000000..c2f35bb6b78ecdeb9f0955f1661a662480588159
Binary files /dev/null and b/execinfo.slx differ
diff --git a/freqinfo.m b/freqinfo.m
new file mode 100644
index 0000000000000000000000000000000000000000..a4af5da84dfefdc4203caf600d80ccb9c555b996
--- /dev/null
+++ b/freqinfo.m
@@ -0,0 +1,15 @@
+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()
diff --git a/getdlq.m b/getdlq.m
new file mode 100644
index 0000000000000000000000000000000000000000..78991539c10ac8b7f33e25ae05c3a0db70e9110f
--- /dev/null
+++ b/getdlq.m
@@ -0,0 +1,9 @@
+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
+
diff --git a/init.m b/init.m
new file mode 100644
index 0000000000000000000000000000000000000000..349ec5b5b71c542b0aea9b9050140ecd2f63e80b
--- /dev/null
+++ b/init.m
@@ -0,0 +1,28 @@
+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 = {};
diff --git a/init_mpc.m b/init_mpc.m
new file mode 100644
index 0000000000000000000000000000000000000000..ac3d8deb50e834c8bbdb1ee568df3129367c7365
--- /dev/null
+++ b/init_mpc.m
@@ -0,0 +1,22 @@
+% 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;
+
+
diff --git a/lqgsamp.m b/lqgsamp.m
new file mode 100644
index 0000000000000000000000000000000000000000..f6d3d5d6e3a8fe25bf5afb1aea1ca1c0919abdf3
--- /dev/null
+++ b/lqgsamp.m
@@ -0,0 +1,57 @@
+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);
diff --git a/metrics.slx b/metrics.slx
new file mode 100644
index 0000000000000000000000000000000000000000..5b5905c6b4da86209a0ce23d8c6bfd808f049291
Binary files /dev/null and b/metrics.slx differ
diff --git a/rsmpc.slx b/rsmpc.slx
new file mode 100644
index 0000000000000000000000000000000000000000..ed4b33f5dd3535e9bbce7c45e2ebcc6ef23201c8
Binary files /dev/null and b/rsmpc.slx differ
diff --git a/runquadprog.m b/runquadprog.m
new file mode 100644
index 0000000000000000000000000000000000000000..d5dc8e09055680ceb79ead2350a49c209b3a44d9
--- /dev/null
+++ b/runquadprog.m
@@ -0,0 +1,53 @@
+% 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);
+
diff --git a/simrsmpc.m b/simrsmpc.m
new file mode 100644
index 0000000000000000000000000000000000000000..b0df266a2dd1f897619d76f8d72fbfa8e7f7d9c1
--- /dev/null
+++ b/simrsmpc.m
@@ -0,0 +1,124 @@
+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
diff --git a/sysmodel.m b/sysmodel.m
new file mode 100644
index 0000000000000000000000000000000000000000..7d32c073e31d2ffc4240244a6002ee11971709e6
--- /dev/null
+++ b/sysmodel.m
@@ -0,0 +1,16 @@
+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'};
diff --git a/tt_rsmpc_init.m b/tt_rsmpc_init.m
new file mode 100644
index 0000000000000000000000000000000000000000..2234121c5aa8d1d4dab98bafc97b158ebf2f4838
--- /dev/null
+++ b/tt_rsmpc_init.m
@@ -0,0 +1,55 @@
+% 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
+
diff --git a/tt_rsmpc_main.m b/tt_rsmpc_main.m
new file mode 100644
index 0000000000000000000000000000000000000000..d628c0f0b7e182aedb69a303ab9954b047b8035a
--- /dev/null
+++ b/tt_rsmpc_main.m
@@ -0,0 +1,223 @@
+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
+ if data.local_run_at == -1 % New drop
+ data.local_run_at = data.k;
+ end
+ elseif data.mode == 2 || data.mode == 3
+ if data.local_run_at == -1
+ data.local_run_at = data.k;
+ end
+ elseif data.mode == 0 && data.local_run_at == -1 % Local only, start and keep running
+ data.local_run_at = data.k;
+ end
+
+ case 7
+ if data.local_run_at == data.k
+ x = data.mpcL.A*data.xk + data.mpcL.B*data.u;
+ [ox, fv, fl, op] = runquadprog(data.mpcL, x, data.xref, data.NL, data.NL);
+ data.flag_j = fl;
+ data.pend_j = data.k;
+ if fl == 1
+ data.pend_Lu = ox(data.NL*3+1);
+ else
+ data.u = 0; % Hm....
+ end
+ data.local_run_at = data.k+2;
+ data.localExec = 1;
+ end
+ ttAnalogOut(3, data.localExec);
+ data.localExec = -1*data.localExec;
+
+ case 8
+ exectime = 0.01;
+
+ case 9
+ exectime = -1;
+ end
+end