%% Validate Nonlinear MPC Prediction Model and Custom Functions
% Create nonlinear MPC controller with six states, six outputs, and four inputs.

nx = 6;
ny = 6;
nu = 4;
nlobj = nlmpc(nx,ny,nu);

%
% Specify the controller sample time and horizons.

Ts = 0.4;
p = 30;
c = 4;
nlobj.Ts = Ts;
nlobj.PredictionHorizon = p;
nlobj.ControlHorizon = c;

%
% Specify the prediction model state function and the Jacobian of the state function. For this example, use a model of a flying robot.

nlobj.Model.StateFcn = "FlyingRobotStateFcn";
nlobj.Jacobian.StateFcn = "FlyingRobotStateJacobianFcn";

%
% Specify a custom cost function for the controller that replaces the standard cost function.

nlobj.Optimization.CustomCostFcn = @(X,U,e,data) Ts*sum(sum(U(1:p,:)));
nlobj.Optimization.ReplaceStandardCost = true;

%
% Specify a custom constraint function for the controller.

nlobj.Optimization.CustomEqConFcn = @(X,U,data) X(end,:)';

%
% Validate the prediction model and custom functions at the initial states (x0) and initial inputs (u0) of the robot.

x0 = [-10;-10;pi/2;0;0;0];
u0 = zeros(nu,1); 
validateFcns(nlobj,x0,u0);

%
% Copyright 2018 The MathWorks, Inc.