clean up main code and make it rapid

[Ali_E]

ok I want to clean up my code. it has two loops. first one is for yalmip and its solver and second one is for close loop response of states etc.
my code that is not cleaned yet 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 = 11;
%% 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];
options = sdpsettings('solver','sedumi');
controller = optimize(constraint,objective,options);  
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,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

I tried to take yalmip's loop out of close loop response to make it faster but the result is not okey.
what I configured 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 = 6;
%% 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;
%% Yalmip loop
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];
options = sdpsettings('solver','mosek');
controller = optimize(constraint,objective,options);  
u = value(muu{1});
mux3 = value(mux{1});
F = value(F{1});
X = value(sigmax{1});
sigmax{1} = value(sigmax{2});
x = value(x);
%% Main loop
for t = t0:dt:tf-dt
c = c+1;  
U1(c) = u(1,1);
U2(c) = u(2,1);
U3 = [U1;U2];  
mux1(c) = mux3(1,1);
mux2(c) = mux3(2,1);
f1{c} = F;
X1{c} = X;
K = F*inv(X);
% K=value(K{1})
K1{c} = K;
K2 = value(K1{c});
u1 = u+K2*(x-mux3);
u2{c} = u1;
x = A*x+B*u1+G*w(:,c);
mux{1} = value(mux{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,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

output is doomed!

[Johan Löfberg]

you want to simulate a control loop where the state changes, and you want to compute a new control for every new state. Logically then, your set of constraints should somehow be affected by the state, but in your code they are not as you setup the model outside the loop and then never touches it. Indeed, you only solve 1 optimization problem, so how can you think that is sufficient if you previously solved in every state

Have you really read the MPC example, where this is detailed from naive slow way (where you started) to fastest possible using optimizer

[Ali_E]

so all of them get back to main closed loop, but what about variables??? you once said I can take them out of loop so I did that:

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;

x = randn(2,1);
c = 0;

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));

%% Main closed loop

for t = t0:dt:tf-dt
c = c+1;

 
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];
options = sdpsettings('solver','mosek');
controller = optimize(constraint,objective,options);  
u = value(muu{1});
mux3 = value(mux{1});
F = value(F{1});
X = value(sigmax{1});
sigmax{1} = value(sigmax{2});

U1(c) = u(1,1);

U2(c) = u(2,1);
U3 = [U1;U2];  
mux1(c) = mux3(1,1);
mux2(c) = mux3(2,1);
f1{c} = F;
X1{c} = X;
K = F*inv(X);

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});

 
ud = value(u2{c});

but it says:
Brace indexing is not supported for variables of this type.
Error in Test_Faster (line 55)

[Johan Löfberg]

the error message is telling you very clearly which construction fail, hence you debug that failure like you debug any crashing code

Now you moved most stuff back into the loop? Have you really read, carefully, the MPC link I supplied.

However, why do you have to solve the problem for every new state.Your optimization problem does not depend on the current state. Aren't you missing something like mux{1}==x or something, i.e. mux{1} is the parameter (current state) when you setup an optimizer object. As it is now, it makes no sense. The input u1 is defined from muu, K2, mux3, and all those can be computed without even defining x is the code. Hence, you should be able to solve 1 problem, and then use that solution for any x (unless you've forgot to connect x and mux or something like that)

[Ali_E]

yes it could be forgotten. I tried to simulate this:

[Ali_E]

ah... what's the matter with this? (I want to construct optimizer):

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 = 11;

%% 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];
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;

%x = randn(2,1);
x = randn(2,1);
x0=[1;1];
c = 0;

%% Main closed 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)];

    constraints = [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];
constraints = [constraint,constraint8,constraint9];

options = sdpsettings('solver','mosek');

%controller = optimize([constraint,x0==randn(2,1)],objective,options);  
controller = optimizer(constraints,objective,options,x0,muu{1});  
u = controller{x};

Error using  +  (line 25)
Both arguments must be constraints

Error in optimizer (line 187)
    [aux1,aux2,aux3,model] = export((x ==
    repmat(pi,nIn*mIn,1))+Constraints,Objective,options,[],[],0);

Error in Test_Faster (line 71)
controller = optimizer(constraints,objective,options,x0,muu{1});
 

[Johan Löfberg]

creating an optimizer inside the time loop makes no sense, and trying to create an optimizer with a constant as the parameter object makes even less sense

Your code shoul be a variant of

x0 = sdpvar
other sdpvars
Controller= optimizer(Constraints,Objective,options,x0,stuff);
x0 = numerical value
for t =
 sol = Controller(x0);
 u = some function of sol etc
 x0 = A*x0 + B*u + ...
end

[Ali_E]

I'm doomed :(

x0 = sdpvar(2,1);

controller = optimizer(constraints,objective,options,x0,muu{1});

 
x0 = [1;1];
x = randn(2,1);
c = 0;

%% Main closed loop
for t = t0:dt:tf-dt
c = c+1;

u = controller{x0};

[Ali_E]

excuse me professor you answered me above:
----------------------------

you want to simulate a control loop where the state changes, and you want to compute a new control for every new state. Logically then, your set of constraints should somehow be affected by the state, but in your code they are not as you setup the model outside the loop and then never touches it. Indeed, you only solve 1 optimization problem, so how can you think that is sufficient if you previously solved in every state

----------------------------
how can this be possible? I mean I have to put optimizer out of time structure and my set of constraints should somehow be affected by the state. how can both of these take place in a code???

[Johan Löfberg]

That's the whole idea of the optimizer object. You set up a model symbolically in the initial state, and then you run a loop where you ask the optimizer object to derive and solve the model for a specific value of that state

[Ali_E]

I get this idea, but I'm not able to implement it. would you please construct an optimizer for my code so I can learn how to do? because I'm getting error after error and a bunch of bugs are injected to my code and I'm finished lol

[Johan Löfberg]

I've said it before: Your optimization problem does not depend on the current state (x0 you've called it), hence there is no reason to solve an optimization problem repeatedly. You have either misunderstood the theory you are trying to implement, or you have forgotten to add some constraint. 

[Ali_E]

greetings professor,
I checked my theories and I got that x (t | t) = x (t | t-1) and mux (t | t) = mux (t | t-1).
but sadly I still have problem constructing optimizer.
my code (without defining x and mux because when I define them they inject bug) is as follows:

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 + 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;
        constraint = [constraint,constraint2,constraint3,constraint4,constraint5,constraint6];
    end
    objective = objective + trace(Q*pN);
    constraint7 = (h1x'*mux{N})<=((1-(0.5*epsilon))*g1)-((0.95/(2*epsilon*g1*(1-0.95)))*(h1x'*sigmax{N}*h1x));
    constraint8 = [pN-sigmax{N},mux{N};(mux{N})',1]>=0;
    constraint = [constraint,constraint7,constraint8];

    options = sdpsettings('solver','mosek');

    %controller = optimizer(constraint,objective,options,mux{1},muu{1});
    controller = optimize(constraint,objective,options);
    %mux{1}=[-1;1];
    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,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(2)

[Johan Löfberg]

Well of course this code doesn't run as it uses mu but there is no variable or constant with that name

Undefined variable "mux" or class "mux".

 

[Ali_E]

yeah excuse me it was this:

x{1} = [1;1];
mux{1} = [1;1];
% mux{1} = sdpvar(2,1);

%% 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

        x{k+1} = x{k};

        mux{k+1} = A*mux{k} + B*muu{k};
        objective = objective + 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;
        constraint = [constraint,constraint2,constraint3,constraint4,constraint5,constraint6];
    end
    objective = objective + trace(Q*pN);
    constraint7 = (h1x'*mux{N})<=((1-(0.5*epsilon))*g1)-((0.95/(2*epsilon*g1*(1-0.95)))*(h1x'*sigmax{N}*h1x));
    constraint8 = [pN-sigmax{N},mux{N};(mux{N})',1]>=0;
    constraint = [constraint,constraint7,constraint8];
    options = sdpsettings('solver','mosek');
    %controller = optimizer(constraint,objective,options,mux{1},muu{1});
    controller = optimize(constraint,objective,options);

    u = value(muu{1});
    U1(c) = u(1,1);
    U2(c) = u(2,1);
    U3 = [U1;U2];

    x3 = value(x{1});

    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});

    u1 = u+K2*(x3-mux3);
    u2{c} = u1;
    x3 = A*x3+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) = x3(1,1);
    x2(c) = x3(2,1);

[Johan Löfberg]

There is no question here.

[Ali_E]

yesterday you asked me about current state and I found x (t | t) = x (t | t-1) and mux (t | t) = mux (t | t-1).
and you said u1 can be obtained without x but we got:

u1 = u+K2*(x3-mux3);

and x3 is value(x{1}), but I still don't have any idea about connection between mux and x.
I thought x (t | t) = x (t | t-1) and mux (t | t) = mux (t | t-1) could help to construct optimizer. couldn't?

[Johan Löfberg]

that's something you wlll have to figure out. we cannot answer things about modelling problems not even you understand what they are supposed to be