How to solve fmincon error line 848?

1 次查看（过去 30 天）
Ivan Dwi Putra
Ivan Dwi Putra 2020-11-5
评论： Walter Roberson 2020-11-6
This is the error
Warning: Integer operands are required for colon operator when used as index.
> In Train_PF (line 23)
Steps i= 2
Elapsed time is 2.445768 seconds.
Elapsed time is 2.569651 seconds.
Elapsed time is 2.301178 seconds.
Elapsed time is 2.385135 seconds.
Elapsed time is 2.421040 seconds.
Elapsed time is 2.495927 seconds.
Elapsed time is 2.441482 seconds.
Elapsed time is 2.567495 seconds.
Elapsed time is 0.108825 seconds.
Elapsed time is 0.082693 seconds.
Steps i= 3
Elapsed time is 2.315649 seconds.
Elapsed time is 2.540296 seconds.
Elapsed time is 2.758976 seconds.
Elapsed time is 2.504796 seconds.
Elapsed time is 2.498014 seconds.
Elapsed time is 2.265725 seconds.
Elapsed time is 2.335359 seconds.
Elapsed time is 2.457487 seconds.
Elapsed time is 0.051261 seconds.
Elapsed time is 0.070154 seconds.
Steps i= 4
Elapsed time is 2.559171 seconds.
Elapsed time is 2.372711 seconds.
Elapsed time is 2.366638 seconds.
Elapsed time is 2.340516 seconds.
Elapsed time is 2.465409 seconds.
Elapsed time is 2.435260 seconds.
Elapsed time is 2.336442 seconds.
Elapsed time is 2.379422 seconds.
Elapsed time is 0.043620 seconds.
Elapsed time is 0.051571 seconds.
Steps i= 5
Elapsed time is 2.432453 seconds.
Elapsed time is 2.583358 seconds.
Elapsed time is 2.184551 seconds.
Elapsed time is 2.453908 seconds.
Elapsed time is 2.363166 seconds.
Elapsed time is 2.490000 seconds.
Elapsed time is 2.245316 seconds.
Elapsed time is 2.304422 seconds.
Elapsed time is 0.053047 seconds.
Elapsed time is 0.041417 seconds.
Steps i= 6
Elapsed time is 3.072459 seconds.
Elapsed time is 3.027455 seconds.
Elapsed time is 3.121872 seconds.
Elapsed time is 3.427046 seconds.
Elapsed time is 3.302124 seconds.
Elapsed time is 3.084401 seconds.
Elapsed time is 2.974774 seconds.
Elapsed time is 3.047564 seconds.
Elapsed time is 0.053087 seconds.
Elapsed time is 0.047509 seconds.
Steps i= 7
Elapsed time is 2.344178 seconds.
Elapsed time is 2.286563 seconds.
Elapsed time is 2.670974 seconds.
Elapsed time is 2.285991 seconds.
Elapsed time is 2.342094 seconds.
Elapsed time is 2.419631 seconds.
Elapsed time is 2.477580 seconds.
Elapsed time is 2.725343 seconds.
Elapsed time is 0.060547 seconds.
Elapsed time is 0.065780 seconds.
Steps i= 8
Elapsed time is 0.041389 seconds.
Elapsed time is 0.046428 seconds.
Elapsed time is 0.045194 seconds.
Elapsed time is 0.047166 seconds.
Elapsed time is 0.039004 seconds.
Elapsed time is 0.039632 seconds.
Elapsed time is 0.037557 seconds.
Elapsed time is 0.040774 seconds.
Elapsed time is 0.057348 seconds.
Elapsed time is 0.058088 seconds.
Steps i= 9
Elapsed time is 2.198259 seconds.
Elapsed time is 2.478802 seconds.
Elapsed time is 2.296305 seconds.
Elapsed time is 2.281409 seconds.
Elapsed time is 2.146461 seconds.
Elapsed time is 2.170087 seconds.
Elapsed time is 2.193114 seconds.
Elapsed time is 2.173746 seconds.
Elapsed time is 3.124503 seconds.
Elapsed time is 2.993703 seconds.
Steps i= 10
Elapsed time is 2.087948 seconds.
Elapsed time is 2.171360 seconds.
Elapsed time is 4.013461 seconds.
Elapsed time is 2.630125 seconds.
Elapsed time is 3.061610 seconds.
Elapsed time is 2.178988 seconds.
Elapsed time is 2.976232 seconds.
Elapsed time is 2.979393 seconds.
Elapsed time is 2.337152 seconds.
Elapsed time is 3.767039 seconds.
Steps i= 11
Error using barrier
Objective 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 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), ...
This is all my code
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,R,g,f,Eta,Ca)
% 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;8500;8443;8396;8491;8095;8000;8000];
% Parameter Gaya
u = [205*10^3;302*10^3;205*10^3;302*10^3;205*10^3;302*10^3;302*10^3;302*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;0.01176;0.01176;0.01176;];
Ca_1 = [0.00077616;0.00077616;0.00077616;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;4.48;4.48;4.48;];
Ca = [2.39900000000000;2.48000000000000;2.47140000000000;2.48000000000000;2.46860000000000;2.45920000000000;2.47820000000000;2.39900000000000;2.38000000000000;2.38000000000000];
% Desired Speed
v_0 = 300;
% Desired Train Following Headway Time
hstar = 120;
Num_step = 100;
Num_veh = 10;
AccMax = 6;
AccMin = -6;
Eta = 0.9600;
f = 0.0100;
g = 9.800;
i = 10;
Tim_step = 50000; % first 0.1
Time_sim = 0.1;
R = [0.604750000000000;0.625000000000000;0.622850000000000;0.625000000000000;0.622150000000000;0.619800000000000;0.624550000000000;0.604750000000000;0.600000000000000;0.600000000000000];
Tao = [2.62850000000000;2.75000000000000;2.73710000000000;2.75000000000000;2.73290000000000;2.71880000000000;2.74730000000000;2.62850000000000;2.60000000000000;2.60000000000000];
Torquebound = [-30596.5703125000,30596.5703125000;-33203.1250000000,33203.1250000000;-32921.5153125000,32921.5153125000;-33203.1250000000,33203.1250000000;-32830.0778125000,32830.0778125000;-32524.0050000000,32524.0050000000;-33144.0878125000,33144.0878125000;-30596.5703125000,30596.5703125000;-30000.0000000000,30000.0000000000;-30000.0000000000,30000.0000000000];
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 = 10; % The number of vehicles in a platoon;
Tim_step = 0.01; % Time step;
Time_sim = 100; % Time length for simulation
Num_step = Time_sim/Tim_step; % Simulation setps
Mass = [8095;8500;8457;8500;8443;8396;8491;8095;8000;8000]; % 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 = 1/2*0.4*2*1.23; % ·ç×è
R = 3 +(Mass - 2000)/2000 * 0.1 ; % °ë¾¶
%% Acceleration bounds -6,6
AccMax = 6; AccMin = -6;
Torquebound = zeros(9,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);
plot(t, Velocity(:,8),'b--o','linewidth',2);
plot(t, Velocity(:,9),'c*','linewidth',2);
plot(t, Velocity(:,10),'m--','linewidth',2);
h = legend('0','1','2','3','4','5','6','7','8','9','10','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 400 450]);
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;
plot(t, Torque(:,8),'b--o','linewidth',2);hold on;
plot(t, Torque(:,9),'c*','linewidth',2);hold on;
plot(t, Torque(:,10),'m--','linewidth',2);hold on;
h = legend('1','2','3','4','5','6','7','8','9','10');
set(h,'box','off'); box off;
xlabel('Time (s)');ylabel('Torque (N)');
xlim([0 T])
set(gcf,'Position',[250 150 300 350 400 450]);
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;
plot(t, (Eta*Torque(:,8)/R(8) - Ca(8)*Velocity(:,8).^2 - Mass(8)*g*f)/Mass(8),'b--o','linewidth',2);hold on;
plot(t, (Eta*Torque(:,9)/R(9) - Ca(9)*Velocity(:,9).^2 - Mass(9)*g*f)/Mass(9),'c*','linewidth',2);hold on;
plot(t, (Eta*Torque(:,10)/R(10) - Ca(10)*Velocity(:,10).^2 - Mass(10)*g*f)/Mass(10),'m--','linewidth',2);hold on;
h = legend('1','2','3','4','5','6','7','8','9','10','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 400 450]);
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;
plot(t, Postion(:,8) - d - Postion(:,9),'b--o','linewidth',2);hold on;
plot(t, Postion(:,9) - d - Postion(:,10),'c*','linewidth',2);hold on;
h = legend('1','2','3','4','5','6','7','8','9','10');
set(h,'box','off'); box off;
xlabel('Time (s)');ylabel('Spacing error (m)');
xlim([0 T])
set(gcf,'Position',[250 150 300 350 400 450]);
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;
plot(t, Postion(:,8)-(x0- 8*d),'b--o','linewidth',2);hold on;
plot(t, Postion(:,9)-(x0- 9*d),'c*','linewidth',2);hold on;
plot(t, Postion(:,10)-(x0- 10*d),'m--','linewidth',2);hold on;
h = legend('1','2','3','4','5','6','7','8','9','10');
set(h,'box','off'); box off;
xlabel('Time (s)');ylabel('Spacing error (m)');
xlim([0 T])
set(gcf,'Position',[250 150 300 350 400 450]);
Every Function used in main program
This is the main program
%% DMPC for platoons with PF topology
clc;clear;close all;
% load PlatoonParameter.mat % This set of parameters was used in the paper
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;
F8 = 10*eye(2); G8 = 5*eye(2);Q8 = 0*eye(2); R8 = 1;
F9 = 10*eye(2); G9 = 5*eye(2);Q9 = 0*eye(2); R9 = 1;
F10 = 10*eye(2);G10= 5*eye(2);Q10 = 0*eye(2);R10 = 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),R(i),g,f,Eta,Ca(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
%% Vehicle Eight
tic
Vehicle_Type = [Mass(8),Ca_0(8),Ca_1(8),Ca_2(8),Tao(8),R(8),g,f,Eta,Ca(8)]; % the vehicle parameters ： Mass,R,g,f,Eta,Ca(i),Tao,
X0 = [Postion(i-1,8),Velocity(i-1,8),Torque(i-1,8)]; % the vehicle variable in the last time
Pd = zeros(Np+1,1); Vd = zeros(Np+1,1);
Xdes = [Pd,Vd]; % Udes = Td;
Xa = [Pa(:,8),Va(:,8)];
Xnfa = [Pa(:,7) - d, Va(:,7)];
u0 = ua(:,8);
A = [];b = []; Aeq = []; beq = [];
lb = Torquebound(8,1)*ones(Np,1); ub = Torquebound(8,2)*ones(Np,1);
Pnp = Xnfa(end,1); Vnp = Xnfa(end,2);
Xend(i,8) = Pnp; Vend(i,8) = Vnp; Tnp = (Ca(8)*Vnp.^2 + Mass(8)*g*f)/Eta*R(8);
% MPC-subproblem
[u, Cost(i,8), Exitflg(i,8), 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,8) = u(1);
[Postion(i,8),Velocity(i,8),Torque(i,8)] = traindiscretemodel(U(i,8),Tim_step,Postion(i-1,8),Velocity(i-1,8),Torque(i-1,8),Mass(8),R(8),g,f,Eta,Ca(8),Tao(8),Ca_0(8),Ca_1(8),Ca_2(8));
% update assumed state
Temp = zeros(Np+1,3);
Temp(1,:) = [Postion(i,8),Velocity(i,8),Torque(i,8)];
ua(1:Np-1,8) = u(2:Np);
for j = 1:Np-1
[Temp(j+1,1),Temp(j+1,2),Temp(j+1,3)] = traindiscretemodel(ua(j,8),Tim_step,Temp(j,1),Temp(j,2),Temp(j,3),Mass(8),R(8),g,f,Eta,Ca(8),Tao(8),Ca_0(8),Ca_1(8),Ca_2(8));
end
ua(Np,8) = (Ca(8)*Temp(Np,2).^2 + Mass(8)*g*f)/Eta*R(8);
[Temp(Np+1,1),Temp(Np+1,2),Temp(Np+1,3)] = traindiscretemodel(ua(Np,8),Tim_step,Temp(Np,1),Temp(Np,2),Temp(Np,3),Mass(8),R(8),g,f,Eta,Ca(8),Tao(8),Ca_0(8),Ca_1(8),Ca_2(8));
Pa_next(:,8) = Temp(:,1);
Va_next(:,8) = Temp(:,2);
toc
%% Vehicle Nine
tic
Vehicle_Type = [Mass(9),Ca_0(9),Ca_1(9),Ca_2(9),Tao(9),R(9),g,f,Eta,Ca(9)]; % the vehicle parameters ： Mass,R,g,f,Eta,Ca(i),Tao,
X0 = [Postion(i-1,9),Velocity(i-1,9),Torque(i-1,9)]; % the vehicle variable in the last time
Pd = zeros(Np+1,1); Vd = zeros(Np+1,1);
Xdes = [Pd,Vd]; % Udes = Td;
Xa = [Pa(:,9),Va(:,9)];
Xnfa = [Pa(:,8) - d, Va(:,8)];
u0 = ua(:,9);
A = [];b = []; Aeq = []; beq = [];
lb = Torquebound(9,1)*ones(Np,1); ub = Torquebound(9,2)*ones(Np,1);
Pnp = Xnfa(end,1); Vnp = Xnfa(end,2);
Xend(i,9) = Pnp; Vend(i,9) = Vnp; Tnp = (Ca(9)*Vnp.^2 + Mass(9)*g*f)/Eta*R(9);
% MPC-subproblem
[u, Cost(i,9), Exitflg(i,9), 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,9) = u(1);
[Postion(i,9),Velocity(i,9),Torque(i,9)] = traindiscretemodel(U(i,9),Tim_step,Postion(i-1,9),Velocity(i-1,9),Torque(i-1,9),Mass(9),R(9),g,f,Eta,Ca(9),Tao(9),Ca_0(9),Ca_1(9),Ca_2(9));
% update assumed state
Temp = zeros(Np+1,3);
Temp(1,:) = [Postion(i,9),Velocity(i,9),Torque(i,9)];
ua(1:Np-1,9) = u(2:Np);
for j = 1:Np-1
[Temp(j+1,1),Temp(j+1,2),Temp(j+1,3)] = traindiscretemodel(ua(j,9),Tim_step,Temp(j,1),Temp(j,2),Temp(j,3),Mass(9),R(9),g,f,Eta,Ca(9),Tao(9),Ca_0(9),Ca_1(9),Ca_2(9));
end
ua(Np,9) = (Ca(9)*Temp(Np,2).^2 + Mass(9)*g*f)/Eta*R(9);
[Temp(Np+1,1),Temp(Np+1,2),Temp(Np+1,3)] = traindiscretemodel(ua(Np,9),Tim_step,Temp(Np,1),Temp(Np,2),Temp(Np,3),Mass(9),R(9),g,f,Eta,Ca(9),Tao(9),Ca_0(9),Ca_1(9),Ca_2(9));
Pa_next(:,9) = Temp(:,1);
Va_next(:,9) = Temp(:,2);
toc
%% Vehicle Ten
tic
Vehicle_Type = [Mass(10),Ca_0(10),Ca_1(10),Ca_2(10),Tao(10),R(10),g,f,Eta,Ca(10)]; % the vehicle parameters ： Mass,R,g,f,Eta,Ca(i),Tao,
X0 = [Postion(i-1,10),Velocity(i-1,10),Torque(i-1,10)]; % the vehicle variable in the last time
Pd = zeros(Np+1,1); Vd = zeros(Np+1,1);
Xdes = [Pd,Vd]; % Udes = Td;
Xa = [Pa(:,10),Va(:,10)];
Xnfa = [Pa(:,9) - d, Va(:,9)];
u0 = ua(:,10);
A = [];b = []; Aeq = []; beq = [];
lb = Torquebound(10,1)*ones(Np,1); ub = Torquebound(10,2)*ones(Np,1);
Pnp = Xnfa(end,1); Vnp = Xnfa(end,2);
Xend(i,10) = Pnp; Vend(i,10) = Vnp; Tnp = (Ca(10)*Vnp.^2 + Mass(10)*g*f)/Eta*R(10);
% MPC-subproblem
[u, Cost(i,10), Exitflg(i,10), 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,10) = u(1);
[Postion(i,10),Velocity(i,10),Torque(i,10)] = traindiscretemodel(U(i,10),Tim_step,Postion(i-1,10),Velocity(i-1,10),Torque(i-1,10),Mass(10),R(10),g,f,Eta,Ca(10),Tao(10),Ca_0(10),Ca_1(10),Ca_2(10));
% update assumed state
Temp = zeros(Np+1,3);
Temp(1,:) = [Postion(i,10),Velocity(i,10),Torque(i,10)];
ua(1:Np-1,10) = u(2:Np);
for j = 1:Np-1
[Temp(j+1,1),Temp(j+1,2),Temp(j+1,3)] = traindiscretemodel(ua(j,10),Tim_step,Temp(j,1),Temp(j,2),Temp(j,3),Mass(10),R(10),g,f,Eta,Ca(10),Tao(10),Ca_0(10),Ca_1(10),Ca_2(10));
end
ua(Np,10) = (Ca(10)*Temp(Np,2).^2 + Mass(10)*g*f)/Eta*R(10);
[Temp(Np+1,1),Temp(Np+1,2),Temp(Np+1,3)] = traindiscretemodel(ua(Np,10),Tim_step,Temp(Np,1),Temp(Np,2),Temp(Np,3),Mass(10),R(10),g,f,Eta,Ca(10),Tao(10),Ca_0(10),Ca_1(10),Ca_2(10));
Pa_next(:,10) = Temp(:,1);
Va_next(:,10) = Temp(:,2);
toc
%% Update assumed data
Pa = Pa_next;
Va = Va_next;
end
FigurePlot
  2 个评论
Alan Weiss
Alan Weiss 2020-11-6
I suggest that you edit this question to describe the problem you are having and what kind of help you need. Remove the huge code blocks and, if you like, include them as attachments.
As it is, your problem descriptions are hidden in massive code blocks.
Alan Weiss
MATLAB mathematical toolbox documentation
Walter Roberson
Walter Roberson 2020-11-6
In my tests, iteration 9 gives a warning about singular matrix when estimating the gradient.
By iteration 11, the final entry of u has grown large enough that the result of calling the function on u0 is infinity.

请先登录，再进行评论。

请先登录，再回答此问题。

回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Code Generation 的更多信息

标签

产品


版本

R2020b

Community Treasure Hunt

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

Start Hunting!

Translated by