close all;
clear;
clc;
format long;
%% NOW SAME THING BUT FOR THE STRAIGHT WIRE
load y0_Straight_Wire.mat y0 H_tr Piston_Pos0
load Pantograph_results_Straight.mat yout tout
%% Subtracting train-roof height from all vertical DOF
y0(2,:) = y0(2,:) - H_tr;
y0(5,:) = y0(5,:) - H_tr;
y0(8,:) = y0(8,:) - H_tr;
y0(11,:) = y0(11,:) - H_tr;
y0(14,:) = y0(14,:) - H_tr;
y0(17,:) = y0(17,:) - H_tr;
y0(20,:) = y0(20,:) - H_tr;
y0(23,:) = y0(23,:) - H_tr;
Atmos = 101325;
PeS = 4.59362E5*(1-0.0)+Atmos;%bar %70N regulator setting
%Pe=4.854989763905562E5*(1-0.0)+Atmos;%bar % 90N regulator setting
t_inputS = 0:0.001:11;
Piston_Pos0S = Piston_Pos0;
DorificeS = 0.002; %Orifice diameter of pneumatic system
e_20 = y0(6);
e_60 = y0(18);
R_20 = y0(4:5);
R_60 = y0(16:17);
At_2 = [cos(e_20), -sin(e_20); sin(e_20), cos(e_20)];
At_6 = [cos(e_60), -sin(e_60); sin(e_60), cos(e_60)];
u2_Ax = -0.66;
u2_Ay = 0.136;
u2_A = [u2_Ax; u2_Ay];
u6_Fx=-0.044;%%%%%%%%%?
u6_Fy=-0.043;
u6_F = [u6_Fx;u6_Fy];
r26 = R_20 + (At_2*u2_A) - (R_60 + (At_6*u6_F));
l_26s = norm(r26); %initial length of spring-damper system
y0s = y0;
t_input = 0:0.001:11;
d_panhead = interp1(tout,yout(20,:)-H_tr,t_input);
h = t_input(2)-t_input(1);
v_panhead = gradient(d_panhead,t_input);
a_panhead = gradient(v_panhead,t_input);
H = [d_panhead;
v_panhead;
a_panhead;
t_input];
Hs = H;
u_f1_2_=[-0.650;-0.061];
u_f1_1_=[0.647;0.039];
L0_pistonS = pneumatic_actuator_length_function(u_f1_2_,u_f1_1_,y0(4:6),y0(1:3)); %Initial terminal length between base and lower-arm
%% Loading Pantograph initial conditions and Panhead input
load y0_Gradient_1_150DM_S0_800_4.mat y0 H_tr Piston_Pos0
load Pantograph_results_1_150DM_Default.mat yout tout
%% Subtracting train-roof height from all vertical DOF
y0(2,:) = y0(2,:) - H_tr;
y0(5,:) = y0(5,:) - H_tr;
y0(8,:) = y0(8,:) - H_tr;
y0(11,:) = y0(11,:) - H_tr;
y0(14,:) = y0(14,:) - H_tr;
y0(17,:) = y0(17,:) - H_tr;
y0(20,:) = y0(20,:) - H_tr;
y0(23,:) = y0(23,:) - H_tr;
Atmos = 101325;
Pe = 4.59362E5*(1-0.0)+Atmos;%bar %70N regulator setting
%Pe=4.854989763905562E5*(1-0.0)+Atmos;%bar % 90N regulator setting
c2 = 20.6865; %Head suspension damping
%% Calculating the initial legth of my spring-damper system:
e_20 = y0(6);
e_60 = y0(18);
R_20 = y0(4:5);
R_60 = y0(16:17);
At_2 = [cos(e_20), -sin(e_20); sin(e_20), cos(e_20)];
At_6 = [cos(e_60), -sin(e_60); sin(e_60), cos(e_60)];
u2_Ax = -0.66;
u2_Ay = 0.136;
u2_A = [u2_Ax; u2_Ay];
u6_Fx=-0.044;%%%%%%%%%?
u6_Fy=-0.043;
u6_F = [u6_Fx;u6_Fy];
r26 = R_20 + (At_2*u2_A) - (R_60 + (At_6*u6_F));
l_26_0 = norm(r26); %initial length of spring-damper system
%% Setting up input
t_input = 0:0.001:25; %the input timeframe
d_panhead = interp1(tout,yout(20,:)-H_tr,t_input);
h = t_input(2)-t_input(1);
v_panhead = gradient(d_panhead,t_input);
a_panhead = gradient(v_panhead,t_input);
H = [d_panhead;
v_panhead;
a_panhead;
t_input];
u_f1_2_=[-0.650;-0.061];
u_f1_1_=[0.647;0.039];
L0_piston = pneumatic_actuator_length_function(u_f1_2_,u_f1_1_,y0(4:6),y0(1:3)); %Initial terminal length between base and lower-arm
c_body4 = [0.811;-0.001];
Dorifice = 0.002; %Orifice diameter of pneumatic system
%% OPTIMIZATION PARAMETERS
% Define initial guess for parameters [k, c, b]
%initial_params = [78, 10, 6];
% Define initial guess for parameters [k, c, b]
initial_params = [1, 1, 1];
% Define lower and upper bounds for parameters
lb = [0, 0, 0]; % Lower bounds for k, c, b
ub = [1000, 1000, 100]; % Upper bounds for k, c, b
% Define the nonlinear constraint function
nonlcon = @(params) constraint_function(params, y0s, l_26s, PeS, t_inputS, Hs, L0_pistonS, DorificeS, Piston_Pos0S);
% Define patternsearch options including the nonlinear constraint
opts = optimoptions('patternsearch', 'MaxIterations', 10 , 'MaxFunctionEvaluations', 100, 'PlotFcn', @psplotbestf, 'Display', 'iter', 'UseCompletePoll', true, 'StepTolerance', 1e-6);
global n
n = 1;
% Perform optimization using patternsearch with the nonlinear constraint
[opt_params, min_cost] = patternsearch(@(params) simulate_and_calculate_std(params, y0, l_26_0, Pe, t_input,H,L0_piston,Dorifice,Piston_Pos0), initial_params, [], [], [], [], lb, ub, nonlcon, opts);
% Output optimal parameters and minimum cost
disp('Optimal Parameters:');
disp(opt_params);
disp(['Minimum Cost (STDd): ', num2str(min_cost)]);
% Define nonlinear constraint function
function [c, ceq] = constraint_function(params, y0s, l_26s, PeS, t_inputS, Hs, L0_pistonS, DorificeS, Piston_Pos0S)
% Evaluate the constraint function "straight_constraint" with parameters "params"
constraint_value = straight_constraint(params, y0s,l_26s,PeS,t_inputS,Hs,L0_pistonS,DorificeS,Piston_Pos0S);
% Define the inequality constraint: constraint_value <= 15.66
c = constraint_value - 15.66;
% There are no equality constraints, so ceq is empty
ceq = [];
end