diff --git a/@datadriven/emptyStruct.m b/@datadriven/emptyStruct.m index 14c4f1b..c633c99 100644 --- a/@datadriven/emptyStruct.m +++ b/@datadriven/emptyStruct.m @@ -1,20 +1,20 @@ function [SYS, OBJ, CON, PAR] = emptyStruct() % datadriven.emptyStruct() % % Author: Philippe Schuchert % Philippe.schuchert@epfl.ch % EPFL % June 2021 % % Empty structures required for the datadriven solve function. ctrl = struct('num',[],'den',[],'Ts',[],'Fx',[],'Fy',[]); SYS = struct('model',[],'W',[],'controller',ctrl); -PAR = struct('tol',1e-6,'maxIter',50,'radius',1,'robustNyquist',true,'solveForm','dual','radius_zeros',[]); +PAR = struct('tol',1e-6,'maxIter',50,'radius',1,'robustNyquist',true,'solveForm','dual','min_dcgain_num',[]); OBJ = struct('inf',struct('meanNorm',1,'W1',[],'W2',[],'W3',[],'W4',[]),... 'two', struct('meanNorm',1,'W1',[],'W2',[],'W3',[],'W4',[])); CON = struct('W1',[],'W2',[],'W3',[],'W4',[]); end \ No newline at end of file diff --git a/@datadriven/private/solveddlpv.m b/@datadriven/private/solveddlpv.m index 2030141..a5e8d0e 100644 --- a/@datadriven/private/solveddlpv.m +++ b/@datadriven/private/solveddlpv.m @@ -1,389 +1,402 @@ function [controller,sol,diagnostic] = solveddlpv(system,objective,constraint,parameters,sol) % solveddlpv % % Author: Philippe Schuchert % Philippe.schuchert@epfl.ch % EPFL % June 2021 % % Solve the problem using MOSEK 9.2 + MOSEK Fusion controller = system.controller; import mosek.fusion.*; Ts = controller.Ts; W = system.W; M = Model('DATA-DRIVEN OPTIMIZATION'); nCon = length(W); % number of constraint nMod = length(system.model); % number of different models autoScalingObj = 1; if isempty(sol) % solution struct is not provided: 1st iter sol.satisfyConstraints = 0; sol.nIter = 0; sol.H2 = 0; sol.Hinf = 0; else if (sol.H2 || sol.Hinf) - % scale such that objective is approx 100 - autoScalingObj = 0.01*sol.obj; + % scale such that objective is approx 2 (better nuerically scaled) + autoScalingObj = 1;%sol.obj; end end -autoScaling = 100*(max(abs(controller.num),[],'all')); +autoScaling = max(abs(controller.num),[],'all'); if sol.satisfyConstraints % If last solution almost satisfies the constraint, fix slack and % optimize over the objective slack = Expr.constTerm(zeros(2,1)); - sol.slack = 0; + sol.slack = 0; + sol.satisfyconstraint = 1; + else + sol.satisfyConstraints = 0; slack = M.variable('slack',2,Domain.greaterThan(0)); end %% -Xlpv = controller.num/autoScaling; % rescale numerator -Ylpv = controller.den; +Xlpv = controller.num/autoScaling; +Ylpv = controller.den; % rescale numerator [szx,ntheta] = size(Xlpv); % number of parameters num [szy,~] = size(Ylpv); % number of parameters num % Controller coefficient variables X = M.variable('X',[szx,ntheta]); Y = M.variable('Y',[szy,ntheta]); for ii = 1 : (ntheta)-1 % 1st coefficient of Y has do dependency on scheduling paramters M.constraint(Y.index([0,ii]), Domain.equalsTo(0)); % leading coefficient does not depend on scheduling parameter(s) end M.constraint(Y.index([0,0]), Domain.equalsTo(1)); z = resp(tf('z',Ts),W); Zy = z.^((szy-1):-1:0); % [0,z,...,z^nx] Zx = z.^((szx-1):-1:0); % [0,z,...,z^ny] ZFy = Zy.*resp(tf(controller.Fy,1,Ts),W); ZFx = Zx.*resp(tf(controller.Fx,1,Ts),W); %% %{ OBJECTIVE : 1st step find controller that satisfies the constraint, then optimize over the objective using constraint. Can be changed using the softconstraint field, but it is generally not advised (root cause: constraint impossible to achieve, "bad" system, etc..). %} % H_infinity objectives gamma_inf = M.variable('gamma_inf',[1,1],Domain.greaterThan(0)); gamma_inf_mmod = M.variable('gamma_inf_mmod',[1,nMod],Domain.greaterThan(0)); if objective.inf.meanNorm M.constraint(Expr.sub(gamma_inf,Expr.mul(1/nMod,Expr.sum(gamma_inf_mmod))),Domain.greaterThan(0)); else M.constraint(Expr.sub(Expr.mul(gamma_inf,Matrix.dense(ones(1,nMod))),gamma_inf_mmod),Domain.greaterThan(0)); end gamma_Inf = Expr.mul(Matrix.dense(ones(nCon,1)),gamma_inf_mmod); % H_2 objectives integ = Ts/(2*pi)*([diff(W(:));0] + [W(1);diff(W(:))]); gamma_2_mmod = M.variable('gamma2',[nCon,nMod],Domain.greaterThan(0)); meanNorm = Expr.constTerm(zeros(1,1)); % do not "new" frequencies from the adaptive grid in the objective. % Objective may be non-decreasing because of the additionals frequency % points. obj_2 = M.variable('o_2',1,Domain.greaterThan(0)); % used for obj_2.level() if not(sol.satisfyConstraints) % some constraint not satified OBJ = Expr.sum(slack) ; else % all constraint are satified. Optimize over the H2/Hinf objectives OBJ = Expr.add(Expr.sum(gamma_inf), obj_2); end obj = M.variable('objective',1,Domain.greaterThan(0)); M.constraint(Expr.sub(obj,OBJ),Domain.greaterThan(0)); % for obj.level(); M.objective('obj', ObjectiveSense.Minimize, OBJ); for mod_ = 1: nMod % Get Local controller Q = controller.theta(system.model(:,:,mod_)); Q = Q(:); X_c = Xlpv*Q; Y_c = Ylpv*Q; X_n = Expr.mul(X,Matrix.dense(Q)); Y_n = Expr.mul(Y,Matrix.dense(Q)); XY_n = Expr.vstack(X_n,Y_n); + + Yc = ZFy*Y_c; Xc = ZFx*X_c; % Frequency response Kinit with the fixed parts %% Important (new) values PLANT = resp(system.model(:,:,mod_),W)*autoScaling; % Model Frequency response Pc = PLANT.*Xc + Yc; % previous "" Open-loop "" (without numerator Yc) Cp = [PLANT.*ZFx, ZFy]; % P = Cp*[X;Y], Cp complex, [X;Y] real, new "" Open-loop "" frequency response - %% STABILITY STUFF, MUY IMPORTANTE! - % Nyquist stability at |z| = infinity - if abs(sum(controller.Fy)) < 1e-4 - if X_c(1) - M.constraint(Expr.mul(sign(X_c(1)),X_n.index([0,0])),Domain.greaterThan(0)); + + if ~isempty(parameters.min_dcgain_num) + M.constraint(Expr.mul(Matrix.dense(sign(sum(X_c))),Expr.sum(X_n)),Domain.greaterThan(abs(parameters.min_dcgain_num)/autoScaling)); + else + if sum(abs(controller.Fy) < 1e-4) + % When integrator is present, make sure no sign change at low + % freq. + M.constraint(Expr.mul(Matrix.dense(sign(sum(X_c))),Expr.sum(X_n)),Domain.greaterThan(0)); end - M.constraint(Expr.mul(Matrix.dense(sign(sum(X_c))),Expr.sum(X_n)),Domain.greaterThan(0)); + end % Stability constraint if parameters.robustNyquist % close the polygonal chain PcExtended = [conj(Pc(1));Pc;conj(Pc(end))]; CpExtended = [conj(Cp(1,:));Cp;conj(Cp(end,:))]; dP = getNormalDirection(PcExtended); - CpExtended = CpExtended./abs(PcExtended); + x1_a = real(CpExtended(2:end,:).*conj(dP)); x1_b = real(CpExtended(1:end-1,:).*conj(dP)); - M.constraint(Expr.mul(Matrix.dense(x1_a),XY_n),Domain.greaterThan(1e-4)); - M.constraint(Expr.mul(Matrix.dense(x1_b),XY_n),Domain.greaterThan(1e-4)); + M.constraint(Expr.mul(Matrix.dense(x1_a),XY_n),Domain.greaterThan(1e-6)); + M.constraint(Expr.mul(Matrix.dense(x1_b),XY_n),Domain.greaterThan(1e-6)); end % Pole controller location if (parameters.radius) && (numel(Q)>1 || mod_==1) % (numel(Q)>1 || mod==1) : for non LPV controller, constraint poles % only once %close the polygonal chain - zExtended = [conj(z(1));1;1;z;conj(z(end))]; + zExtended = [conj(z(1));z;conj(z(end))]; ZyExtended = (parameters.radius*zExtended).^((szy-1):-1:0); YcsExtended= ZyExtended*(Y_c); dY = getNormalDirection(YcsExtended); - ZyExtended = ZyExtended./abs(YcsExtended); + x1_a = 2*real(conj(dY).*ZyExtended(2:end,:)); x1_b = 2*real(conj(dY).*ZyExtended(1:end-1,:)); - M.constraint(Expr.mul(Matrix.dense(x1_a),Y_n),Domain.greaterThan(1e-4)); - M.constraint(Expr.mul(Matrix.dense(x1_b),Y_n),Domain.greaterThan(1e-4)); + M.constraint(Expr.mul(Matrix.dense(x1_a),Y_n),Domain.greaterThan(1e-6)); + M.constraint(Expr.mul(Matrix.dense(x1_b),Y_n),Domain.greaterThan(1e-6)); end + %{ % Zero controller location + error("NOT USED") if ~isempty(parameters.radius_zeros) && (numel(Q)>1 || mod_==1) % (numel(Q)>1 || mod==1) : for non LPV controller, constraint poles % only once %close the polygonal chain - zExtended = [conj(z(1));1;1;z;conj(z(end))]; + + zExtended = [conj(z(1));z;conj(z(end))]; ZxExtended = (parameters.radius_zeros*zExtended).^((szx-1):-1:0); XcsExtended= ZxExtended*(X_c); dX = getNormalDirection(XcsExtended); x1_a = 2*real(conj(dX).*ZxExtended(2:end,:)); x1_b = 2*real(conj(dX).*ZxExtended(1:end-1,:)); - M.constraint(Expr.mul(Matrix.dense(x1_a),X_n),Domain.greaterThan(1e-5)); - M.constraint(Expr.mul(Matrix.dense(x1_b),X_n),Domain.greaterThan(1e-5)); + M.constraint(Expr.mul(Matrix.dense(x1_a),X_n),Domain.greaterThan(0e-6)); + M.constraint(Expr.mul(Matrix.dense(x1_b),X_n),Domain.greaterThan(0e-6)); end - + %} %% OBJECTIVES % only do if controller satisifies constraint - scaling = (abs(1./Pc)).^(3/2); - x1 = Expr.add( Expr.mul(2*real(Cp.*conj(Pc).*scaling),XY_n), -conj(Pc).*Pc.*scaling ); + x1 = Expr.add( Expr.mul(2*real(Cp.*conj(Pc)),XY_n), -conj(Pc).*Pc ); + if (sol.satisfyConstraints) % H_inf objectives % Reminder : rotated cones 2*x1*x2 ≥ ||x3||^2, ||.|| Euclidean norm anyInfObjective = (~isempty(objective.inf.W1) || (~isempty(objective.inf.W4))) || ... (~isempty(objective.inf.W2) || (~isempty(objective.inf.W3))); if anyInfObjective x3 = []; if (~isempty(objective.inf.W1) || (~isempty(objective.inf.W4))) W1 = respOrZero(objective.inf.W1,W); W4 = respOrZero(objective.inf.W4,W,PLANT/autoScaling); infW14 = sqrt(abs(W1).^2+abs(W4).^2); x3_d = infW14.*ZFy; x3 = Expr.hstack( Expr.mul(real(x3_d),Y_n),Expr.mul(imag(x3_d),Y_n)); else infW14 = 0; end if (~isempty(objective.inf.W2) || (~isempty(objective.inf.W3))) W2 = respOrZero(objective.inf.W2,W,PLANT); W3 = respOrZero(objective.inf.W3,W,autoScaling); infW23 = sqrt(abs(W2).^2+abs(W3).^2); x3_d = infW23.*ZFx; if isempty(x3) x3 = Expr.hstack(Expr.mul(real(x3_d),X_n),Expr.mul(imag(x3_d),X_n)); else x3 = Expr.hstack(x3, Expr.mul(real(x3_d),X_n),Expr.mul(imag(x3_d),X_n)); end else infW23 = 0; end if ~isempty(x3) % 2*x1*x2 ≥ ||x3||^2, ||.|| Euclidean norm - scaling2 = 1./vecnorm([infW14.*Yc, infW23.*Xc]./Pc,2,2); + scaling = 1./vecnorm([infW14.*Yc, infW23.*Xc]./Pc,inf,2); sz = x3.getShape(); - x2_scaled = Expr.mulElm(Matrix.dense(scaling2*0.5/autoScalingObj),gamma_Inf.slice([0,mod_-1],[nCon,mod_])); - x3_scaled = Expr.mulElm(Matrix.dense(kron(sqrt(scaling2.*scaling),ones(1,sz(2)))),x3); + x2_scaled = Expr.mulElm(Matrix.dense(scaling*0.5/autoScalingObj),gamma_Inf.slice([0,mod_-1],[nCon,mod_])); + x3_scaled = Expr.mulElm(Matrix.dense(kron(sqrt(scaling),ones(1,sz(2)))),x3); M.constraint(Expr.hstack(x1,x2_scaled,x3_scaled), Domain.inRotatedQCone()); end % END Hinf end % H2 anyTwoObjective = (~isempty(objective.two.W1) || (~isempty(objective.two.W4))) || ... (~isempty(objective.two.W2) || (~isempty(objective.two.W3))); if anyTwoObjective % local 2 norm gamma_2 = gamma_2_mmod.slice([0,mod_-1],[nCon,mod_]); if objective.two.meanNorm meanNorm = Expr.add(meanNorm, Expr.dot(integ/nMod,gamma_2)); else M.constraint(Expr.sub(obj_2,Expr.dot(integ,gamma_2)),Domain.greaterThan(0)); end x3 = []; if (~isempty(objective.two.W1) || (~isempty(objective.two.W4))) W1 = respOrZero(objective.two.W1,W); W4 = respOrZero(objective.two.W4,W,PLANT/autoScaling); twoW14 = sqrt(abs(W1).^2+abs(W4).^2); x3_d = twoW14.*ZFy; x3 = Expr.hstack( Expr.mul(real(x3_d),Y_n),Expr.mul(imag(x3_d),Y_n)); else twoW14 = 0; end if (~isempty(objective.two.W2) || (~isempty(objective.two.W3))) W2 = respOrZero(objective.two.W2,W,PLANT); W3 = respOrZero(objective.two.W3,W,autoScaling); twoW23 = sqrt(abs(W2).^2+abs(W3).^2); x3_d = twoW23.*ZFx; if isempty(x3) x3 = Expr.hstack( Expr.mul(real(x3_d),X_n),Expr.mul(imag(x3_d),X_n)); else x3 = Expr.hstack( x3, Expr.mul(real(x3_d),X_n),Expr.mul(imag(x3_d),X_n)); end else twoW23 = 0; end if ~isempty(x3) - scaling2 = 1./(vecnorm([twoW14.*Yc, twoW23.*Xc]./Pc,inf,2)); + scaling = 1./vecnorm([twoW14.*Yc, twoW23.*Xc]./Pc,inf,2); sz = x3.getShape(); - x2_scaled = Expr.mulElm(Matrix.dense(scaling2*0.5/autoScalingObj),gamma_2); - x3_scaled = Expr.mulElm(Matrix.dense(kron(sqrt(scaling2.*scaling),ones(1,sz(2)))),x3); + x2_scaled = Expr.mulElm(Matrix.dense(scaling*0.5/autoScalingObj),gamma_2); + x3_scaled = Expr.mulElm(Matrix.dense(kron(sqrt(scaling),ones(1,sz(2)))),x3); M.constraint(Expr.hstack(x1,x2_scaled,x3_scaled), Domain.inRotatedQCone()); end % END H2 end end % END OBJ %% Constraint % CONSTAINTS ||W_n S_n||_inf< 1 W1, W2, W3, W4 % ||W1 S||_inf< 1 -> max(|W1|,|system.model*W4|)*||Y/P||_inf < 1 % ||W2 T||_inf< 1 % ||W3 U||_inf< 1 -> max(|system.model*W2|,|W3|)*||X/P||_inf < 1 % ||W3 D||_inf< 1 - + scaling = abs(1./Pc); + x1 = Expr.add( Expr.mul(2*real(Cp.*conj(Pc).*scaling),XY_n), -conj(Pc).*Pc.*scaling ); if (~isempty(constraint.W1) || ~isempty(constraint.W4)) x2 = Expr.add(0.5*ones(nCon,1), Expr.mul(Matrix.dense(0.5*ones(nCon,1)),slack.pick(0))); % batch W1 and W4 -> max(|W1|,|system.model*W4|)*||Y/P||_inf < 1 W1 = respOrZero(constraint.W1,W); W4 = respOrZero(constraint.W4,W,PLANT/autoScaling); cW14 = max(abs(W1),abs(W4)); x3_d = cW14.*ZFy.*sqrt(scaling) ; x3 = Expr.hstack( Expr.mul(real(x3_d),Y_n),Expr.mul(imag(x3_d),Y_n)); M.constraint((Expr.hstack(x1,x2,x3)), Domain.inRotatedQCone()); end if (~isempty(constraint.W2) || ~isempty(constraint.W3)) x2 = Expr.add(0.5*ones(nCon,1), Expr.mul(Matrix.dense(0.5*ones(nCon,1)),slack.pick(1))); % batch W2 and W3 -> max(|system.model*W2|,|W3|)*||X/P||_inf < 1 W2 = respOrZero(constraint.W2,W,PLANT); W3 = respOrZero(constraint.W3,W,autoScaling); cW23 = max(abs(W2),abs(W3)); x3_d = cW23.*ZFx.*sqrt(scaling) ; x3 = Expr.hstack( Expr.mul(real(x3_d),X_n),Expr.mul(imag(x3_d),X_n)); M.constraint((Expr.hstack(x1,x2,x3)), Domain.inRotatedQCone()); end end if objective.two.meanNorm M.constraint(Expr.sub(obj_2,meanNorm),Domain.equalsTo(0)); end if (sol.H2 || sol.Hinf) - % M.constraint(Expr.sub(OBJ, sol.obj*autoScalingObj),Domain.lessThan(0)); + % M.constraint(Expr.sub(sol.obj,obj),Domain.greaterThan(0)); end M.setSolverParam("intpntSolveForm", parameters.solveForm); + +M.setSolverParam("intpntTolPsafe", 1e-4); +M.setSolverParam("intpntTolDsafe", 1e-4); +M.setSolverParam("intpntTolPath", 1e-4); + t1 = tic; M.solve(); diagnostic.solvertime = toc(t1); M.acceptedSolutionStatus(AccSolutionStatus.Anything); sol.H2 = sqrt(obj_2.level()/autoScalingObj); sol.Hinf = sqrt(gamma_inf.level()/autoScalingObj); -sol.obj = max(M.dualObjValue(),0)/(autoScalingObj); +sol.obj = max(obj.level(),0)/(autoScalingObj); diagnostic.primal = char(M.getPrimalSolutionStatus); diagnostic.dual = char(M.getDualSolutionStatus); diagnostic.primalVal = M.primalObjValue(); diagnostic.dualVal = M.dualObjValue(); -if M.dualObjValue() < -1e-8 - M.dualObjValue() - error('Dual negative') -end if ~sol.satisfyConstraints sol.slack = max(abs(slack.level())); end if ~(sol.slack <= 1e-4 && ~sol.satisfyConstraints) controller.num = reshape(X.level(),[ntheta, szx])'*autoScaling; controller.den = reshape(Y.level(),[ntheta, szy])'; end sol.nIter = sol.nIter +1; M.dispose(); end % END DATA_DRIVEN_SOLVE \ No newline at end of file