Constrained MPC initial values problem

I am trying to use a linear constrained model predictive controller to control a nonlinear two imput, one output discrete system. The linear model was acquired via Jacobian linearization about a nominal operating point.

For a initial point (nominal values), the system will correct to zero for some disturbance. If I change the setpoint and initial values to some value not equal to the nominal values, such that the controller should maintain that setpoint unless a disturbance acts upon it, it ALWAYS makes a sharp move towards the nominal on the first mpcmove command, regardless of whether the setpoint is above or below the nominal values.

I have been searching for days and I cannot figure out why or how to correct it. I think I am missing a setting on the MPC setup somewhere because the error occurs due to the first optimal move from the MPC, before it even has a chance to pass through the nonlinear function. Please help!

________

Problem Example and code below

________

For example, with a setpoint of 0.7, initial state of 0.7, and the optimal control inputs found to stay at that position, I get this. In theory the response should be a rather straight line at 0.7, as there is no disturbance, but it makes a sharp move towards the nominal values (at the bottom of the top graph) on the first controller move, then recorrects over a sizable time towards the setpoint. Here, the top plot is the response and the bottom plot are the two inputs.

My program:

Nominal values are in the linear plant model: Linear_sys.

Nonlinear_Fcn is of the form : x(i+1) = f( x(i),u(1),u(2) )

%MPC Arguments
P_Horiz = 8; %Prediction Horizon
C_Horiz = 2; %Control Horizon

Q = [1]; %
Ru = [[0.1],[0.1]]; %
Rdu = [[0.1],[0.1]]; %

%Create Controller Object
mpcobj = mpc(Linear_Sys,Ts_N,P_Horiz,C_Horiz);

%Update MPC Weights
mpcobj.Weights.OutputVariables = Q;
mpcobj.Weights.ManipulatedVariables = Ru;
mpcobj.Weights.ManipulatedVariablesRate = Rdu;

%MPC constraints
mpcobj.Input(1).Min = 0*ones(1,P_Horiz); 
mpcobj.Input(1).Max = (theta_i_max)*ones(1,P_Horiz);
mpcobj.Input(2).Min = P_min*ones(1,P_Horiz);
mpcobj.Input(2).Max = (P_max)*ones(1,P_Horiz); 
mpcobj.Output.Min = (thetadot_m_i_min)*ones(1,P_Horiz);
mpcobj.Output.Max = (thetadot_m_i_max)*ones(1,P_Horiz);

%Update MPC Scale factors 
mpcobj.Input(1).ScaleFactor = theta_i_max - theta_i_min; 
mpcobj.Input(2).ScaleFactor = P_max - P_min; 
mpcobj.Output.ScaleFactor = thetadot_m_i_max - thetadot_m_i_min;

%Prepare to store the closed-loop MPC responses
YY = C_lin*0.7; %C matrix is [1], so Y = [1]*X
UU = [0.3441;0.2796]; %U such that X(i+1) = 0.7 = Nonlinear_function( X(i)=0.7 , U ) 
TT = 0; %Initial Time

%Use MPCSTATE object to specify the initial states for controller
xmpc = mpcstate(mpcobj,YY,[],[],UU);

%Closed loop response
Time_Length = 50; %Setting
Time = 0; %Time counter
Data = [0,YY,Y_t,UU'];
m = 1;

while Time <= Time_Length
    %Loop while current time is less than Time length.
    %Increment by Ts for current loop each pass until Time_length is
    %reached.

    %Controller, outputs U vector based on, "0" is nominal op pt
    [u,info] = mpcmove(mpcobj,xmpc,YY(end),SetPt);
    
    %Append inputs for next pass
    UU = [UU,u]; 
    
    %Calculate Time step, base on current theta_dot, current theta
    Ts_1st = Time_Step_Fcn(YY(end),UU(1,end));
    
    %Simulate response against nonlinear system
    X = Nonlinear_Fcn(Time,YY(end),UU(:,end));
    
    %Reference variable for plotting
    X_t = thetadot_m_i_N;
    
    %Append Output at Time+Ts to output matrix
    YY = [YY,C_lin*X];
    Y_t = [Y_t,C_lin*X_t];
    
    %Calculate step i time step
    Ts_2nd = Time_Step_Fcn(YY(end),UU(1,end-1));
    Ts = Ts_1st+Ts_2nd;
    TT = [TT, TT(end)+Ts];
    
    %Diagnostic matricies
    Data = [Data;Ts,YY(end-1),Y_t(end-1),UU(:,end)'];        
    
    %Update Time counter
    Time = Time + Ts;      
    
    %Progress display
    disp(Time);
    
    %Update incrementer
    m = m+1;
    
end  

Answer 1

[Hossein Rezazadeh]

Did you check the dx/dt values at initial condition ? Are you sure the initial condition is steady state ?

Answer 2

[Patricio]

I had a similar problem and found this question. Not sure if it was ever resolved by the original poster, but it appears that Matlab's mpc API does weird things to/with non-zero initial conditions. Looking at many of the linear MPC examples, they all begin at the origin. I had a trajectory tracking problem and the mpc code would work perfectly when the start of the trajectory was the origin. Shfiting the initial condition and the trajectory away from the origin led to the behavior described in the original post. For both the ``mpcmove`` and for ``sim`` function call. Oddly, the problem got worse the further from the origin, which seems to indicate that internally Matlab is doing something to the initial condition on purpose. Try doing the same and see if the odd behavior also scales. If so, then that means it is Matlab's implementation that is giving the odd behavior.

For my solution to work, code was added to shift every trajectory's initial condition to the origin, compute the solution, then shift back to the actual start point. The update/dynamics will also need to be adjusted to work in the modified coordinates. It's frustrating that Matlab hides so much of the implementation in the API. I'd love to turn off all noise/uncertainty and set the system to being purely deterministic, but that seems to be impossible and the documentation is sorely lacking. In truth, it would be awesome to have MPC run as a discrete time optimal control scheme since that is what lies at the heart of the code. Doing so is not possible, thus any custom MPC-like solutions are impossible. MPC solutions must be run the way Matlab envisions they should, not how users might conceivably choose to.

Also, in my case, I tried trivial discrete dynamics (A = eye) so all initial conditions in the plane are steady state based on the first answer. The problem still occurs.