Too Many Input Arguments

1 visualización (últimos 30 días)
Ivan Dwi Putra
Ivan Dwi Putra el 3 de Nov. de 2020
Comentada: Ivan Dwi Putra el 4 de Nov. de 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 comentario
Voss
Voss el 3 de Nov. de 2020
Looks like traindiscretemodel is defined to have 10 input arguments, but you are sometimes calling it with 15.

Iniciar sesión para comentar.

Iniciar sesión para responder a esta pregunta.

Respuestas (1)

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

Iniciar sesión para comentar.

Categorías

Más información sobre Code Generation en Help Center y File Exchange.

Etiquetas

Productos


Versión

R2020b

Community Treasure Hunt

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

Start Hunting!

Translated by