Re-write MPC cost function to QP formulation

Hello!

I've been trying to make a simple script to re-write the cost function in the usual MPC formulation to the QP formulation used to solve the optmization problem. My code is as follows: (Note its using symbolics toolbox)

xd = xda+gradxda*(x7-thetaka);
yd = yda+gradyda*(x7-thetaka);
x = [x1 x2 x3 x4 x5 x6 x7].';
u = [u1 u2 u3 u4 u5].';
z = [x;u];

e_c = sinx*(x1-xd)-cosx*(x2-yd);
e_l = -cosx*(x1-xd)-sinx*(x2-yd);

Q = [q_c 0; 0 q_l];

J = [e_c;e_l].'*Q*[e_c;e_l] - q_t*x7;

H = hessian(J,z);

c = gradient(J,z);
c = subs(c, z, [0;0;0;0;0;0;0;0;0;0;0;0]);

f = 0.5*z.'*H*z+c.'*z;

isequal(simplify(J), simplify(f))

Should not J and f be equal? The isequal command will yield a logical 0 for me. However I tested it on an example cost function, found here: https://se.mathworks.com/help/optim/ug/quadprog.html#bssh6y6-8 and using that cost function with my above code will yield a logical 1, as I expect. Have I misunderstood the connection between J and f or have I made a coding mistake?

Best regards MC

Answer 1

[Nicolas Schmit]

J and f are not equal because of the constant term in J. If you take the Taylor expansion of J, you obtain:

J == J(z=0) + gradient(J, z=0)'*z + 1/2* z'*hessian(J, z=0)*z

Thus, f = J - J(z=0), Where J(z=0) = subs(J, z, zeros(size(z)).

The example cost function in the documentation yields a logical 1 because it has no constant term.

You can always remove the constant term from the cost function because it has no impact on the final solution.