Custom objective function on MPC problem

[Alberto Brandl]

I'd like to define a custom objective function to solve an MPC problem. Actually, I'm very new on this stuff so maybe it is not possible. Looking for this problem I found some similar thread but they did not help me.
x is the state with position and speed as [x; dxdt; y; dydt]
I tried to add to the classical

 objective = objective + norm(Q*x{k},1) + norm(R*u{k},1);
 inside the following loop

constraints = [];
objective = 0;
for k = 1:N
 objective = objective + norm(Q*x{k},1) + norm(R*u{k},1);
 constraints = [constraints, x{k+1} == A*x{k} + B*u{k};
 constraints = [constraints, -100 <= u{k}<= 100];
end

a nonlinear term nf function of x{k}. I can evaluate this term before the execution but if I add the term nf(ind_x, ind_y) it does not work (during the definition of the loop value(x{k}) is nan, so i believe that evaluate ind_x based on x{k} is not possible).
If I define nf inside the loop and try to add it to objective I get "Failed exporting the model: Model creation failed (Model not available in constraint #2 at level 1)" calling optimizer.
nf contains only ratio of norm of position vector raised to some constant values.

Is there any way to use such an objective function? Thanks

[Johan Löfberg]

what do you man you can evaluate it before the execution. If it is evaluated already, why even add it to the objective? You will have to show with an example what you are trying to do

[Alberto Brandl]

Thank you for your reply. I'd like to add a penalization depending on the position. So, for instance, i have a function J = norm(x - xc, 2)^2 where x is the position and xc the coordinate of a point in the phase space. The fact is that now I don't actually Know the function, it may contain norms and powers.

[Johan Löfberg]

So you mean you want to setup an optimizer object where you can select the objective once your call the optimizer, i.e. that the choice of objective is a parameter? Or do you simply mean that xc is a parameter which you don't know until you call the optimizer

[Alberto Brandl]

I apologize if i'm not able to better explain myself. In the example above, I know xc. The additive term to the objective function is hence known a priori as a real function defined on 2D space spanned by all possible position.
I want to penalize some trajectories instead of others adding a term to the objective function function of the position. So I'll find the optimal control vector respecting state dynamics and preferring some trajectories instead of others.

[Johan Löfberg]

Why don't you supply an example of what you try to do

[Alberto Brandl]

Let add_term to be a real valued matrix evaluated at each point of the 2D space as

[xx, yy] = ndgrid(1:100, 1:100);
add_term = exp(-((xx-50).^2 + (yy-50).^2))

figure; mesh(xx, yy, add_term); % should resemble a bell function centered in (50, 50)

constraints = [];
objective = 0;
for k = 1:N

 objective = objective + norm(Q*x{k},2) + norm(R*u{k},2) + add_term(x{k}(1), x{k}(3));

 constraints = [constraints, x{k+1} == A*x{k} + B*u{k}];

 constraints = [constraints, -100 <= u{k}<= 100, 0<=x{k+1}<=100];
end

[Johan Löfberg]

Still weird notation. 

 add_term(x{k}(1), x{k}(3), hence you mean add-term is a function which takes two scalars and returns a scalar.

Here though

[xx, yy] = ndgrid(1:100, 1:100);
add_term = exp(-((xx-50).^2 + (yy-50).^2))

you have add_term defined as a big matrix. What is the actual function, and what is fixed and what are arguments? Is "50" fixed, and you simply want exp(-((x(1)-50)^2 + (x(3)-50)^2))?

And do you really want norm(,2)? Most often, people work with quadratic terms, not non-smooth 2-norms in objectives

[Alberto Brandl]

Thanks for your cooperation. The norm fact is another stuff that I cannot understand. Because I want to define the objective function with the term x'Q*x (which is quadratic) so I thought that norm(Q*x, 2) was correct (at least Q is not the same but could be sqrtm(Q)), by the way reading "Design and Implementation of Model Predictive Control using Multi-Parametric Toolbox and YALMIP" I thought that norm(Q*x, 2) was actually corresponding to x'*Q*x. At the same time the l2 norm of a vector consider the square root too.
About the add_term, 50 is known and fixed. add_term is a real-valued function defined on the 2D space. I simply want to add (as you said) exp(-((x(1)-50)^2 + (x(3)-50)^2)) but I'd like to evaluate it during a pre-processing, instead of evaluating a nonlinear function each step. Is that possible?

[Johan Löfberg]

Euclidean norm is defined as ||x|| = sqrt(x'*Q*x), hence, to get x'Q*x you must define it as norm(R*x) where Q=R'*R, i.e R is a Cholesky factor for instance. 

However, why even work with the norm operaotr if you simply want x*Q*x

What do you mean with "evaluate it during preprocessing". If x is a decision variable, it cannot be simplified further or evaluated, and if x is fixed, why even bother with including it in the cost