Passing in state of 0s into solveMPC()

Hi Alex, just wondering about this line in the fullsize branch, specifically on the (x0 - 1.0*x0) term. Doesn't this just make all the states 0?

MPCC/C++/MPC/mpc.cpp

Line 238 in 302e12a

    
           State x0_normalized = vectorToState(normalization_param_.T_x_inv*(stateToVector(x0)-1.0*stateToVector(x0)));

And why does it work even though every single state that is passed into solveMPC() is 0?

The fullsize branch uses a proper SQP formulation where the optimization variable is the difference to the linearization point. Thus, at the zero time step where the linearization point is the measurement x0, instead of having a constraint that the state should be identical to the measured state, we need a constraint the the first "differential state" optimization variable is zero.
If i find time i will update the readme.
Best,
Alex