Too Many Input Arguments

I implement vehicle platooning to train model

This is my model stored in function

function [PositionN, VelocityN, TorqueN] = traindiscretemodel(u,Tim_step,Position,Velocity,Torque,Mass,Ca_0,Ca_1,Ca_2,Tao)
% Train Model

   PositionN = Position + Velocity*Tim_step;
   VelocityN = (u - 1/Mass*(Ca_0 + Ca_1*Velocity + Ca_2*Velocity^2));
   TorqueN = Torque - 1/Tao*Torque*Tim_step + 1/Tao*u*Tim_step;
    
end

and this is parameter value i use saved in .MAT

% Parameter Massa
Mass = [8095;8500;8457;8650;8700;8500;8300;];

% Parameter Gaya
u = [205*10^3;302*10^3;205*10^3;302*10^3;205*10^3;302*10^3;302*10^3;];

% Parameter Resistansi
Ca_0 = [0.01176;0.01176;0.01176;0.01176;0.01176;0.01176;0.01176;];
Ca_1 = [0.00077616;0.00077616;0.00077616;0.00077616;0.00077616;0.00077616;0.00077616;];
Ca_2 = [4.48;4.48;4.48;4.48;4.48;4.48;4.48;];

Ca = [0.987142335714838;1.14982586117376;1.16679864955151;1.11574703097155;1.13154802611567;1.12862649362498;1.05844540390683;];
    

% Desired Speed
v_0 = 300;

% Desired Train Following Headway Time
hstar = 120;

Num_step = 100;

Num_veh = 7;

AccMax = 6;
AccMin = -6;

Eta = 0.9600;
f = 0.0100;
g = 9.800;
i = 7;
Tim_step = 0.100;
Time_sim = 10;

R = [0.303571167857419;0.384912930586878;0.393399324775755;0.367873515485777;0.375774013057833;0.374313246812492;0.339222701953417;];
Tao = [0.510713503572257;0.754738791760633;0.780197974327265;0.703620546457332;0.727322039173500;0.722939740437475;0.617668105860250;];

Torquebound = [-1965.07627392709,1965.07627392709;-4448.46112597519,4448.46112597519;-4755.19773617931,4755.19773617931;-3859.76376866931,3859.76376866931;-4128.20664237638,4128.20664237638;-4077.98483605068,4077.98483605068;-2951.71882061833,2951.71882061833;];

cd('G:\Ivan\Semester 10\Tugas Akhir II\matlab\DMPC Train Model');
save valueparameters.mat

My Parameter_Initial

clc;clear;close all;

%% Parameter Initialation

Num_veh  = 7;                            % The number of vehicles in a platoon; 
Tim_step = 0.1;                          % Time step;
Time_sim = 10;                           % Time length for simulation
Num_step = Time_sim/Tim_step;            % Simulation setps

Mass =  zeros(Num_veh,1) + 1000*rand(Num_veh,1);  % Vehilce mass
Tao  = 0.5 +(Mass - 1000)/1000 * 0.3;   % Time lag
f    = 0.01;                            % rolling friction
Eta  = 0.96;                             % Efficency
g    = 9.8;                             % gravitity 
Ca   = 0.98 + (Mass - 1000)/1000 * 0.2;                  % ·ç×è 
Ca_0 = 0.98 + (Mass - 1000)/1000 * 0.2 /83.5 ;
Ca_1 = 0.98 + (Mass - 1000)/1000 / 1271856556;
Ca_2 = 0.98 + (Mass - 1000)/1000 * 0.2 * 4;
% Ca   = 1/2*0.4*2*1.23;                  % ·ç×è 
R    = 0.3 +(Mass - 2000)/2000 * 0.1 ;   % °ë¾¶

%% Acceleration bounds -6,6
AccMax = 6; AccMin = -6;
Torquebound = zeros(6,2);               %   [low up]
for i = 1:Num_veh
    Torquebound(i,1) = Mass(i)*AccMin*R(i)/Eta;
    Torquebound(i,2) = Mass(i)*AccMax*R(i)/Eta;
end

My Cost Function

function Cost = Fungsicos(Np, Tim_step, X0, u, Vehicle_Type, Q, Xdes, R, F, Xa, G, Xnba)
% Cost function

    Pp = zeros(Np,1);     % Predictive Position
    Vp = zeros(Np,1);     % Predictive Velocity
    Tp = zeros(Np,1);     % Predictive Torque
    
    Mass = Vehicle_Type(1);Radius = Vehicle_Type(2); g = Vehicle_Type(3);f = Vehicle_Type(4);
    Eta = Vehicle_Type(5);Ca = Vehicle_Type(6);Tao = Vehicle_Type(7);Ca_0 = Vehicle_Type(8);
    Ca_1 = Vehicle_Type(9);Ca_2 = Vehicle_Type(10);
    
    [Pp(1),Vp(1),Tp(1)] = traindiscretemodel(u(1),Tim_step,X0(1),X0(2),X0(3),Mass,Ca_0,Ca_1,Ca_2,Tao,Radius,g,f,Eta,Ca);
    for i = 1:Np-1
        [Pp(i+1),Vp(i+1),Tp(i+1)] = traindiscretemodel(u(i+1),Tim_step,Pp(i),Vp(i),Tp(i),Mass,Ca_0,Ca_1,Ca_2,Tao,Radius,g,f,Eta,Ca);
    end
    
    Xp = [Pp,Vp];      % Predictive State
    
    Udes = Radius/Eta*(Ca*Vp.^2 + Mass*g*f);
    U0 = Radius/Eta*(Ca*X0(2).^2 + Mass*g*f);
    
    Cost = (X0(1:2)-Xdes(1,:))*Q*(X0(1:2)-Xdes(1,:))' + ...
                (u(1)-U0)*R*(u(1)-U0) + (X0(1:2)-Xa(1,:))*F*(X0(1:2)-Xa(1,:))'+ ...
                (X0(1:2)-Xnba(1,:))*G*(X0(1:2)-Xnba(1,:))';                               % 第一步的优化值
    for i = 1:Np-1        %% 注意范数的定义问题， X'Q'QX
        Cost = Cost + (Xp(i,:)-Xdes(i+1,:))*Q*(Xp(i,:)-Xdes(i+1,:))' + ...
                (u(i+1)-Udes(i))*R*(u(i+1)-Udes(i)) + (Xp(i,:)-Xa(i+1,:))*F*(Xp(i,:)-Xa(i+1,:))'+ ...
                (Xp(i,:)-Xnba(i+1,:))*G*(Xp(i,:)-Xnba(i+1,:))';               
    end
   
end

My non linear constrain

function [C, Ceq] = kendalanonlinear(Np, Tim_step, X0, u, Vehicle_Type,x0,v0,T0)
%UNTITLED5 Summary of this function goes here
%   Detailed explanation goes here

    Pp = zeros(Np,1);     % Predictive Position
    Vp = zeros(Np,1);     % Predictive Velocity
    Tp = zeros(Np,1);     % Predictive Torque
    
    Mass = Vehicle_Type(1);Radius = Vehicle_Type(2); g = Vehicle_Type(3);f = Vehicle_Type(4);
    Eta = Vehicle_Type(5);Ca = Vehicle_Type(6);Tao = Vehicle_Type(7);Ca_0 = Vehicle_Type(8);
    Ca_1 = Vehicle_Type(9);Ca_2 = Vehicle_Type(10);
    
    [Pp(1),Vp(1),Tp(1)] = traindiscretemodel(u(1),Tim_step,X0(1),X0(2),X0(3),Mass,Radius,g,f,Eta,Ca,Tao,Ca_0,Ca_1,Ca_2);
    for i = 1:Np-1
        [Pp(i+1),Vp(i+1),Tp(i+1)] = traindiscretemodel(u(i+1),Tim_step,Pp(i),Vp(i),Tp(i),Mass,Radius,g,f,Eta,Ca,Tao,Ca_0,Ca_1,Ca_2);
    end
    
    Xp = [Pp,Vp,Tp];      % Predictive State
    
    C = [];
    Ceq = [Pp(Np) - x0;Vp(Np)-v0; Tp(Np) - T0];%Radius*(Ca*v0^2 + Mass*g*f)/Eta];   


end

This is the figure plot function

%% »æͼ

close all
figure;
T = 6
t = (1:Time_sim/Tim_step)*Tim_step;
 plot(t,v0,'m--','linewidth',2);
hold on; plot(t, Velocity(:,1),'r','linewidth',2);
plot(t, Velocity(:,2),'b','linewidth',2);
plot(t, Velocity(:,3),'k','linewidth',2);
plot(t, Velocity(:,4),'g','linewidth',2);
plot(t, Velocity(:,5),'m','linewidth',2);
plot(t, Velocity(:,6),'r--','linewidth',2);
plot(t, Velocity(:,7),'b--','linewidth',2);
h = legend('0','1','2','3','4','5','6','7','Location','SouthEast');
set(h,'box','off'); box off;
xlabel('Time (s)');ylabel('Speed (m/s)');
axis([0 T 19 floor(max(max(Velocity))+1)])
set(gcf,'Position',[250 150 300 350]);

figure;
plot(t, Torque(:,1),'r','linewidth',2);hold on;
plot(t, Torque(:,2),'b','linewidth',2);hold on;
plot(t, Torque(:,3),'k','linewidth',2);hold on;
plot(t, Torque(:,4),'g','linewidth',2);hold on;
plot(t, Torque(:,5),'m','linewidth',2);hold on;
plot(t, Torque(:,6),'r--','linewidth',2);hold on;
plot(t, Torque(:,7),'b--','linewidth',2);hold on;
h = legend('1','2','3','4','5','6','7');
set(h,'box','off'); box off;
xlabel('Time (s)');ylabel('Torque (N)');
xlim([0 T])
set(gcf,'Position',[250 150 300 350]);

figure;
plot(t, (Eta*Torque(:,1)/R(1) - Ca(1)*Velocity(:,1).^2 - Mass(1)*g*f)/Mass(1),'r','linewidth',2);hold on;
plot(t, (Eta*Torque(:,2)/R(2) - Ca(2)*Velocity(:,2).^2 - Mass(2)*g*f)/Mass(2),'b','linewidth',2);hold on;
plot(t, (Eta*Torque(:,3)/R(3) - Ca(3)*Velocity(:,3).^2 - Mass(3)*g*f)/Mass(3),'k','linewidth',2);hold on;
plot(t, (Eta*Torque(:,4)/R(4) - Ca(4)*Velocity(:,4).^2 - Mass(4)*g*f)/Mass(4),'g','linewidth',2);hold on;
plot(t, (Eta*Torque(:,5)/R(5) - Ca(5)*Velocity(:,5).^2 - Mass(5)*g*f)/Mass(5),'m','linewidth',2);hold on;
plot(t, (Eta*Torque(:,6)/R(6) - Ca(6)*Velocity(:,6).^2 - Mass(6)*g*f)/Mass(6),'r--','linewidth',2);hold on;
plot(t, (Eta*Torque(:,7)/R(7) - Ca(7)*Velocity(:,7).^2 - Mass(7)*g*f)/Mass(7),'b--','linewidth',2);hold on;
h = legend('1','2','3','4','5','6','7','Location','NorthEast');
set(h,'box','off'); box off;
xlabel('Time (s)');ylabel('Acceleration (m/s^2)');
xlim([0 T])
set(gcf,'Position',[250 150 300 350]);

figure;
plot(t, x0 - Postion(:,1) - d,'r','linewidth',2);hold on;
plot(t, Postion(:,1) - d - Postion(:,2),'b','linewidth',2);hold on;
plot(t, Postion(:,2) - d - Postion(:,3),'k','linewidth',2);hold on;
plot(t, Postion(:,3) - d - Postion(:,4),'g','linewidth',2);hold on;
plot(t, Postion(:,4) - d - Postion(:,5),'m','linewidth',2);hold on;
plot(t, Postion(:,5) - d - Postion(:,6),'r--','linewidth',2);hold on;
plot(t, Postion(:,6) - d - Postion(:,7),'b--','linewidth',2);hold on;
plot(t, Postion(:,7) - d - Postion(:,8),'k--','linewidth',2);hold on;
h = legend('1','2','3','4','5','6','7');
set(h,'box','off'); box off;
xlabel('Time (s)');ylabel('Spacing error (m)');
xlim([0 T])
set(gcf,'Position',[250 150 300 350]);

figure;
plot(t, Postion(:,1)- (x0- d),'r','linewidth',2);hold on;
plot(t, Postion(:,2)-(x0- 2*d) ,'b','linewidth',2);hold on;
plot(t, Postion(:,3)-(x0- 3*d),'k','linewidth',2);hold on;
plot(t, Postion(:,4)-(x0- 4*d),'g','linewidth',2);hold on;
plot(t, Postion(:,5)-(x0- 5*d),'m','linewidth',2);hold on;
plot(t, Postion(:,6)-(x0- 6*d),'r--','linewidth',2);hold on;
plot(t, Postion(:,7)-(x0- 7*d),'b--','linewidth',2);hold on;
h = legend('1','2','3','4','5','6','7');
set(h,'box','off'); box off;
xlabel('Time (s)');ylabel('Spacing error (m)');
xlim([0 T])
set(gcf,'Position',[250 150 300 350]);

Every Function used in main program

This is the main program

%% DMPC for platoons with PF topology
clc;clear;close all;

load valueparameters.mat

%% Initial Virables 
Postion  = zeros(Num_step,Num_veh);     % postion of each vehicle;
Velocity = zeros(Num_step,Num_veh);     % velocity of each vehicle;
Torque   = zeros(Num_step,Num_veh);     % Braking or Tracking Torque of each vehicle;
U        = zeros(Num_step,Num_veh);     % Desired Braking or Tracking Torque of each vehicle;

Cost    = zeros(Num_step,Num_veh);      % Cost function
Exitflg = zeros(Num_step,Num_veh);      % Stop flag - solvers

% Leading vehicle
d  = 20;                                % Desired spacing
a0 = zeros(Num_step,1); 
v0 = zeros(Num_step,1); 
x0 = zeros(Num_step,1);


% Transient process of leader, which is given in advance
v0(1) = 20; a0(1/Tim_step+1:2/Tim_step) = 2; 
for i = 2:Num_step
    v0(i) = v0(i-1)+a0(i)*Tim_step; 
    x0(i) = x0(i-1)+v0(i)*Tim_step;    
end
 
% Zero initial error for the followers
for i = 1:Num_veh
    Postion(1,i)  = x0(1)-i*d;
    Velocity(1,i) = 20;             
    Torque(1,i)   = (Mass(i)*g*f + Ca(i)*Velocity(1,i)^2)*R(i)/Eta;
end


%% MPC weighted matrix in the cost function
% PF topology --> Fi > Gi+1
% Q1 : leader weighted matrix for state; 
% R1 --> leader weighted matrix for control input
% Fi --> penalty for deviation of its own assumed trajectory 
% Gi --> penalty for deviation of its neighbors' assumed trajectory
 F1 = 10*eye(2); G1 = 0;       Q1 = 10*eye(2);R1 = 1;       
 F2 = 10*eye(2); G2 = 5*eye(2);Q2 = 0*eye(2); R2 = 1;       
 F3 = 10*eye(2); G3 = 5*eye(2);Q3 = 0*eye(2); R3 = 1;      
 F4 = 10*eye(2); G4 = 5*eye(2);Q4 = 0*eye(2); R4 = 1;       
 F5 = 10*eye(2); G5 = 5*eye(2);Q5 = 0*eye(2); R5 = 1;       
 F6 = 10*eye(2); G6 = 5*eye(2);Q6 = 0*eye(2); R6 = 1;      
 F7 = 10*eye(2); G7 = 5*eye(2);Q7 = 0*eye(2); R7 = 1;  

 
 
% Distributed MPC assumed state
Np = 20;                      % Predictive horizon
Pa = zeros(Np,Num_veh);       % Assumed postion of each vehicle;
Va = zeros(Np,Num_veh);       % Assumed velocity of each vehicle;
ua = zeros(Np,Num_veh);       % Assumed Braking or Tracking Torque input of each vehicle;

Pa_next = zeros(Np+1,Num_veh);  % 1(0): Assumed postion of each vehicle at the newt time step;
Va_next = zeros(Np+1,Num_veh);  % Assumed velocity of each vehicle at the newt time step;
ua_next = zeros(Np+1,Num_veh);  % Assumed Braking or Tracking Torque of each vehicle at the newt time step;


% Initialzie the assumed state for the first computation: constant speed
for i = 1:Num_veh
    ua(:,i) = Torque(1,i);
    Pa(1,i) = Postion(1,i);                % The first point should be interpreted as k = 0 (current state)
    Va(1,i) = Velocity(1,i);
    Ta(1,i) = Torque(1,i);
    for j = 1:Np
        [Pa(j+1,i),Va(j+1,i),Ta(j+1,i)] = traindiscretemodel(ua(j,i),Tim_step,Pa(j,i),Va(j,i),Ta(j,i),Mass(i),Ca_0(i),Ca_1(i),Ca_2(i),Tao(i));
    end    
end


tol_opt = 1e-5;
options = optimset('Display','off','TolFun', tol_opt, 'MaxIter', 2000,...
                'LargeScale', 'off', 'RelLineSrchBnd', [], 'RelLineSrchBndDuration', 1);
 
 %%  For debugging
 % Terminal state
 Xend = zeros(Num_step,Num_veh); Vend = zeros(Num_step,Num_veh);
 
 tol_opt = 1e-5;
options = optimset('Display','off','TolFun', tol_opt, 'MaxIter', 2000,...
                'LargeScale', 'off', 'RelLineSrchBnd', [], 'RelLineSrchBndDuration', 1);
 
 %%  For debugging
 % Terminal state
 Xend = zeros(Num_step,Num_veh); Vend = zeros(Num_step,Num_veh);
 
%% Iterative Simulation  
for i = 2:Num_step - Np
    
    fprintf('\n Steps i= %d\n',i)
    
    % Solve optimization problem
    tic
    %% Vehicle one
    Vehicle_Type = [Mass(1),Ca_0(1),Ca_1(1),Ca_2(1),Tao(1),R(1),g,f,Eta,Ca(1)];             % the vehicle parameters ： Mass,R,g,f,Eta,Ca(i),Tao, 
    X0 = [Postion(i-1,1),Velocity(i-1,1),Torque(i-1,1)];            % the vehicle variable in the last time step
    Pd = x0(i-1:i+Np-1) - d;  Vd = v0(i-1:i+Np-1);                  % Np+1 points in total: i-1 last state， i to be optimized
    Xdes = [Pd,Vd];                                                 % Desired state of the first vehicle
    Xa = [Pa(:,1),Va(:,1)];                                         % Assumed state, which is passed to the next vehicle
    Xnba = zeros(Np+1,2);                                           % 1：last state
   
    u0 = ua(:,1);                                                            % initial searching point    
    A = [];b = []; Aeq = []; beq = [];                                       % no linear constraints
    lb = Torquebound(1,1)*ones(Np,1); ub = Torquebound(1,2)*ones(Np,1);      % upper and lower bound for input              
    Pnp = Pd(end,1); Vnp = Vd(end,1);                                        % Terminal constraints
    Xend(i,1) = Pnp; Vend(i,1) = Vnp; Tnp = (Ca(1)*Vnp.^2 + Mass(1)*g*f)/Eta*R(1);
    % MPC - subproblem in vehicle 1
    [u, Cost(i,1), Exitflg(i,1), output] = fmincon(@(u) Fungsicos( Np, Tim_step, X0 ,u, Vehicle_Type,Q1,Xdes,R1,F1,Xa,G1,Xnba), ...
        u0, A, b, Aeq, beq, lb, ub, @(u) kendalanonlinear(Np, Tim_step, X0, u, Vehicle_Type,Pnp,Vnp,Tnp),options); 
    
    % state involves one step
    U(i,1) = u(1);
    [Postion(i,1),Velocity(i,1),Torque(i,1)] = traindiscretemodel(U(i,1),Tim_step,Postion(i-1,1),Velocity(i-1,1),Torque(i-1,1),Mass(1),R(1),g,f,Eta,Ca(1),Tao(1),Ca_0(1),Ca_1(1),Ca_2(1));
    
    % Update assumed state
    Temp = zeros(Np+1,3);
    Temp(1,:) = [Postion(i,1),Velocity(i,1),Torque(i,1)];   
    ua(1:Np-1,1) = u(2:Np);
    for j = 1:Np-1
        [Temp(j+1,1),Temp(j+1,2),Temp(j+1,3)] = traindiscretemodel(ua(j,1),Tim_step,Temp(j,1),Temp(j,2),Temp(j,3),Mass(1),R(1),g,f,Eta,Ca(1),Tao(1),Ca_0(1),Ca_1(1),Ca_2(1));
    end    
    ua(Np,1) = (Ca(1)*Temp(Np,2).^2 + Mass(1)*g*f)/Eta*R(1);
    [Temp(Np+1,1),Temp(Np+1,2),Temp(Np+1,3)] = traindiscretemodel(ua(Np,1),Tim_step,Temp(Np,1),Temp(Np,2),Temp(Np,3),Mass(1),R(1),g,f,Eta,Ca(1),Tao(1),Ca_0(1),Ca_1(1),Ca_2(1));
    Pa_next(:,1) = Temp(:,1);
    Va_next(:,1) = Temp(:,2);
    toc
    
  %% Vehicle two
    tic
    Vehicle_Type = [Mass(2),Ca_0(2),Ca_1(2),Ca_2(2),Tao(2),R(2),g,f,Eta,Ca(2)];                 % the vehicle parameters ： Mass,R,g,f,Eta,Ca(i),Tao, 
    X0 = [Postion(i-1,2),Velocity(i-1,2),Torque(i-1,2)];                % the vehicle variable in the last time
    Pd = zeros(Np+1,1);  Vd = zeros(Np+1,1);                      
    Xdes = [Pd,Vd];  
    Xa = [Pa(:,2),Va(:,2)];                                         
    Xnfa = [Pa(:,1) - d, Va(:,1)];                                         
   
    u0 = ua(:,2);   
    A = [];b = []; Aeq = []; beq = [];                                   
    lb = Torquebound(2,1)*ones(Np,1); ub = Torquebound(2,2)*ones(Np,1);         
    Pnp = Xnfa(end,1); Vnp = Xnfa(end,2);  
    Xend(i,2) = Pnp; Vend(i,2) = Vnp; Tnp = (Ca(2)*Vnp.^2 + Mass(2)*g*f)/Eta*R(2);
    % MPC - subporblem for vehicle 2
    [u, Cost(i,2), Exitflg(i,2), output] = fmincon(@(u) Fungsicos( Np, Tim_step, X0 ,u, Vehicle_Type,Q2,Xdes,R2,F2,Xa,G2,Xnfa), ...
        u0, A, b, Aeq, beq, lb, ub, @(u) kendalanonlinear(Np, Tim_step, X0, u, Vehicle_Type,Pnp,Vnp,Tnp),options); 
    
    % state involves one step
    U(i,2) = u(1);
    [Postion(i,2),Velocity(i,2),Torque(i,2)] = traindiscretemodel(U(i,2),Tim_step,Postion(i-1,2),Velocity(i-1,2),Torque(i-1,2),Mass(2),R(2),g,f,Eta,Ca(2),Tao(2),Ca_0(2),Ca_1(2),Ca_2(2));
    
    % Update assumed state
    Temp = zeros(Np+1,3);
    Temp(1,:) = [Postion(i,2),Velocity(i,2),Torque(i,2)]; 
    ua(1:Np-1,2) = u(2:Np);
    for j = 1:Np-1
        [Temp(j+1,1),Temp(j+1,2),Temp(j+1,3)] = traindiscretemodel(ua(j,2),Tim_step,Temp(j,1),Temp(j,2),Temp(j,3),Mass(2),R(2),g,f,Eta,Ca(2),Tao(2));
    end    
    
    ua(Np,2) = (Ca(2)*Temp(Np,2).^2 + Mass(2)*g*f)/Eta*R(2);
    [Temp(Np+1,1),Temp(Np+1,2),Temp(Np+1,3)] = traindiscretemodel(ua(Np,2),Tim_step,Temp(Np,1),Temp(Np,2),Temp(Np,3),Mass(2),R(2),g,f,Eta,Ca(2),Tao(2));
    Pa_next(:,2) = Temp(:,1);
    Va_next(:,2) = Temp(:,2);
    toc
    
    
    
    %% vehicle three
    tic
    Vehicle_Type = [Mass(3),Ca_0(3),Ca_1(3),Ca_2(3),Tao(3),R(3),g,f,Eta,Ca(3)];                 % the vehicle parameters ： Mass,R,g,f,Eta,Ca(i),Tao, 
    X0 = [Postion(i-1,3),Velocity(i-1,3),Torque(i-1,3)];                % the vehicle variable in the last time
    Pd = zeros(Np+1,1);  Vd = zeros(Np+1,1);                     
    Xdes = [Pd,Vd]; 
    Xa = [Pa(:,3),Va(:,3)];                                           
    Xnfa = [Pa(:,2) - d, Va(:,2)];                                            
   
    u0 = ua(:,3);  
    A = [];b = []; Aeq = []; beq = [];                                    
    lb = Torquebound(3,1)*ones(Np,1); ub = Torquebound(3,2)*ones(Np,1);             
    Pnp = Xnfa(end,1); Vnp = Xnfa(end,2);  
    Xend(i,3) = Pnp; Vend(i,3) = Vnp; Tnp = (Ca(3)*Vnp.^2 + Mass(3)*g*f)/Eta*R(3);
    % MPC-subproblem
    [u, Cost(i,3), Exitflg(i,3), output] = fmincon(@(u) Fungsicos( Np, Tim_step, X0 ,u, Vehicle_Type,Q3,Xdes,R3,F3,Xa,G3,Xnfa), ...
        u0, A, b, Aeq, beq, lb, ub, @(u) kendalanonlinear(Np, Tim_step, X0, u, Vehicle_Type,Pnp,Vnp,Tnp),options); 
    
    % state involves one step
    U(i,3) = u(1);
    [Postion(i,3),Velocity(i,3),Torque(i,3)] = traindiscretemodel(U(i,3),Tim_step,Postion(i-1,3),Velocity(i-1,3),Torque(i-1,3),Mass(3),R(3),g,f,Eta,Ca(3),Tao(3),Ca_0(3),Ca_1(3),Ca_2(3));
    
    % update assumed state
    Temp = zeros(Np+1,3);
    Temp(1,:) = [Postion(i,3),Velocity(i,3),Torque(i,3)];    
    ua(1:Np-1,3) = u(2:Np);
    for j = 1:Np-1
        [Temp(j+1,1),Temp(j+1,2),Temp(j+1,3)] = traindiscretemodel(ua(j,3),Tim_step,Temp(j,1),Temp(j,2),Temp(j,3),Mass(3),R(3),g,f,Eta,Ca(3),Tao(3),Ca_0(3),Ca_1(3),Ca_2(3));
    end   
    
    ua(Np,3) = (Ca(3)*Temp(Np,2).^2 + Mass(3)*g*f)/Eta*R(3);
    [Temp(Np+1,1),Temp(Np+1,2),Temp(Np+1,3)] = traindiscretemodel(ua(Np,3),Tim_step,Temp(Np,1),Temp(Np,2),Temp(Np,3),Mass(3),R(3),g,f,Eta,Ca(3),Tao(3),Ca_0(3),Ca_1(3),Ca_2(3));
    Pa_next(:,3) = Temp(:,1);
    Va_next(:,3) = Temp(:,2);
    toc
    
    
    
    %% vehicle four
    tic
    Vehicle_Type = [Mass(4),Ca_0(4),Ca_1(4),Ca_2(4),Tao(4),R(4),g,f,Eta,Ca(4)];                 % the vehicle parameters ： Mass,R,g,f,Eta,Ca(i),Tao, 
    X0 = [Postion(i-1,4),Velocity(i-1,4),Torque(i-1,4)];                
    Pd = zeros(Np+1,1);  Vd = zeros(Np+1,1);                   
    Xdes = [Pd,Vd];                                
    Xa = [Pa(:,4),Va(:,4)];                                            
    Xnfa = [Pa(:,3) - d, Va(:,3)];                                        
    
    u0 = ua(:,4); 
    A = [];b = []; Aeq = []; beq = [];                                     
    lb = Torquebound(4,1)*ones(Np,1); ub = Torquebound(4,2)*ones(Np,1);              
    Pnp = Xnfa(end,1); Vnp = Xnfa(end,2);   
    Xend(i,4) = Pnp; Vend(i,4) = Vnp; Tnp = (Ca(4)*Vnp.^2 + Mass(4)*g*f)/Eta*R(4);
    % MPC-subproblem
    [u, Cost(i,4), Exitflg(i,4), output] = fmincon(@(u) Fungsicos( Np, Tim_step, X0 ,u, Vehicle_Type,Q3,Xdes,R3,F3,Xa,G3,Xnfa), ...
        u0, A, b, Aeq, beq, lb, ub, @(u) kendalanonlinear(Np, Tim_step, X0, u, Vehicle_Type,Pnp,Vnp,Tnp),options); 
    
    % state involves one step
    U(i,4) = u(1);
    [Postion(i,4),Velocity(i,4),Torque(i,4)] = traindiscretemodel(U(i,4),Tim_step,Postion(i-1,4),Velocity(i-1,4),Torque(i-1,4),Mass(4),R(4),g,f,Eta,Ca(4),Tao(4),Ca_0(4),Ca_1(4),Ca_2(4));
    
    % Update assumed state
    Temp = zeros(Np+1,3);
    Temp(1,:) = [Postion(i,4),Velocity(i,4),Torque(i,4)];  
    ua(1:Np-1,4) = u(2:Np);
    for j = 1:Np-1
        [Temp(j+1,1),Temp(j+1,2),Temp(j+1,3)] = traindiscretemodel(ua(j,4),Tim_step,Temp(j,1),Temp(j,2),Temp(j,3),Mass(4),R(4),g,f,Eta,Ca(4),Tao(4),Ca_0(4),Ca_1(4),Ca_2(4));
    end
    
    ua(Np,4) = (Ca(4)*Temp(Np,2).^2 + Mass(4)*g*f)/Eta*R(4);
    [Temp(Np+1,1),Temp(Np+1,2),Temp(Np+1,3)] = traindiscretemodel(ua(Np,4),Tim_step,Temp(Np,1),Temp(Np,2),Temp(Np,3),Mass(4),R(4),g,f,Eta,Ca(4),Tao(4),Ca_0(4),Ca_1(4),Ca_2(4));
    Pa_next(:,4) = Temp(:,1);
    Va_next(:,4) = Temp(:,2);
    toc
    
    
    
    %% vehicle five
    tic
    Vehicle_Type = [Mass(5),Ca_0(5),Ca_1(5),Ca_2(5),Tao(5),R(5),g,f,Eta,Ca(5)];                
    X0 = [Postion(i-1,5),Velocity(i-1,5),Torque(i-1,5)];             
    Pd = zeros(Np+1,1);  Vd = zeros(Np+1,1);                    
    Xdes = [Pd,Vd];  % Udes = Td;                                   
    Xa = [Pa(:,5),Va(:,5)];                                      
    Xnfa = [Pa(:,4) - d, Va(:,4)];                                        
   
    u0 = ua(:,5);  
    A = [];b = []; Aeq = []; beq = [];                                     
    lb = Torquebound(5,1)*ones(Np,1); ub = Torquebound(5,2)*ones(Np,1);                 
    Pnp = Xnfa(end,1); Vnp = Xnfa(end,2);
    Xend(i,5) = Pnp; Vend(i,5) = Vnp; Tnp = (Ca(5)*Vnp.^2 + Mass(5)*g*f)/Eta*R(5);
    % MPC -subproblem
    [u, Cost(i,5), Exitflg(i,5), output] = fmincon(@(u) Fungsicos( Np, Tim_step, X0 ,u, Vehicle_Type,Q3,Xdes,R3,F3,Xa,G3,Xnfa), ...
        u0, A, b, Aeq, beq, lb, ub, @(u) kendalanonlinear(Np, Tim_step, X0, u, Vehicle_Type,Pnp,Vnp,Tnp),options); 
    
    % state involves one step
    U(i,5) = u(1);
    [Postion(i,5),Velocity(i,5),Torque(i,5)] = traindiscretemodel(U(i,5),Tim_step,Postion(i-1,5),Velocity(i-1,5),Torque(i-1,5),Mass(5),R(5),g,f,Eta,Ca(5),Tao(5),Ca_0(5),Ca_1(5),Ca_2(5));
    
    % update assumed state
    Temp = zeros(Np+1,3);
    Temp(1,:) = [Postion(i,5),Velocity(i,5),Torque(i,5)];   
    ua(1:Np-1,5) = u(2:Np);
    for j = 1:Np-1
        [Temp(j+1,1),Temp(j+1,2),Temp(j+1,3)] = traindiscretemodel(ua(j,5),Tim_step,Temp(j,1),Temp(j,2),Temp(j,3),Mass(5),R(5),g,f,Eta,Ca(5),Tao(5),Ca_0(5),Ca_1(5),Ca_2(5));
    end
    
    ua(Np,5) = (Ca(5)*Temp(Np,2).^2 + Mass(5)*g*f)/Eta*R(5);
    [Temp(Np+1,1),Temp(Np+1,2),Temp(Np+1,3)] = traindiscretemodel(ua(Np,5),Tim_step,Temp(Np,1),Temp(Np,2),Temp(Np,3),Mass(5),R(5),g,f,Eta,Ca(5),Tao(5),Ca_0(5),Ca_1(5),Ca_2(5));
    Pa_next(:,5) = Temp(:,1);
    Va_next(:,5) = Temp(:,2);

    toc
    
    %% vehicle six
    tic
    Vehicle_Type = [Mass(6),Ca_0(6),Ca_1(6),Ca_2(6),Tao(6),R(6),g,f,Eta,Ca(6)];                 % the vehicle parameters ： Mass,R,g,f,Eta,Ca(i),Tao, 
    X0 = [Postion(i-1,6),Velocity(i-1,6),Torque(i-1,6)];                % the vehicle variable in the last time
    Pd = zeros(Np+1,1);  Vd = zeros(Np+1,1);                      
    Xdes = [Pd,Vd];                            
    Xa = [Pa(:,6),Va(:,6)];                                           
    Xnfa = [Pa(:,5) - d, Va(:,5)];                                        
   
    u0 = ua(:,6);  
    A = [];b = []; Aeq = []; beq = [];                                      
    lb = Torquebound(6,1)*ones(Np,1); ub = Torquebound(6,2)*ones(Np,1);           
    Pnp = Xnfa(end,1); Vnp = Xnfa(end,2); 
    Xend(i,6) = Pnp; Vend(i,6) = Vnp; Tnp = (Ca(6)*Vnp.^2 + Mass(6)*g*f)/Eta*R(6);
    % MPC 优化求解
    [u, Cost(i,6), Exitflg(i,6), output] = fmincon(@(u) Fungsicos( Np, Tim_step, X0 ,u, Vehicle_Type,Q3,Xdes,R3,F3,Xa,G3,Xnfa), ...
        u0, A, b, Aeq, beq, lb, ub, @(u) kendalanonlinear(Np, Tim_step, X0, u, Vehicle_Type,Pnp,Vnp,Tnp),options); 
    
    % state involves one step
    U(i,6) = u(1);
    [Postion(i,6),Velocity(i,6),Torque(i,6)] = traindiscretemodel(U(i,6),Tim_step,Postion(i-1,6),Velocity(i-1,6),Torque(i-1,6),Mass(6),R(6),g,f,Eta,Ca(6),Tao(6),Ca_0(6),Ca_1(6),Ca_2(6));
    
    % update assumed state
    Temp = zeros(Np+1,3);
    Temp(1,:) = [Postion(i,6),Velocity(i,6),Torque(i,6)]; 
    ua(1:Np-1,6) = u(2:Np);
    for j = 1:Np-1
        [Temp(j+1,1),Temp(j+1,2),Temp(j+1,3)] = traindiscretemodel(ua(j,6),Tim_step,Temp(j,1),Temp(j,2),Temp(j,3),Mass(6),R(6),g,f,Eta,Ca(6),Tao(6),Ca_0(6),Ca_1(6),Ca_2(6));
    end
     
    ua(Np,6) = (Ca(6)*Temp(Np,2).^2 + Mass(6)*g*f)/Eta*R(6);
    [Temp(Np+1,1),Temp(Np+1,2),Temp(Np+1,3)] = traindiscretemodel(ua(Np,6),Tim_step,Temp(Np,1),Temp(Np,2),Temp(Np,3),Mass(6),R(6),g,f,Eta,Ca(6),Tao(6),Ca_0(6),Ca_1(6),Ca_2(6));
    Pa_next(:,6) = Temp(:,1);
    Va_next(:,6) = Temp(:,2);

    toc
    
    %% vehicle seven
    tic
    Vehicle_Type = [Mass(7),Ca_0(7),Ca_1(7),Ca_2(7),Tao(7),R(7),g,f,Eta,Ca(7)];                 % the vehicle parameters ： Mass,R,g,f,Eta,Ca(i),Tao, 
    X0 = [Postion(i-1,7),Velocity(i-1,7),Torque(i-1,7)];                % the vehicle variable in the last time
    Pd = zeros(Np+1,1);  Vd = zeros(Np+1,1);                    
    Xdes = [Pd,Vd];  % Udes = Td;                                     
    Xa = [Pa(:,7),Va(:,7)];                                            
    Xnfa = [Pa(:,6) - d, Va(:,6)];                                       
   
    u0 = ua(:,7);   
    A = [];b = []; Aeq = []; beq = [];                                       
    lb = Torquebound(7,1)*ones(Np,1); ub = Torquebound(7,2)*ones(Np,1);               
    Pnp = Xnfa(end,1); Vnp = Xnfa(end,2);   
    Xend(i,7) = Pnp; Vend(i,7) = Vnp; Tnp = (Ca(7)*Vnp.^2 + Mass(7)*g*f)/Eta*R(7);
    % MPC-subproblem
    [u, Cost(i,7), Exitflg(i,7), output] = fmincon(@(u) Fungsicos( Np, Tim_step, X0 ,u, Vehicle_Type,Q3,Xdes,R3,F3,Xa,G3,Xnfa), ...
        u0, A, b, Aeq, beq, lb, ub, @(u) kendalanonlinear(Np, Tim_step, X0, u, Vehicle_Type,Pnp,Vnp,Tnp),options); 
    
    % state involves one step
    U(i,7) = u(1);
    [Postion(i,7),Velocity(i,7),Torque(i,7)] = traindiscretemodel(U(i,7),Tim_step,Postion(i-1,7),Velocity(i-1,7),Torque(i-1,7),Mass(7),R(7),g,f,Eta,Ca(7),Tao(7),Ca_0(7),Ca_1(7),Ca_2(7));
    
    % update assumed state
    Temp = zeros(Np+1,3);
    Temp(1,:) = [Postion(i,7),Velocity(i,7),Torque(i,7)];
    ua(1:Np-1,7) = u(2:Np);
    for j = 1:Np-1
        [Temp(j+1,1),Temp(j+1,2),Temp(j+1,3)] = traindiscretemodel(ua(j,7),Tim_step,Temp(j,1),Temp(j,2),Temp(j,3),Mass(7),R(7),g,f,Eta,Ca(7),Tao(7),Ca_0(7),Ca_1(7),Ca_2(7));
    end

    ua(Np,7) = (Ca(7)*Temp(Np,2).^2 + Mass(7)*g*f)/Eta*R(7);
    [Temp(Np+1,1),Temp(Np+1,2),Temp(Np+1,3)] = traindiscretemodel(ua(Np,7),Tim_step,Temp(Np,1),Temp(Np,2),Temp(Np,3),Mass(7),R(7),g,f,Eta,Ca(7),Tao(7),Ca_0(7),Ca_1(7),Ca_2(7));
    Pa_next(:,7) = Temp(:,1);
    Va_next(:,7) = Temp(:,2);

    toc
    
    %% Update assumed data
    Pa = Pa_next;
    Va = Va_next;
       
end

FigurePlot

When I run the main program, there is error like this

Steps i= 2
Error using traindiscretemodel
Too many input arguments.

Error in Fungsicos (line 12)
    [Pp(1),Vp(1),Tp(1)] = traindiscretemodel(u(1),Tim_step,X0(1),X0(2),X0(3),Mass,Ca_0,Ca_1,Ca_2,Tao,Radius,g,f,Eta,Ca);

Error in Train_PF>@(u)Fungsicos(Np,Tim_step,X0,u,Vehicle_Type,Q1,Xdes,R1,F1,Xa,G1,Xnba) (line 115)
    [u, Cost(i,1), Exitflg(i,1), output] = fmincon(@(u) Fungsicos( Np, Tim_step, X0 ,u,
    Vehicle_Type,Q1,Xdes,R1,F1,Xa,G1,Xnba), ...

Error in fmincon (line 566)
      initVals.f = feval(funfcn{3},X,varargin{:});

Error in Train_PF (line 115)
    [u, Cost(i,1), Exitflg(i,1), output] = fmincon(@(u) Fungsicos( Np, Tim_step, X0 ,u,
    Vehicle_Type,Q1,Xdes,R1,F1,Xa,G1,Xnba), ...

Caused by:
    Failure in initial objective function evaluation. FMINCON cannot continue.
 

I don't know why in my traindiscretemodel function said Too Many Input Arguments and the other error. Need Help

[Voss]

Looks like traindiscretemodel is defined to have 10 input arguments, but you are sometimes calling it with 15.

Answer 1

[Walter Roberson]

function [PositionN, VelocityN, TorqueN] = traindiscretemodel(u,Tim_step,Position,Velocity,Torque,Mass,Ca_0,Ca_1,Ca_2,Tao)
% Train Model

10 inputs permitted

    [Pp(1),Vp(1),Tp(1)] = traindiscretemodel(u(1),Tim_step,X0(1),X0(2),X0(3),Mass,Ca_0,Ca_1,Ca_2,Tao,Radius,g,f,Eta,Ca);

15 inputs provided.

In particular Radius, g, f, Eta, Ca are not inputs defined by the function declaration

[Ivan Dwi Putra]

ok thank you. I will see my code

[Ivan Dwi Putra]

I just change the code but now error like this

 Steps i= 4
Elapsed time is 1.072240 seconds.
Error using barrier
Nonlinear constraint function is undefined at initial point. Fmincon cannot continue.

Error in fmincon (line 848)
    [X,FVAL,EXITFLAG,OUTPUT,LAMBDA,GRAD,HESSIAN] = barrier(funfcn,X,A,B,Aeq,Beq,l,u,confcn,options.HessFcn, ...

Error in Train_PF (line 150)
    [u, Cost(i,2), Exitflg(i,2), output] = fmincon(@(u) Fungsicos( Np, Tim_step, X0 ,u,
    Vehicle_Type,Q2,Xdes,R2,F2,Xa,G2,Xnfa), ...