tuning convex linear MPC

[Ali_E]

Hello professor. I had a non-convex and non-linear problem which I've solved recently by yalmip. I attached my solved problem which is a convex linear problem.
I'm looking for an idea to tune this problem's Q and R weighting matrices. I've looked for an idea and I've read a lot of papers, but I couldn't find any idea for tuning this. I wanted to ask you if there is any idea or strategy or any algorithm for tuning weighting matrices???
I've also read two papers I attach here. can these papers help to tune my problem???
any help would be appreciated.

[Johan Löfberg]

Pretty sure I've already told you that you will have to resort to some black-box solver such as fminsearch etc, and then simply write a wrapper around your goal

function solvethproblem

% Find LQ tunng to achive desired response

n = 3;

m = 2;

Q0 = eye(n); q0 = Q0(find(triu(ones(n))));

R0 = eye(m); r0 = R0(find(triu(ones(m))));

x0 = [q0;r0];

x = fminunc(@theproblem,x0,optimset('Display','iter-detailed'))

function f = theproblem(x)

n = 3;

m = 2;

q = x(1:n*(n+1)/2);

r = x(n*(n+1)/2+1:end);

pattern = find(triu(ones(n)));

Q(pattern) = q;

Q = reshape(Q,n,n);

Q = Q+Q'-diag(diag(Q));

pattern = find(triu(ones(m)));

R(pattern) = r;

R = reshape(R,m,m);

R = R+R'-diag(diag(R));

% parameterizing roots of weight to ensure the objects always are psd

% to avoid problems in LQR, hence Q is really a root factor of the weight

f = computegoodness(Q'*Q,R'*R);

% Does not work well as solver makes too long steps into negative definite

%f = computegoodness(Q,R);

function f = computegoodness(Q,R)

% Create LQ controller with this Q and R

A = [1 2 3;4 5 6;7 8 9];

B = [1 0;0 1;0 0];

C = [1 0 0;0 1 0];

L = lqr(A,B,Q,R);

L0 = pinv(-C*inv(A-B*L)*B);

Gc = ss(A-B*L,B*L0,C,0);

y = step(Gc,0:0.01:10);

clf

subplot(2,2,1);plot(y(:,1,1));hold on

subplot(2,2,2);plot(y(:,2,1));hold on;

subplot(2,2,3);plot(y(:,1,2));hold on;

subplot(2,2,4);plot(y(:,2,2));hold on;

% Dream step response

Gdream = ss(-.5*eye(2),.5*eye(2),eye(2),zeros(2));

ydream =  step(Gdream,0:0.01:10);

subplot(2,2,1);plot(ydream(:,1,1),'--');hold on

subplot(2,2,2);plot(ydream(:,2,1),'--');hold on;

subplot(2,2,3);plot(ydream(:,1,2),'--');hold on;

subplot(2,2,4);plot(ydream(:,2,2),'--');hold on;

drawnow

f = norm((y(:)-ydream(:)))^2;

[Ali_E]

yeah I recall we talked about this, but how can I connect my code (which solves my LQ convex problem) to this functions you've given me???
my code is:

clc;
clear;
close all;
%% Time structure
t0 = 0;        
tf = 10;        
dt = 0.1;      
time = t0:dt:tf-dt;
%% System structure  ===> x(k+1) = A*x(k) + B*u(k) + G*w(k)
A = [1.02 -0.1;0.1 0.98];
B = [0.5 0;0.05 0.5];
G = [0.3 0;0 0.3];
[nx,nu] = size(B);
%% MPC parameters
Q = eye(nx);
R = eye(nu)*50;
Qf = eye(nu)*50;
N = 26;
%% Problem parameters
M = blkdiag(Q,R);
w1 = randn(1,100);
w1 = w1-mean(w1);
muw1 = mean(w1);
sigmaw1 = var(w1);
w2 = randn(1,100);
w2 = w2-mean(w2);
muw2 = mean(w2);
sigmaw2 = var(w2);
w = [w1;w2];
sigmaw = [sigmaw1 0 ;0 sigmaw2];
x = randn(2,1);
mux{1} = [-2;2];
sigmax{1} = [1,0;0,1];
h1x = [-1/sqrt(5);-2/sqrt(5)];
h2u = [-0.4/sqrt(1.16);1/sqrt(1.16)];
epsilon = 0.5;
g1 = 3;
g2 = 0.2;
c = 0;
%% Main loop
for t = t0:dt:tf-dt
c = c+1;  
sigmax = sdpvar(repmat(nx,1,N+1),repmat(nu,1,N+1));
p = sdpvar(repmat(4,1,N-1),repmat(4,1,N-1));
pN = sdpvar(2,2);
F = sdpvar(repmat(2,1,N+1),repmat(2,1,N+1));
K = sdpvar(repmat(2,1,N+1),repmat(2,1,N+1));
muu = sdpvar(repmat(nu,1,N),repmat(1,1,N));
sigmau = sdpvar(repmat(2,1,N),repmat(2,1,N));
constraint = [];
objective = 0;
for k = 1:N-1  
    mux{k+1} = A*mux{k} + B*muu{k};        
    objective = objective + 1/2*(trace(M*p{k}));        
    constraint2 = [sigmax{k+1} (A*sigmax{k}+B*F{k}) G*sigmaw;(A*sigmax{k}+B*F{k})' sigmax{k} zeros(2);(G*sigmaw)' zeros(2) sigmaw]>=0;
    constraint3 = [sigmau{k} (F{k});(F{k})' sigmax{k}]>=0;
    constraint4 = h1x'*mux{k}<=(1-(0.5*epsilon))*g1-((0.95/2*epsilon*g1*(1-0.95))*h1x'*sigmax{k}*h1x);
    constraint5 = h2u'*muu{k}<=(1-(0.5*epsilon))*g2-((0.95/2*epsilon*g2*(1-0.95))*h2u'*sigmau{k}*h2u);
    constraint6 = [p{k} [sigmax{k};F{k}] [mux{k};muu{k}];[sigmax{k};F{k}]' sigmax{k} zeros(2,1);[mux{k};muu{k}]' zeros(1,2) ones(1)]>=0;
    constraint7 = [F{k} K{k};sigmax{k} eye(2)];
    constraint = [constraint,constraint2,constraint3,constraint4,constraint5,constraint6,constraint7];          
end        
objective = objective + (1/2*trace((trace(Q*pN))));
constraint8 = h1x'*mux{N}<=(1-(0.5*epsilon))*g1-((0.95/2*epsilon*g1*(1-0.95))*h1x'*sigmax{N}*h1x);
constraint9 = [pN-sigmax{N} mux{N};mux{N}' 1];
constraint = [constraint,constraint8,constraint9];
option = sdpsettings('solver','sedumi');
a = optimize (constraint,objective,option);  
u = value(muu{1});
U1(c) = u(1,1);
U2(c) = u(2,1);
U3 = [U1;U2];  
mux3 = value(mux{1});
mux1(c) = mux3(1,1);
mux2(c) = mux3(2,1);
F = value(F{1});
X = value(sigmax{1});
f1{c} = F;
X1{c} = X;
K = F*inv(X);
% K=value(K{1})
K1{c} = K;
K2 = value(K1{c});
x = value(x);
u1 = u+K2*(x-mux3);
u2{c} = u1;
x = A*x+B*u1+G*w(:,c);
mux{1} = value(mux{2});
sigmax{1} = value(sigmax{2});    
ud = value(u2{c});
ud1(c) = ud(1,1);
ud2(c) = ud(2,1);
x1(c) = x(1,1);
x2(c) = x(2,1);      
end
%% Plot results
figure(1)
subplot(4,1,1)
plot(time,ud1,'LineWidth',2)
xlabel('time')
ylabel('ud1')
title('Input signal')
grid on
subplot(4,1,2)
plot(time,ud2,'LineWidth',2)
xlabel('time')
ylabel('ud2')
grid on
subplot(4,1,3)
plot(time,x1,'LineWidth',2)
xlabel('time')
ylabel('x1')
title('State 1')
grid on
subplot(4,1,4)
plot(time,x2,'LineWidth',2)
xlabel('time')
ylabel('x2')
title('State 2')
grid on
figure(2)
subplot(4,1,1)
plot(time,x1,'b',time,mux1,'r','LineWidth',2)
xlabel('time')
legend({'x1','Mean of x1'},'Location','northeast')
grid on
subplot(4,1,2)
plot(time,x2,'b',time,mux2,'r','LineWidth',2)
xlabel('time')
legend({'x2','Mean of x2'},'Location','northeast')
grid on
subplot(4,1,3)
plot(mux1,mux2,'LineWidth',2)
title('State space phase plane')
xlabel('mux1')
ylabel('mux2')
grid on
subplot(4,1,4)
plot(U1,U2,'LineWidth',2)
title('Control space phase plane')
xlabel('U1')
ylabel('U2')
grid on
figure(3)
subplot(4,1,1)
plot(time,mux1,'b','LineWidth',1)
title('Mean of first state')
xlabel('Time')
ylabel('mux1')
grid on
subplot(4,1,2)
plot(time,mux2,'b','LineWidth',1)
title('Mean of second state')
xlabel('Time')
ylabel('mux2')
grid on
subplot(4,1,3)
plot(time,U1,'r','LineWidth',1)
title('First input')
xlabel('Time')
ylabel('U1')
grid on
subplot(4,1,4)
plot(time,U2,'r','LineWidth',1)
title('Second input')
xlabel('Time')
ylabel('U2')
grid on

[Johan Löfberg]

f is defined

[Ali_E]

yeah excuse me professor you are right.
another subject is that above I've provided code that solves my convex linear problem and it is state feedback not output feedback. I want to tune those 2 states. how can I connect those black-box functions to my state feedback code? and get tuned states?

[Johan Löfberg]

I've given you a complete framework for "Given initial guess matrices Q0 and R0, find matrices Q and R (or Q'*Q and R'R if required psd) such that some performance measure is optimized using the solvers fminunc or fminsearch". Your task is to change the way the measure f is defined in the function computegoodness. Only you can do that.

[Ali_E]

oh, ok!
so I should change "computegoodness" in a way that it can define my state feedback problem then I'll get tuned states.
am I right???

[Johan Löfberg]

computegoodness is a file which returns your performance measure f, saying how good Q and R are, the black-box. The smaller f is, the better you think Q and R are. In the example, the deviation of between a a desired step response and the designed step response (in squared L2-norm)

[Ali_E]

thank you very much professor. it was a great help. I'll try to deal with it. thanks again!

[Ali_E]

pardon me professor, how can I make my state feedback code faster???
is there any way?
because it takes to much time for it to run.
I don't know how to make it rapid.

[Johan Löfberg]

optimizer objects would be a start

[Ali_E]

yeah my optimizer could be "sedumi", "mosek" and "sdpt3" all of them work.
but you are talking about "objective". what should I do about them???

[Johan Löfberg]

https://yalmip.github.io/command/optimizer/

examplified in https://yalmip.github.io/example/standardmpc/

[Ali_E]

ok I put my code inside those functions, but I'm not sure if it's working fine or not?
my dream response is zeros(1,100) and I got two states that I want them to get near to zero as soon as possible.
my code (functions and other files) is:

clc;
clear;
close all;
%% Find LQ tunng to achive desired response
n = 2;

m = 2;
Q0 = eye(n);
q0 = Q0(find(triu(ones(n))));
R0 = eye(m);
r0 = R0(find(triu(ones(m))));
x0 = [q0;r0];

options = optimset('Display','iter-detailed','MaxIter',2);
x = fminsearch(@theproblem,x0,options);

function f = theproblem(x)
n = 2;

m = 2;
q = x(1:n*(n+1)/2);
r = x(n*(n+1)/2+1:end);
pattern = find(triu(ones(n)));
Q(pattern) = q;
Q = reshape(Q,n,n);
Q = Q+Q'-diag(diag(Q));
pattern = find(triu(ones(m)));
R(pattern) = r;
R = reshape(R,m,m);
R = R+R'-diag(diag(R));
% parameterizing roots of weight to ensure the objects always are psd
% to avoid problems in LQR, hence Q is really a root factor of the weight

%f = computegoodness(Q,R);
%f = computegoodness_test_1(Q'*Q,R'*R);
f = computegoodness_test_1(Q,R);

% Does not work well as solver makes too long steps into negative definite
%f = computegoodness(Q,R);

end

function f = computegoodness_test_1(Q,R)

% Create LQ controller with this Q and R

%% Time structure
t0 = 0;        
tf = 10;        
dt = 0.1;      
time = t0:dt:tf-dt;
%% System structure  ===> x(k+1) = A*x(k) + B*u(k) + G*w(k)
A = [1.02 -0.1;0.1 0.98];
B = [0.5 0;0.05 0.5];
G = [0.3 0;0 0.3];
[nx,nu] = size(B);
%% MPC parameters

%Q = eye(nx);
%R = eye(nu)*50;
Qf = eye(nu)*50;
N = 6;

options = sdpsettings('solver','sedumi');
controller = optimize(constraint,objective,options);  
u = value(muu{1});

mux=[mux1;mux2];
%clf
subplot(2,1,1);plot(mux(1,:),'linewidth',2);hold on
subplot(2,1,2);plot(mux(2,:),'linewidth',2);hold on;
% Dream step response
ydream = zeros(2,100);
subplot(2,1,1);plot(ydream(1,:),'--','linewidth',2);hold on
subplot(2,1,2);plot(ydream(2,:),'--','linewidth',2);hold on;
drawnow
f = norm((mux(:)-ydream(:)))^2;
end

problem is that I've defined two iterations, but it works for ever and doesn't stop!!!
I don't know why.

[Johan Löfberg]

You don't seem to have read the links I supplied. You code it as slow as it possibly can be coded

First, you do

for loop

 define variables

 define constraints

 solve problem

end

a trivial improvement is

define variables

for loop

 define constraints

 solve problem

 x = A*x+B*u...

end

but what you have to do is to come up with an optimizer setup

define variables

create optimizer object P parameterized e.g. in x or what ever you have

for loop

 u = P(x)

 x = A*x + B*u

end

However, it looks like your simulation, which defines the objective, depends on random data. That means the objective is a stochastic object, and most solver will have severe issues with that, as two consecutive calls with exactly the same iterate will lead to different objectives, hence it is impossible to estimate what effect a change in the decision variable has, and the solver will just stall or fail n some way

runs though, but it simply not suitable for fminsearch I guess as it terminates rather soon

 

 Iteration   Func-count     min f(x)         Procedure

     0            1          13.2336         

     1            7          13.0873         initial simplex

     2            9           12.947         expand

 

Exiting: Maximum number of iterations has been exceeded

         - increase MaxIter option.

         Current function value: 12.946998 

[Ali_E]

the main problem is stochastic, but I convert it to a deterministic linear convex problem.
you've changed those random variables like x=randn(2,1) and then it worked?
which solver is suitable for this kind of problem??? 

[Johan Löfberg]

I just ran your code, no changes (except turning off display of course)

there is no good solver for these problems. you simply test and see

[Ali_E]

so is there any other tuning strategy (that tune by optimization) for this kind of problem?
I've read a lot of tuning related papers, but nothing found for this kind of unique problem...!

[Johan Löfberg]

there is really nothing special here. you have an optimization problem with expensive black-box function evaluation, and people try to write solvers for that, but it is a hard problem

[Ali_E]

I ran my code.the exact code I give you, but I can't get this:
----------------------------------

Iteration   Func-count     min f(x)         Procedure

     0            1          13.2336         

     1            7          13.0873         initial simplex

     2            9           12.947         expand

 

Exiting: Maximum number of iterations has been exceeded

         - increase MaxIter option.

         Current function value: 12.946998 

------------------------------------

it just do it again and again for ever and doesn't stop because of stochastic parts in my problem etc

or maybe that's because I havn't turn off plot???

[Ali_E]

so here is end of searching for a tuning strategy (sth like controller matching) and what I need is a solver and that solver doesn't exist and end of story

[Johan Löfberg]

have you turned off ll the display during simulation. it will of course destroy the outer solver display if you print stuff in the function evaluation

[Johan Löfberg]

google expensive black-box function evaluation and you will find more proposed solver algorithms than you ever could ask for

...or start by cleaning up the code so t doesn't run that slowly, and it is probably not particularly expensive any longer

[Ali_E]

no no pardon me professor you mean if I clean up this code it can give result and be tuned???
because you said I got stochastic problem, so "fminsearch" or any other solver can't solve that and runs it for infinity
I meant if there aren't any solver so end of story...

[Johan Löfberg]

Your use of random data causes objective noise on the fifth decimal. That will most likely not cause problems for fminsearch and similiar solvers, while gradient based solvers such as fminunc and fmincon etc might have problems. You never know until you try

>> f = computegoodness_test_1(Q,R)

f =

     1.323363788222039e+01

>> f = computegoodness_test_1(Q,R)

f =

     1.323357186150603e+01

>> f = computegoodness_test_1(Q,R)

f =

     1.323358605788902e+01

If the noise actually causes problems just add the word noisy to your google search, and you will find that optimization with noise expensive function evaluations is yet another well established field (if you actually keep the random stuff, as it is now your with your setup trying to judge the suitability of Q and R and then using different initial conditions and noise on every call makes no sense. Was the deterioration in objective caused by an unfortunate initial state, or an effect of the change in Q and R?...) 

Cleaning up the data is not related to the likelihood of things working, it's just to speed things up to avoid having to have to sit and stare at the screen for 1 minute per function evaluation

[Ali_E]

ok so it's not end of story etc
I'll clean my code up for sure, but talking about judgment... I want better tracking of "dream response" so judgment takes place there, but I don't know about those initial conditions Q0 and R0. they are two assumed parameters given by problem maker and I want to tune them for a better tracking, less rise time, less overshoot for output etc.
and if those stochastic variables won't cause problem for "fminsearch" so no problem there anymore but if they do cause problem there I should google "noisy function evaluation".
I hope I understand your idea...!

[Johan Löfberg]

Think of it terms of a car analogy

You want to design a car (control system). The two choices are tire width and length of spoiler (Q and R). The goal is to minimize fuel consumption (your objective).under varying stochastic (weather, road surface, terrain) scenarios 

What you do now is that you start with a reasonable initial guess on tire width and spoiler length (Q0 and R0). You perform an experiment by driving car from city B to city A (your simulation) and record fuel consumption, say 56.6578 liters. You now make minor adjustments (that's what the solver does) to the decision variables. Would it make sense to mount 2 mm wider tires and perform an experiment where you now go from city C to city A, this time in rain, to say anything about the effect of tire width? Of course not. If the car now consumes 56.66 liters, you don't know if that was due to the tires, or that the weather was different, or it there are more hills from C to A compared to B to A. Even though your objective is to perform well under varying conditions, you cannot judge the performance based on single different realization (what you could do is to have 1000 different routes under different conditions and weather, and then repeat those and take the average, to reduce influence of stochastic part)

[Ali_E]

yeah exactly. thank you very much professor.

[Ali_E]

greeting professor,
I wanted to ask you is there any method for tuning a MPC problem in a way that it runs in each sample of time. I mean a method that runs the code with Q0 and R0 and find an input (first time sample) then another Q and R in next sample and new input till the end of iteration and best Q and R matrices?

[Johan Löfberg]

Why do you need to automatically tune the controller. What is wrong with the response, and why can't you fix that with standard manual tuning (asking since you appear to spend a lot of frustration on trying to solve problems which turn extremely hard for you)

[Ali_E]

there is nothing wrong with the response. I just wanted to know is there any online tuning method for these LMI based problems? because there are a bunch of methods for simple MPC online tuning not LMI based problems and convex ones.
I wanted to know that is there any way to implement that black-box method in main MPC program and run main code once based on time samples? I mean in a way that our black-box "fminsearch" solver works with time samples.

[Johan Löfberg]

I don't see how you could collect any information relevant to a black-box solver, using 1 sample only

[Ali_E]

so we need it to run the code and get "f" then after running a bunch of times it finds the best one (optimize).
so we can't put that "fminsearch" inside main code and run it once?
the main problem with the result is that it is time consuming and the reason I want to put my BB in main code is that it might take less time to find best Q and R.

[Johan Löfberg]

I dunno, just saying I don't see how one control computation could extract sufficient information for solver to take a good step.

Once again car analogy. You plan to go to city A. Your plan says it takes 57 liters of gas. you start driving and 1 minute later you plan again, this time starting slightly closer to target, and you've changed to 2mm wider tires in your plan and now your computations say the optimal drive will take 56.98 liters with those tires. was that due to the change in tire choice (i.e can you use (56.98-57)/2 as an approximation of derivative), or simply a fact from being closer to target this time

[Ali_E]

yeah you are right that is of no use. Thanks a lot.

[A_E]

hello professor, I was thinking about implementing black-box solver to main code (MPC or any other control system sumulation) and make it work with time samples and have just one main code instead of functions and iterations. I mean sth like adaptive control. in each sample time our code runs and gives "f" to fminsearch then next sample time again main code gives another "f" to fminsearch and fminsearch tune the response and give better tracking (for example).
is this feasible on your simple example:

function solvethproblem
% Find LQ tunng to achive desired response
n = 3;

m = 2;
Q0 = eye(n); q0 = Q0(find(triu(ones(n))));
R0 = eye(m); r0 = R0(find(triu(ones(m))));
x0 = [q0;r0];

x = fminunc(@theproblem,x0,optimset('Display','iter-detailed'))


function f = theproblem(x)
n = 3;

m = 2;
q = x(1:n*(n+1)/2);
r = x(n*(n+1)/2+1:end);
pattern = find(triu(ones(n)));
Q(pattern) = q;
Q = reshape(Q,n,n);
Q = Q+Q'-diag(diag(Q));
pattern = find(triu(ones(m)));
R(pattern) = r;
R = reshape(R,m,m);
R = R+R'-diag(diag(R));


% parameterizing roots of weight to ensure the objects always are psd
% to avoid problems in LQR, hence Q is

really a root factor of the weight
f = computegoodness(Q'*Q,R'*R);

% Does not work well as solver makes too long steps into negative definite
%f = computegoodness(Q,R);

function f = computegoodness(Q,R)

% Create LQ controller with this Q and

R
A = [1 2 3;4 5 6;7 8 9];
B = [1 0;0 1;0 0];
C = [1 0 0;0 1 0];
L = lqr(A,B,Q,R);
L0 = pinv(-C*inv(A-B*L)*B);
Gc = ss(A-B*L,B*L0,C,0);
y = step(Gc,0:0.01:10);
clf
subplot(2,2,1);plot(y(:,1,1));hold on
subplot(2,2,2);plot(y(:,2,1));hold on;
subplot(2,2,3);plot(y(:,1,2));hold on;
subplot(2,2,4);plot(y(:,2,2));hold on;


% Dream step response
Gdream = ss(-.5*eye(2),.5*eye(2),eye(2),zeros(2));
ydream =  step(Gdream,0:0.01:10);
subplot(2,2,1);plot(ydream(:,1,1),'--');hold on
subplot(2,2,2);plot(ydream(:,2,1),'--');hold on;
subplot(2,2,3);plot(ydream(:,1,2),'--');hold on;
subplot(2,2,4);plot(ydream(:,2,2),'--');hold on;
drawnow
f = norm((y(:)-ydream(:)))^2;

[Johan Löfberg]

You already asked that, and already accepted my answer 

[Johan Löfberg]

The code you show has nothing to do with your question. I've shown you generic (naive brute-force) code to tune an LQ controller via black-box optimization