Too Many Input Arguments

1 Ansicht (letzte 30 Tage)
Ivan Dwi Putra
Ivan Dwi Putra am 3 Nov. 2020
Kommentiert: Ivan Dwi Putra am 4 Nov. 2020
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
  1 Kommentar
Voss
Voss am 3 Nov. 2020
Looks like traindiscretemodel is defined to have 10 input arguments, but you are sometimes calling it with 15.

Melden Sie sich an, um zu kommentieren.

Melden Sie sich an, um diese Frage zu beantworten.

Antworten (1)

Walter Roberson
Walter Roberson am 3 Nov. 2020
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
  2 Kommentare
Ivan Dwi Putra
Ivan Dwi Putra am 4 Nov. 2020
ok thank you. I will see my code
Ivan Dwi Putra
Ivan Dwi Putra am 4 Nov. 2020
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), ...

Melden Sie sich an, um zu kommentieren.

Kategorien

Mehr zu Code Generation finden Sie in Help Center und File Exchange

Tags

Produkte


Version

R2020b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by