% predictive plasma current and shape controller
% Samo Gerksic 2013

clear all
%=============================================================
% this is from the genmod_Samo script for VS 
%  (for the NewEquilibrium scheme - gaps needed for CSC)
%  (E F matrices for bp li disturbance input not used)

%%% Load ITER equilibrium (plasma linearized model)
%modelFile  = 'LinearModel_#052_Ip15d00_li0d80_bp0d10_div';    %LM52.mat
%modelFile  = 'LinearModel_#053_Ip15d00_li1d00_bp0d10_div';   %LM53.mat
%modelFile  = 'LinearModel_#059_Ip15d00_li0d60_bp0d60_div';   %LM59.mat
%modelFile  = 'LinearModel_#060_Ip15d00_li0d80_bp0d60_div';   %LM60.mat
modelFile  = 'Equil_00019_CL.mat';                           %LMVS.mat 'nominal' in the VS paper Model 1 (nominal), Ip=14.5 MA, bp=0.11, li=0.85
% NewEquilibrium WS.mat ITER_December_2009\822130_t0d - no model file; in the VS paper Model 2, Ip=15 MA, bp=0.1, li=1.21

% for LMVS
%!!! not compatible with the original SC, gaps are different
%!!! ad-hoc choice copied, so that dimensions match (gap names exist)
ControlledGapName = {'GAP21','GAP26','GAP15','GAP12','GAP10','GAP03' };

% for LM52-60
% ControlledGapName = {'GAP21','GAP26','GAP15','GAP12','GAP10','GAP03' };
   % planned originally but GAP26 is not defined resulting in NaN
% ControlledGapName = {'GAP21','GAP21','GAP15','GAP12','GAP10','GAP03' };
   % to work around the NaN for VS experimentation, happens to do sth with SC too
% ControlledGapName = {'GAP21','GapRSO_low','GAP15','GAP12','GAP10','GAP03' };
  %you need to modify the gain after the model of the gap diagnostics in the shape controller
  %from [sin66 -cos23 1 1 1 1]
  %to [sin66 1 1 1 1 1]
  %(in this way it is possible to use GapRSO_low in place of GAP26...indeed the -cos23 gain is used to project GAP26 on the direction of GapRSO_low)
  %the modified schemes ..._LMnnA take care of this, and also scale bp li disturbance with Ipl 
% gaps in the NewEquilibrium model in WS.mat (modelLello): GAP37 GAP39 GAP10 GAP15 GAP21 GAP29
% gaps for the 'nominal' LMVS.mat 'Equil_00019_CL.mat': unknown!!! 
%   (happens to work but SOF with NE parameters goes unstable while stable without SC)

LinearModel = load(modelFile);  
LinearModel = LinearModel.LinearModel;

PFName = {'ICS1','ICS2U','ICS2L','ICS3U','ICS3L','IPF1','IPF2','IPF3','IPF4','IPF5','IPF6','IVS3'};
PFindex = signalIndexByName(PFName,LinearModel.PoloidalCircuits.Name,LinearModel.PoloidalCircuits.StatePosition);
PFindexOut = signalIndexByName(PFName,LinearModel.PoloidalCircuits.Name,LinearModel.PoloidalCircuits.OutputPosition);
% PF indexes in the input vector (here VS1 is added)
PFNameIn = {'VCS1','VCS2U','VCS2L','VCS3U','VCS3L','VPF1','VPF2','VPF3','VPF4','VPF5','VPF6','VS3','VS1'};
PFindexIn = signalIndexByName(PFNameIn,LinearModel.InputsInfo.Name,[1:length(LinearModel.InputsInfo.Name)]);
% Indexes for the other outputs 
ZCindexout = signalIndexByName('CV-ZC',LinearModel.OutputsInfo.Name,LinearModel.OutputsInfo.OutputPosition);
IpIndexout = LinearModel.PlasmaCurrentInfo.OutputPosition;
ControlledGapIndexout = signalIndexByName(ControlledGapName,LinearModel.OutputsInfo.Name,LinearModel.OutputsInfo.OutputPosition); 
XPointName = {'CV-RX','CV-ZX'};
XPointIndexout = signalIndexByName(XPointName,LinearModel.OutputsInfo.Name,LinearModel.OutputsInfo.OutputPosition); 

GapName = {};
for jj = 1:28
    GapName{end+1} = sprintf('GAP%02d',jj);
end
GapIndexout = signalIndexByName(GapName,LinearModel.OutputsInfo.Name,LinearModel.OutputsInfo.OutputPosition);
nanIndex = find(isnan(LinearModel.YEquil(GapIndexout)));
GapName = GapName(setdiff([1:length(GapName)],nanIndex));
nanIndex = GapIndexout(nanIndex);
GapIndexout = setdiff(GapIndexout,nanIndex);
Gaplength = length(GapIndexout);

% Build model matrices for Simulink simulation 
%
% x' = Ax+Bu+Ew'
% y = Cx+Du+Fw
%
% The INPUT vector holds the voltages applied to the following cicuits:
% CS1, CS2U, CS2L, CS3U, CS3L , PF1-PF6, VS3 (in-vessel coils), VS1
%
% The DISTUBANCES are:
% - betap*Ip
% - li*Ip
%
% The OUTPUT vector holds:
% - the currents in the CS/PF/VS3 coils (typically 12 elements)
% - the plasma current Ip
% - all the gaps defined in the model (i.e., the gaps not equal to NaN)
% - the x-point descriptors
% - the plasma vertical position z_c
% - the plasma vertical velocity \dot{z}_c

A_contr = -inv(LinearModel.L)*LinearModel.R;
B_contr = inv(LinearModel.L)*LinearModel.S(:,PFindexIn);
E_contr = -inv(LinearModel.L)*LinearModel.LE;
C_contr = LinearModel.C([PFindexOut ZCindexout IpIndexout ControlledGapIndexout GapIndexout XPointIndexout],:); 
D_contr = LinearModel.D([PFindexOut ZCindexout IpIndexout ControlledGapIndexout GapIndexout XPointIndexout],PFindexIn);  
F_contr = LinearModel.F; 
F_contr = F_contr([PFindexOut ZCindexout IpIndexout ControlledGapIndexout GapIndexout XPointIndexout],:);

%Czdot = C_contr(end,:)*A_contr;
%Dzdot = C_contr(end,:)*B_contr;
Czdot = C_contr(13,:)*A_contr;
Dzdot = C_contr(13,:)*B_contr;

C_contr = [C_contr; Czdot];
D_contr = [D_contr; Dzdot];
F_contr = [F_contr; 0 0];

% Change the model state in order to avoid the disturbance derivative in
% input.
% The new state is defined as z = x-Ew

A_w = A_contr;
B_w = B_contr;
C_w = C_contr;
D_w = D_contr;
E_w = A_contr*E_contr;
F_w = C_contr*E_contr+F_contr;

%Disturbances
% VDE
% The VDE is specified in cm 

vde = -20.0;

[VV,DD] = eig(A_w);
idx_unstable = find(diag(DD>1e-3));
%Cnew = C_contr(end-1,:)*VV;
Cnew = C_contr(13,:)*VV;
z0=1e-2;
xnewi0 = z0/Cnew(idx_unstable);
xxnewi0 = zeros(size(A_w,1),1);
xxnewi0(idx_unstable) = xnewi0;
xi0 = VV*xxnewi0*vde;

% Plasma current at the equilibrium 
Ip0 = LinearModel.XEquil(LinearModel.PlasmaCurrentInfo.StatePosition);

% Example of plasma disturbance 
% H-L transition
tt1 = 0:.001:150e-3;
tt2 = 0:.01:15;
tau1 = 25e-3;
tau2 = 0.36;
expon1 = exp(-tt1/tau1);
expon2 = exp(-tt2/tau2);
betap = [0 0.1 tt1+0.1 tt2+0.1+tt1(end); ...
    0*Ip0 0*Ip0 -(1-expon1)*Ip0*0.05 -(1-exp(-tt1(end)/tau1))*Ip0*0.05+(-1+expon2)*.4*Ip0]';
li = [0 0.1 tt1+0.1 tt2+0.1+tt1(end); ...
    0*Ip0 0*Ip0 (1-expon1)*Ip0*0.05 (1-exp(-tt1(end)/tau1))*Ip0*0.05+(1-expon2)*.1*Ip0]';
 
ReleON = 100;
ReleOFF=ReleON/20;
UB = 400;
sigma = .58;
B = 1000;
Ksat=300/205;

% vars for the "NewEquilibrium" scheme
modelLello = LinearModel; 
PFindexout = PFindexOut; 
load ShapeCtrl.mat
% aa bb cc dd aa1 bb1 cc1 dd1 aa2 bb2 cc2 dd2 Kp Ki Kx sin66 cos23 Vup Vdown Turns


%==========================================================
% ctLQG VS design
% parts picked up from the SOF VS design script

% Plasma model
plasma = ss(A_w,B_w,C_w,D_w);

% Diagnostic
diagnostic = tf(1,[7e-3 1]);

% Power Supply
ps = tf(1,[7.5e-3 1]);
ps.InputDelay = 2.5e-3;
ps_p = pade(ps,3);


% Subsystem from the plasma model: 
%  inputs: U_VS3 (12) and U_VS1 (13) 
%%%  outputs: I_VS3 (12) and dotZc (15)     modified!
%[PFindexOut ZCindexout IpIndexout ControlledGapIndexout GapIndexout 
%    XPointIndexout], plus dotZc
plasmax = plasma([12 end],[12 13]);
% Append power supplies 
% VS3 ps (above)
% VS1 Power Supply
ps1 = tf(1,[7.5e-3 1]);
ps1.InputDelay = 2.5e-3;
ps1_p = pade(ps1,3);
% merge them 
ps_both = [ps, 0; 0, ps1];
ps_both_p = [ps_p, 0; 0, ps1_p];
% connect in series 
sys_tmp1 = series(ps_both, plasmax, [1 2], [1 2]); 
sys_tmp1_p = series(ps_both_p, plasmax, [1 2], [1 2]); 
% Append diagnostics model on dotZc
diags = tf(1, [7e-3 1]);
diags_both = [1 0; 0 diags]; 
plant0 = series(sys_tmp1, diags_both, [1 2], [1 2]); 
plant0_p = series(sys_tmp1_p, diags_both, [1 2], [1 2]); 
plant0m = minreal(plant0);
plant0m_p = minreal(plant0_p);

% new model 
load ne.mat %A_wn B_wn C_wn D_wn E_wn F_wn
plasman = ss(A_wn,B_wn,C_wn,D_wn);
% VS3: input 12
% VS1: PF voltage inputs 7..10 [0 0 0 0 0 0 1 1 -1 -1 0 0]
%%% (VS1 input could also be used)
% IVS3: output 12 
% ZcDot: last output (end)
plasma0n = plasman([12 end],:); 
plasmaxn = series([0 0 0 0 0 0 0 0 0 0 0 1;0 0 0 0 0 0 1 1 -1 -1 0 0]',plasma0n);
plasma1n = plasmaxn([1 2],1); 
sys_tmp1n = series(ps_both, plasmaxn, [1 2], [1 2]); 
sys_tmp1n_p = series(ps_both_p, plasmaxn, [1 2], [1 2]); 
plant0n = series(sys_tmp1n, diags_both, [1 2], [1 2]); 
plant0n_p = series(sys_tmp1n_p, diags_both, [1 2], [1 2]); 
plant0mn = minreal(plant0n);
plant0mn_p = minreal(plant0n_p);


% %-------------------------------------
% % ctLQG 3rd-order plus analysis, just check if same result
% [plant0r, REDINFOs] = schurmr(plant0m_p, 3);
% [plant0rn, REDINFOsn] = schurmr(plant0mn_p, 3);
% % continuous-time LQ
% QyCc = [1e-5 0; 0 1]; % output cost
% RCc = [8e-3 0; 0 1e-4]; % control cost
% [KCc, SCc, ECc] = lqry(plant0r, QyCc, RCc);
% % continuous-time estimator
% xdim = size(plant0r.a, 1);
% esc = plant0r;           % short
% escG = eye(xdim);
% escH = zeros(2, xdim); 
% sys_extc = ss(esc.A, [esc.B escG], esc.C, [esc.D escH]);
% % tune
% QKc = plant0r.b*plant0r.b'; %  state covariance -loop transfer recovery
% RKc = [1e-3 0; 0 1e-13]; % output covariance
% [kalfiltc, Lkalc, Pkalc] = kalman(sys_extc, QKc, RKc); 
% eig(esc.a - Lkalc*esc.c)
% % single block LQG http://www.mathworks.com/help/toolbox/control/ref/lqgreg.html
% LQGrc = lqgreg(kalfiltc,KCc);    
% 
% % analysis 
% % original scheme as 2x2; loop 1 cut before VS3
% K1 = tf(2300,[1]);
% k2 = -6e-6*2;
% k3 = 0.2; 
% K1_both = [K1 0; 0 1];
% k2k3 = [k2 1; k3 0];
% K1k2k3 = series(k2k3, K1_both); 
% olsys = series(plant0m, K1k2k3); 
% olsysn = series(plant0mn, K1k2k3); 
% olfn1 = feedback(olsys, 1, 2, 2, 1); 
% olfn1n = feedback(olsysn, 1, 2, 2, 1); 
% % LQG
% plantctrl = series(plant0m, -LQGrc); 
% plantctrln = series(plant0mn, -LQGrc); 
% fcn1 = feedback(plantctrl, 1, 2, 2); 
% fcn1n = feedback(plantctrln, 1, 2, 2); 
% figure, nyquist(olfn1(1,1),'b', fcn1(1,1),'g', olfn1n(1,1),'c', fcn1n(1,1),'y');
% legend('SOF - model 1','ctLQG - model 1','SOF - model 2','ctLQG - model 2','Location','NorthWest')
% figure, nichols(olfn1(1,1),'b', fcn1(1,1),'g', olfn1n(1,1),'c', fcn1n(1,1),'y');




%-------------------------------------
% ctLQGz: additional control of zp to 0

%3rd order model
% reduced-order model 
[plant0r, REDINFOs] = schurmr(plant0m_p, 3);
[plant0rn, REDINFOsn] = schurmr(plant0mn_p, 3);
% append an integrator dotZc -> Zc at output 2 (dotZc)
% (assume both with diagnostics delay - from IVS1 different)
nx = size(plant0r.a, 1);
nu = size(plant0r.b, 2);
ny = size(plant0r.c, 1);
yi = 2; % output index
%Aa = [plant0r.a zeros(nx,1); plant0r.c(yi,:) 1];
Aa = [plant0r.a zeros(nx,1); plant0r.c(yi,:) 0];
Ba = [plant0r.b; plant0r.d(yi,:)];
Ca = [plant0r.c zeros(ny,1); zeros(1,nx) 1];
Da = [plant0r.d; zeros(1,nu)];
Aan = [plant0rn.a zeros(nx,1); plant0rn.c(yi,:) 0];
Ban = [plant0rn.b; plant0rn.d(yi,:)];
Can = [plant0rn.c zeros(ny,1); zeros(1,nx) 1];
Dan = [plant0rn.d; zeros(1,nu)];
plant0ra = ss(Aa, Ba, Ca, Da); 
plant0ran = ss(Aan, Ban, Can, Dan); 
% continuous-time LQ
QyCc = diag([1e-5 1 2e2]); % output cost
RCc = [8e-3 0; 0 1e-4]; % control cost
[KCc, SCc, ECc] = lqry(plant0ra, QyCc, RCc);
eig(plant0ra.a - plant0ra.b*KCc)
% continuous-time estimator
xdim = size(plant0ra.a, 1);
esc = plant0ra;           % short
escG = eye(xdim);
escH = zeros(ny+1, xdim); 
sys_extc = ss(esc.A, [esc.B escG], esc.C, [esc.D escH]);
% tune
QKc = plant0ra.b*plant0ra.b' + diag([zeros(1,nx) 1]); %  state covariance LTR+
RKc = diag([1e-3 1e-13 1e-15]); % output covariance
[kalfiltc, Lkalc, Pkalc] = kalman(sys_extc, QKc, RKc); 
eig(esc.a - Lkalc*esc.c)
% single block LQG http://www.mathworks.com/help/toolbox/control/ref/lqgreg.html
LQGrc = lqgreg(kalfiltc,KCc);    

% % ctLQG analysis with the additional zp loop 
% % analysis 
% % original scheme as 2x2; loop 1 cut before VS3
% K1 = tf(2300,[1]);
% k2 = -6e-6*2;
% k3 = 0.2; 
% K1_both = [K1 0; 0 1];
% k2k3 = [k2 1; k3 0];
% K1k2k3 = series(k2k3, K1_both); 
% olsys = series(plant0m, K1k2k3); 
% olsysn = series(plant0mn, K1k2k3); 
% olfn1 = feedback(olsys, 1, 2, 2, 1); 
% olfn1n = feedback(olsysn, 1, 2, 2, 1); 
% % LQG
% plantctrl = series(plant0ra, -LQGrc); 
% plantctrln = series(plant0ran, -LQGrc); 
% fcn1 = feedback(plantctrl, 1, 2, 2); 
% fcn1n = feedback(plantctrln, 1, 2, 2); 
% figure, nyquist(olfn1(1,1),'b', fcn1(1,1),'g', olfn1n(1,1),'c', fcn1n(1,1),'y');
% legend('SOF - model 1','ctLQG - model 1','SOF - model 2','ctLQG - model 2','Location','NorthWest')
% %figure, nichols(olfn1(1,1),'b', fcn1(1,1),'g', olfn1n(1,1),'c', fcn1n(1,1),'y');
% %figure, bode(olfn1(1,1),'b', fcn1(1,1),'g', olfn1n(1,1),'c', fcn1n(1,1),'y');




%=================================================================
% predictive CSC

% plasma model inputs: PFindexIn
% PFNameIn = {'VCS1','VCS2U','VCS2L','VCS3U','VCS3L','VPF1','VPF2','VPF3','VPF4','VPF5','VPF6','VS3','VS1'};
% plasma model outputs: 
% [PFindexOut ZCindexout IpIndexout ControlledGapIndexout GapIndexout 
%    XPointIndexout], plus zcdot (via Czdot Dzdot) 
% PFName = {'ICS1','ICS2U','ICS2L','ICS3U','ICS3L','IPF1','IPF2','IPF3','IPF4','IPF5','IPF6','IVS3'}; and Ip Zc Zcdot  


% open-loop model for both VS and CSC
%  13 inputs: all VPF (1-11), VS3 (12) and VS1 (13)
%  21 outputs: all IPF (1-11), IVS3 (12), Zc (13), Ip (14), CtrlGaps (15-20), dotZc (end),  
%    added CtrlGaps! (AllGaps if the CSC should control all gaps)
plasmaOL = plasma([1:11 12 13  14 15:20 end],[1:11 12 13]);

% append the power supplies
% VS3 PS above: ps, ps_p
% VS1 PS above: ps1, ps1_p
% MC PS (single): psMC, psMC_p
psMC = tf(1,[0.015 1]);
psMC.InputDelay = 0.015;
psMC_p = pade(psMC,3);
% merge them 
ps_all = tf(zeros(13,13));
for ix = 1:11, ps_all(ix,ix) = psMC; end
ps_all(12,12) = ps; 
ps_all(13,13) = ps1; 
ps_all_p = tf(zeros(13,13));
for ix = 1:11, ps_all_p(ix,ix) = psMC_p; end
ps_all_p(12,12) = ps_p; 
ps_all_p(13,13) = ps1_p; 
% connect in series 
sys_temp1 = series(ps_all, plasmaOL, [1:13], [1:13]); 
sys_temp1_p = series(ps_all_p, plasmaOL, [1:13], [1:13]); 

% Append diagnostics model
diags_Z = tf(1, [7e-3 1]);
% as in the original scheme:
diags_IPF = tf(1, [.15 1]);   %!too much, but shouldn't matter
diags_IVS3 = 1;               %(evades the filters in the SC!)
diags_Ip = tf(1, [.15 1]);    %!too much, may matter
diags_Gaps = tf(1, [.15 1]);
% actual ITER IPF measurement spec: 400us delay + 200us first order lag
%   ... use 1ms delay + 5ms FO?
%   for now, use the same lags as in the original CSC
% build
diags_all = tf(zeros(21,21));
for ix = 1:11, diags_all(ix,ix) = diags_IPF; end    
diags_all(12,12) = diags_IVS3; 
diags_all(13,13) = diags_Z; 
diags_all(14,14) = diags_Ip; 
for ix = 15:20, diags_all(ix,ix) = diags_Gaps; end
diags_all(21,21) = diags_Z; 

plantOL = series(sys_temp1, diags_all, [1:21], [1:21]); 
plantOL_p = series(sys_temp1_p, diags_all, [1:21], [1:21]); 
plantOLm = minreal(plant0);
plantOLm_p = minreal(plant0_p);

% make the VS feedback loops of ctLQGz (no constraints...)
%%% ctLQG: 
%%%  plant inputs: U_VS3 (12), U_VS1 (13) 
%%%  plant outputs: I_VS3 (12), dotZc (21), Zc (13)
plantVS = feedback(plantOL, LQGrc, [12 13], [12 21 13], 1);
plantVS_p = feedback(plantOL_p, LQGrc, [12 13], [12 21 13], 1);
% SYS = FEEDBACK(SYS1,SYS2,FEEDIN,FEEDOUT,SIGN)
% The vector FEEDIN contains indices into the input vector of SYS1
% and specifies which inputs u are involved in the feedback loop.
% Similarly, FEEDOUT specifies which outputs y of SYS1 are used for
% feedback.  
% If SIGN=1 then positive feedback is used.  (with lqgreg)
% The resulting LTI model SYS has the same inputs and outputs as SYS1

% subsystem for CSC: strip the VS signals
%  plantVS: 
%  13 inputs: all VPF (1-11), VS3 (12) and VS1 (13)
%  21 outputs: all IPF (1-11), IVS3 (12), Zc (13), Ip (14), CtrlGaps (15-20), dotZc (end),  
%  plantCSC: 
%  11 inputs: all VPF (1-11)
%  21 outputs: the same, sans dotZc 
%  (just put zero output cost if you don't want to control it)  
%  *** but make sure the targets are not in conflict
%  *** and don't demand offset-free tracking (OSD integrators)
%  *** maybe inner loop signals should be removed (Zc - depends on which VS)
%       VS3, IVS3
plantCSC = plantVS([1:20],[1:11]);
plantCSC_p = plantVS_p([1:20],[1:11]);

% compare step responses with simulink: sim_shape_May_2012_s1_zp_OLTEST.mdl
% input block: constant MV:VPF 11x1 (one 1, the rest 0) 
%   amplitude (below saturations)
% outputs: CV_plasma vs CV_pCSC vs CV_pCSC_p
%   run simulation... compare outputs... check other inputs
% +simulacija zašteka... shema za LMNE, načrtovanje z LMVS! manjkalo...
% +dimensions... 
%  Plant model size(ss(A_w, [B_w E_w], C_w, [D_w F_w])): 15x50
%  sum([12 1 1 6 Gaplength+2 1]) = 51!!!,  Gaplength=28
%  ...je bil napačen seznam CtrlGaps, eden ni obstajal (pravega sicer ni)
%  Še na vhodu Plant model, LMVS ima 15 vhodov, LMNE shema 14, ker je VS1 vezan čez K
%  ...dodatni vhod 15 = konstanta 0  
% figure, plot(CV_plasma.time, CV_plasma.signals.values(:,1),'k', CV_pCSC.time, CV_pCSC.signals.values(:,1),'b', CV_pCSC_p.time, CV_pCSC_p.signals.values(:,1),'g')
% ix=2; figure, plot(CV_plasma.time, CV_plasma.signals.values(:,ix),'k', CV_pCSC.time, CV_pCSC.signals.values(:,ix),'b', CV_pCSC_p.time, CV_pCSC_p.signals.values(:,ix),'g')

% minimal realisation 
%plantCSCm = minreal(plantCSC_p);
% !!! MUCH difference at LF in Bode and short steps - do not use !!!
% sminreal: does nothing!

% model reduction 
%[plantCSCr, REDINFOs] = schurmr(plantCSC_p, 20);  
%   this reduction no good here, resulting models sometimes unstable etc!
%plantCSCr = balred(plantCSC_p, 100);
% default method: 'MatchDC', matches DC gain
% looks better, except that D becomes non-zero 
% reset D matrix (appears after balred but not present originally)
%plantCSCr.D = zeros(size(plantCSCr.D));
% resetting D to zero causes a small DC offset, Bode looks worse 
%  (midF ok, but peaks down at HF and LF edges)
% besides, the original model isn't perfect at DC 
% (no true integrators and derivatives, deep-LF poles and zeros instead)
% sth else probably needed for a steady-state target calculator
plantCSCr = balred(plantCSC_p, 100, 'Elimination', 'Truncate');
%...what order? (for the whole reduced model, not one branch)

% see one branch (Zc from first coil)
figure, bode(plantCSC_p(13,1), plantCSCr(13,1))
% step function is not very reliable (uses c2d conversion - do not use long simulation time)
figure, step(plantCSC_p(13,1), plantCSCr(13,1), 25)

% how about integrators and derivatives in the model?
%  not there, only very-low-freq poles and zeros; not accurate at steady-state!
%  but integrating zdot matches Zc over the medium frequencies and
%  deriving Zc matches zdot over the medium frequencies
% in plantCSC_p, zdot looks like a derivative at LF down to 1e-9 
%  then straightens out, with DC gain of -280dB instead of -Inf
%  with balred 100 'Truncate' only down to 1e-2, DC gain -160dB
% many other "off-diagonal" interactions also show derivative response
%  iii= 1; iio= 2; figure, bode(plantCSC_p(iii,iio), plantCSCr(iii,iio))
%  DC gain matches with proprotional ones
% Vcoil(1) to gap(16) : I response (in the relevant range; low freq P again)
%  *** check if ss offset with step disturbances at the output is suppressed (not at the input)

% predictive CSC with MPT (template posoda3 and itervs2)
% concept:
% (no tracking)
% offset-free control of Ip (14), CtrlGaps (15-20) to 0
%   (?OSD integrators for disturbance model - maybe not needed if I process)
% P control of IPF (1-11) to 0 
% mind IVS3 (12), Zc (13), IVS1 (sum of IPF....)  (!persistent disturbance)
% no control: dotZc (21) - (removed!)

% horizons: 
%   closed-loop settling time should be around 25 s 
%   open-loop longer, and integrating response 
%     (model is in fact near-integrating, inf horizon may not work well)
%   ...let's try Ts=0.5s, N=50, Nc shorter

Ts = 0.5;
sysd = c2d(plantCSCr, Ts, 'zoh'); 
[Ad, Bd, Cd, Dd] = ssdata(sysd);

% model for MPT
sysStruct = [];
sysStruct = mpt_sys(sysd, Ts);
sysStruct.umin = [-1.5e3;  -6e3]; %*****************
sysStruct.umax = [1.5e3; 6e3];  %*****************
%sysStruct.dumin = [];
%sysStruct.dumax = [];
%sysStruct.xmin = [];
%sysStruct.xmax = [];
%sysStruct.Pbnd = [];
sysStruct.ymin =  [ -5e4; -5];  %*****************
sysStruct.ymax =  [  1.8e4;  5];  %***************** 

% discrete-time LQG (for comparison, terminal ctrl)
plant0rd = c2d(plant0r, Ts, 'zoh'); 
% output LQ controller
QyC = diag([]); % output cost *********
RC = diag([]); % control cost *********
[KC, SC, EC] = lqry(sysd, QyC, RC);
% equivalent state-cost LQR (mpt does not fully support output-cost)
QxC = Cd'*QyC*Cd;
RxC = RC + Dd'*QyC*Dd;
NxC = Cd'*(QyC*Dd);        % (cross-term is zero if Dd is zero)
QxC = (QxC+QxC')/2;
RxC = (RxC+RxC')/2;
[KxC, SxC, ExC] = lqr(sysd, QxC, RxC, NxC);

probStruct = [];
probStruct.N = 50;% % iexp;
probStruct.inputblocking = [2 2 46];
probStruct.norm = 2; %2;
probStruct.subopt_lev = 0;  
probStruct.tracking = 0;  
probStruct.Tconstraint = 0;% 1 use LQR terminal set (NO with Qy), 2 use provided .Tset
%***probstruct.Tset = ... mpt invariantSet
probStruct.feedback = 0;
%probStruct.FBgain = KxC;   %(otherwise a state-cost LQR will be computed automatically)

% probStruct.Q = diag([1e-20, 1e-20, 1e-20]);    %states 
% probStruct.Qy = QyC; % output cost
% probStruct.R = RC; % control cost
% probStruct.P_N = inv(Cd*Cd')*Cd*SC*Cd'*inv(Cd*Cd');
probStruct.Q = QxC; 
% no probStruct.Qy 
probStruct.R = RxC; % control cost
probStruct.P_N = SxC;   % terminal state cost
% %probStruct.Rdu = diag([10 .01])  %control move cost

onlc = mpt_control(sysStruct, probStruct, 'online');

% %%%% Disturbance Estimation %%%% %%%% %%%%
xdim = size(sysd.a, 1);
es = sysd;           % short
esG = eye(xdim);
esH = zeros(2, xdim); 
sys_ext = ss(es.A, [es.B esG], es.C, [es.D esH], Ts);
QK = sysd.b*sysd.b'; %  state covariance -loop transfer recovery
RK = [1e-3 0; 0 1e-13]; % output covariance
[kalfilt, Lkal, Pkal] = kalman(sys_ext, QK, RK); 
eig(es.a - Lkal*es.c)
kalfiltD = kalfilt.d;
kalfiltABC = kalfilt;
kalfiltABC.d = zeros(size(kalfilt.d));

% Simulink scheme








