%% Compute Nonlinear MPC Control Moves Using Code Generation Data Structures
% Create a nonlinear MPC controller with four states, two outputs, and one input.

nlobj = nlmpc(4,2,1);

%
% Specify the sample time and horizons of the controller.

Ts = 0.1;
nlobj.Ts = Ts;
nlobj.PredictionHorizon = 10;
nlobj.ControlHorizon = 5;

%
% Specify the state function for the controller, which is in the file pendulumDT0.m. This discrete-time model integrates the continuous time model defined in pendulumCT0.m using a multistep forward Euler method.

nlobj.Model.StateFcn = "pendulumDT0";
nlobj.Model.IsContinuousTime = false;

%
% The prediction model uses an optional parameter, Ts, to represent the sample time. Specify the number of parameters and create a parameter vector.

nlobj.Model.NumberOfParameters = 1;
params = {Ts};

%
% Specify the output function of the model, passing the sample time parameter as an input argument.

nlobj.Model.OutputFcn = "pendulumOutputFcn";

%
% Define standard constraints for the controller.

nlobj.Weights.OutputVariables = [3 3];
nlobj.Weights.ManipulatedVariablesRate = 0.1;
nlobj.OV(1).Min = -10;
nlobj.OV(1).Max = 10;
nlobj.MV.Min = -100;
nlobj.MV.Max = 100;

%
% Validate the prediction model functions.

x0 = [0.1;0.2;-pi/2;0.3];
u0 = 0.4;
validateFcns(nlobj,x0,u0,[],params);

%
% Only two of the plant states are measurable. Therefore, create an extended Kalman filter for estimating the four plant states. Its state transition function is defined in pendulumStateFcn.m and its measurement function is defined in pendulumMeasurementFcn.m.

EKF = extendedKalmanFilter(@pendulumStateFcn,@pendulumMeasurementFcn);

%
% Define initial conditions for the simulation, initialize the extended Kalman filter state, and specify a zero initial manipulated variable value.

x0 = [0;0;-pi;0];
y0 = [x0(1);x0(3)];
EKF.State = x0;
mv0 = 0;

%
% Create code generation data structures for the controller, specifying the initial conditions and parameters.

[coreData,onlineData] = getCodeGenerationData(nlobj,x0,mv0,params);

%
% Specify the output reference value in the online data structure.

onlineData.ref = [0 0];

%
% To verify the controller operation, run a simulation for 10 seconds. During each control interval:
% Correct the previous prediction using the current measurement.
% Compute optimal control moves using nlmpcmoveCodeGeneration. This function returns the computed optimal sequences in onlineData. Passing the updated data structure to nlmpcmoveCodeGeneration in the next control interval provides initial guesses for the optimal sequences.
% Predict the model states.
% Apply the first computed optimal control move to the plant, updating the plant states.
% Generate sensor data with white noise.
% Save the plant states.

mv = mv0;
y = y0;
x = x0;
Duration = 10;
xHistory = x0;
for ct = 1:(Duration/Ts)
    % Correct previous prediction
    xk = correct(EKF,y);
    % Compute optimal control move
    [mv,onlineData] = nlmpcmoveCodeGeneration(coreData,xk,mv,onlineData);
    % Predict prediction model states for the next iteration
    predict(EKF,[mv; Ts]);
    % Implement first optimal control move
    x = pendulumDT0(x,mv,Ts);
    % Generate sensor data
    y = x([1 3]) + randn(2,1)*0.01;
    % Save plant states
    xHistory = [xHistory x];
end

%
% Generate a MEX function with MATLAB® Coder™, specifying coreData as a constant.

func = 'nlmpcmoveCodeGeneration';
funcOutput = 'nlmpcmoveMEX';
Cfg = coder.config('mex');
Cfg.DynamicMemoryAllocation = 'off';
codegen('-config',Cfg,func,'-o',funcOutput,'-args',...
    {coder.Constant(coreData),xk,mv,onlineData});

% Copyright 2019 The MathWorks, Inc.