nx = 6;
ny = 6;
nu = 4;
nlobj = nlmpc(nx, ny, nu);
%nlobj = nlmpc(6,6,'MV',4,'MD',1,'UD',4)
nlobj.Model.StateFcn = "QuadrotorStateFcn";
nlobj.Jacobian.StateFcn = @QuadrotorStateJacobianFcn;
% nlobj.Model.OutputFcn = 'QuadrotorOutputFcn';
% nlobj.Model.OutputFcn = @(x,u,Ts) [x(1); x(3);x(5)];
rng(0)
% u=[1 1 1 1];
% validateFcns(nlobj,rand(nx,1),rand(nu,1));
Ts = 25;
p = 18;
m = 18;
% nlobj.Ts = Ts;
% nlobj.PredictionHorizon = p;
% nlobj.ControlHorizon = m;
% nlobj.MV = struct('Min',{0;0;0;0},'Max',{6;6;6;6});
% nlobj.MV = struct( ...
% Min={0;0;0;0}, ...
% Max={6;6;6;6}, ...
% RateMin={-2;-2;-2;-2}, ...
% RateMax={2;2;2;2} ...
% );
% nlobj.Model.NumberOfParameters = 1;
% nlobj.Weights.OutputVariables = [1 1 1 0 0 0];
% nlobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1];
% nlobj.Weights.ManipulatedVariablesRate = [0.1 0.1 0.1 0.1];
%%%%%%%%%%
nlobj.Ts = Ts;
nlobj.PredictionHorizon = p;
nlobj.ControlHorizon = m;
nlobj.MV = struct('Min', {0; 0; 0; 0}, 'Max', {6; 6; 6; 6});
nlobj.MV = struct( ...
Min = { 0; 0; 0; 0}, ...
Max = { 6; 6; 6; 6}, ...
RateMin = {-2; -2; -2; -2}, ...
RateMax = { 2; 2; 2; 2} ...
);
%nlobj.Model.NumberOfParameters = 1;
nlobj.Weights.OutputVariables = [0.1 0.1 0.1 0 0 0];
nlobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1];
nlobj.Weights.ManipulatedVariablesRate = [0.1 0.1 0.1 0.1];
% Specify the initial conditions
x = [0;0;0;0;0;0];
nlobj.Optimization.CustomCostFcn = @(X, U, e, data) Ts*sum(sum(U(1:p, :)));
nlobj.Optimization.ReplaceStandardCost = true;
% Nominal control target (average to keep quadrotor floating)
nloptions = nlmpcmoveopt;
nloptions.MVTarget = [4.9 4.9 4.9 4.9];
mv = nloptions.MVTarget;
% mv = nlmpcmove(nlobj,x,lastmv,ref);
lastmv = mv;
md = 0;
% ref=[1 0 0; 0 1 0; 0 0 1; 0 0 0; 0 0 0; 0 0 0];
% nlmpcmoveopt.options.Parameters={Ts x};
ref = zeros(1, nlobj.ny);
[mv, ~, Info] = nlmpcmove(nlobj, x, lastmv,ref);
validateFcns(nlobj, rand(nx, 1), rand(nu, 1));
