Make this optimization work
Mostra commenti meno recenti
i am developing a 6dof rocket launch problm and i am facing issues optimizing the control law coefficients, The coefficients are 9 in number and have to be optimized. Please find the code below. I have used ga, swarm op, sqp, fmincon, fminmax with no good result. Any aerospace engineers with good opinions are welcome, Code below is sqp optimization.
****
model_6dof_12_opt_sqp_vert_up_cu_ruliu_x16()
function model_6dof_12_opt_sqp_vert_up_cu_ruliu_x16()
%% === Optimization Setup ===
x0 = [0, 0, 0, 0, 0, 0, 0, 0, 0];
lb = [0, 0, 0, 0, 0, 0, 0, 0, 0];
ub = [4, 4, 4, 4, 4, 4, 4, 4, 4];
options = optimoptions('fmincon', ...
'Algorithm', 'sqp', ... % SQP is better for nonlinear problems
'Display', 'iter-detailed', ... % More detailed output
'MaxFunctionEvaluations', 1e5, ... % Increase to allow deeper search
'MaxIterations', 1000, ... % Allow more iterations
'OptimalityTolerance', 1e-6, ... % Stricter stopping criterion
'StepTolerance', 1e-10, ... % Better for precise convergence
'FunctionTolerance', 1e-6, ... % Helps with simulation noise
'UseParallel', false, ... % Enable parallel evaluations (requires Parallel Toolbox)
'FiniteDifferenceType', 'central', ... % More accurate gradient estimation (but slower)
'FiniteDifferenceStepSize', 1e-6); % Fine-tune step size for finite differences
[opt_gains, ~] = fmincon(@cost_function, x0, [], [], [], [], lb, ub, [], options);
fprintf('\n=== Optimized Gains ===\n');
fprintf('k4 = %.4f\n k5 = %.4f\n k7 = %.4f\n k8 = %.4f\n k9 = %.4f\n k1 = %.4f\n k2 = %.4f\n k6 = %.4f\n k3 = %.4f\n', opt_gains);
global k4 k5 k7 k8 k9 k1 k2 k6 k3
k4 = opt_gains(1); k5 = opt_gains(2);
k7 = opt_gains(3); k8 = opt_gains(4); k9 = opt_gains(5);
k1 = opt_gains(6); k2 = opt_gains(7);
k6 = opt_gains(8); k3 = opt_gains(9);
% Desired state values
global x1d x2d x3d x6d x9d x10d x12d x4d x5d x7d x8d
x1d = 0; % vx
x2d = 5; % vy
x3d = 0; % vz
x6d = 0; % r
x9d = 90 * pi/180; % pitch angle
x10d = 0; % x position
x12d = 0; % z position
x4d = 0; % desired roll angular velocity p
x5d = 0; % desired yaw angular velocity q
x7d = 0; % desired roll phi x7 angle
x8d = 0; % desired yaw psi x8 angle
dx = zeros(17,1);
[t, X] = simulate_rocket();
end
%% === Cost Function ===
function J = cost_function(gains)
global k4 k5 k7 k8 k9 k1 k2 k6 k3 x1d x2d x3d x6d x9d x10d x12d x4d x5d x7d x8d
k4 = gains(1); k5 = gains(2); k7 = gains(3); k8 = gains(4); k9 = gains(5);
k1 = gains(6); k2 = gains(7);
k6 = gains(8); k3 = gains(9);
try
[~, X] = simulate_rocket();
x = X(end, :); % Final state
J = ...
(x(1) - x1d)^2 + ...
(x(2) - x2d)^2 + ...
(x(3) - x3d)^2 + ...
(x(6) - x6d)^2 + ...
(x(9) - x9d)^2 + ...
(x(10) - x10d)^2 + ...
(x(12) - x12d)^2 +...
(x(4) - x4d)^2 + ...
(x(5) - x5d)^2 + ...
(x(7) - x7d)^2 + ...
(x(8) - x8d)^2 ;
catch
J = 1e6; % Penalty if simulation fails
end
end
%% === Simulate Rocket ===
function [t, X] = simulate_rocket()
global md xcmd ic jxd jyd jzd df
global k4 k5 k7 k8 k9 k1 k2 k6 k3 x1d x2d x3d x6d x9d x10d x12d x4d x5d x7d x8d yI
md = [100. 30.]; % initial and final mass
xcmd = [0.666 0.456]; % centrul de msa
jxd = [10. 3.]; % transversal moment of inertia, first and last values
jyd = [15. 7.]; % transversal moment of inertia, first and last values
jzd = [15. 7.]; % transversal moment of inertia, first and last values
% ic = 1; % variable for defining mission part
df = 0.6; % rocket diameter
yI=0;
%% ====== Initial Conditions (X0) ======
X0(1) = 0; % initial vx (local frame velocity along oX axis)
X0(2) = 1; % initial vy (local frame velocity along oY axis)
X0(3) = 0; % initial vz (local frame velocity along oZ axis)
X0(4) = 0.; % initial p - body frame rotation velocity around oX axis
X0(5) = 0.; % initial q - body frame rotation velocity around oY axis
X0(6) = 0.; % initial r - body frame rotation velocity around oZ axis
X0(7) = 0.*pi/180; % initial phi - roll angle - rotation around oX axis
X0(8) = 0.*pi/180; % initial psi - yaw angle - rotation around oY axis
X0(9) = 90*pi/180; % initial teta - pitch angle - rotation around oZ axis
X0(10) = 0; % initial oX axis position in inertial frame
X0(11) = 1; % initial oY axis position in inertial frame
X0(12) = 0; % initial oZ axis position in inertial frame
X0(13) = md(1); % initial mass of rocket
X0(14) = 1; % initial axial thrust command
X0(15) = 0.; % initial pitch thrust command
X0(16) = 0.0; % initial roll thrust command
X0(17) = 0*pi/180; % initial yaw thrust command
%% Integration
Xf = zeros(2,17);
tf = zeros(2,1);
% ====== Time Span ======
tfinal = 5; % total time of simulation
nt = 100; % number of time steps
dt = tfinal/nt; % fixed time step size
th = 0; % initial time [s]
for i=1:nt+1
t1 = th;
t2 = th+dt;
tspan = [t1 t2]; % time vector
[t, X] = ode45(@(t,X) model6dof(X,t), tspan, X0); %integration function
th = t2; %new t2 for the following i step
last = size(t,1); %for extracting the last value of the dt step
X0 = X(last,:); %extracting the last value of the dt integration
if X0(11) <= 0 %condition for stopping simulation and integration when altitude <0
% disp ('atins Pamint')
break;
end
Xf(i,:) = X0(:); %storing values for each dt integration
tf(i) = t2; %storing the last t from each dt integration
% tf
end
end
%% === 6dof Rocket model ===
function [dx]=model6dof(X,t)
% global ic T1 T2 md df
global T1 T2 md df yI
global k4 k5 k7 k8 k9 k1 k2 k6 k3 x1d x2d x3d x6d x9d x10d x12d x4d x5d x7d x8d
x1=X(1); %local frame velocity along oX axis
x2=X(2); %local frame velocity along oY axis
x3=X(3); %local frame velocity along oZ axis
x4=X(4); %body frame rotation velocity around oX axis
x5=X(5); %body frame rotation velocity around oY axis
x6=X(6); %body frame rotation velocity around oZ axis
x7=X(7); %roll angle - rotation around oX axis
x8=X(8); %yaw angle - rotation around oY axis
x9=X(9); %pitch angle - rotation around oZ axis
x10=X(10); %oX axis position in inertial frame
x11=X(11); %oY axis position in inertial frame
x12=X(12); %oZ axis position in inertial frame
m=X(13); %dm - mass variation
x14=X(14); %axial thrust command
x15=X(15); %thrust pitch command
x16=X(16); %roll thrust command
x17=X(17); %yaw pitch command
%%
fi=x7; %roll angle - rotation around oX axis
ps=x8; %yaw angle - rotation around oY axis
te=x9; %pitch angle - rotation around oZ axis
SFI = sin( fi ); %simplifying notations to populate the A matrix
CFI = cos( fi ); %simplifying notations to populate the A matrix
STE = sin( te ); %simplifying notations to populate the A matrix
CTE = cos( te ); %simplifying notations to populate the A matrix
SPS = sin( ps ); %simplifying notations to populate the A matrix
CPS = cos( ps ); %simplifying notations to populate the A matrix
A(1,1) = CPS*CTE;
A(1,2) = CPS*STE;
A(1,3) = -SPS;
A(2,1) = -CFI*STE+SFI*SPS*CTE;
A(2,2) = CFI*CTE+SFI*SPS*STE;
A(2,3) = SFI*CPS;
A(3,1) = SFI*STE+CFI*SPS*CTE;
A(3,2) = -SFI*CTE+CFI*SPS*STE;
A(3,3) = CFI*CPS;
% A matrix for rotating from inertial frame to body frame
AI=[A(1,1) A(1,2) A(1,3);...
A(2,1) A(2,2) A(2,3);...
A(3,1) A(3,2) A(3,3)];
V_l=[x1;x2;x3]; %inertial frame veocity vector
V_b=AI*V_l; %body frame velocity vector
x1b=V_b(1); %body frame velocity along oX axis
x2b=V_b(2); %body frame velocity along oY axis
x3b=V_b(3); %body frame velocity along oZ axis
Va=sqrt(x1b^2+x2b^2+x3b^2); %absolute velocity
[xcm,jx,jy,jz]= meca(m);
%%Aerodynamic force
T=-31-0.000998*x11; %atmospheric temperature variation with altitude
p=0.699*exp(-0.00009*x11); %atmospheric pressure variation with altitude
rho=p/(0.1921*(T+273.1)); %atmospheric density variation with altitude
eta=1.81e-5; %atmospheric viscosity
S=pi/4*df^2; %rocket frontal area
Q=0.5*rho*Va^2; %dynamic prssure
l=1; %length of rocket
Re=rho*Va*l/eta; %Reynold number
CD=dragcoefvar(Re); %drag coefficient calculation
CX=-CD*x1b/Va; %drag coefficient along oX axis
CY=-CD*x2b/Va; %drag coefficient along oY axis
CZ=-CD*x3b/Va; %drag coefficient along oZ axis
FA_s=[ CX*Q*S; %aerodynamic drag on all axes in the body frame
CY*Q*S;
CZ*Q*S];
FA_b=FA_s;
%% acc grav
g=3.73; %gravitational acceleration
% m=ms+x13; %mass formula with mass variation
g_b=[0; %gravitational acceleration vector on all three axesin the inertial frame
-g;
0];
Fg_b=g_b; %gravitational force
%% control definition
T0=1200; %maximum thrust for rocket engine
%% %%%%%%%%%%%%%%% VERTICAL LAUNCH AND VERTICAL INITIAL FLIGHT %%%%%%%%%%%%%%%%%
hx=x10-x10d; %difference between actual x10 and desired x10
hy=0; %difference between actual x11 and desired x11
hz=x12-x12d; %difference between actual x12 and desired x12
Vxt=x1-x1d; %difference between actual vx and desired vx
Vyt=x2-x2d; %difference between actual vy and desired vy
Vzt=x3-x3d; %difference between actual vz and desired vz
GR=m*g; %gravitational force
XAA=FA_b(1); %aerodynamic drag force on the oY axis
XTA=GR-XAA; %difference between gravitational force and aerodynamic term on the oX axis
ut0=XTA/T0; %axial thrust control term
un0=0; %pitch control term
%% control limits
utmin=0.25; %inferior limit for axial thrust control
utmax=1; %inferior limit for axial thrust control
if(x14>=utmax) %imposing u3 limits on calculated u1 values
x14=utmax;
elseif(x14<=utmin)
x14=utmin;
end
%-------
unmin=-7*pi/180; %inferior limit for pitch control
unmax=7*pi/180; %superior limit for pitch control
if(x15>=unmax) %imposing u1 limits on calculated u1 values
x15=unmax;
elseif(x15<=unmin)
x15=unmin;
end
%-------
urmin=-1; %inferior limit for pitch control
urmax=1; %superior limit for pitch control
if(x16>=urmax) %imposing u1 limits on calculated u1 values
x16=urmax;
elseif(x16<=urmin)
x16=urmin;
end
%-------
u2min=-7*pi/180; %inferior limit for yaw control
u2max=7*pi/180; %inferior limit for yaw control
if(x17>=u2max) %imposing u2 limits on calculated u1 values
x17=u2max;
elseif(x17<=u2min)
x17=u2min;
end
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
desc = m/md(1); %command attenuation with mass decrease
uv = k7*desc*(x1*AI(1,1)+x2*AI(1,2)+x3*AI(1,3)); %control term for absolute velocity
ux = k8*desc*hx+k9*desc*Vxt; %control term for oX translation and oX velocity
uy = k8*desc*hy+k9*desc*Vyt; %control term for oY translation and oY velocity
uz = k8*desc*hz+k9*desc*Vzt; %control term for oZ translation and oZ velocity
% longitudinal commands
u3 = 1.*ut0-1*uv-ux*AI(1,1)-uy*AI(1,2)-uz*AI(1,3); %thrust control
ut = (-x14/0.5+u3/0.5);
upi = un0-ux*AI(2,1)-uy*AI(2,2)-uz*AI(2,3)-1*k4*(x9-x9d)-k3*(x6-x6d); %pitch control
un = -x15/0.1+upi/0.1;
% lateral commands
u4 = -1.*k2*(x7-x7d)-k1*(x4-x4d); % roll command
ur = -x16/0.1+u4/0.1;
ur1=u4 -yI; % intrarea in integrator
x1e=x16; % iesirea din integrator Iu1
x1f=ur1-x1e; % semn derivata
xxa=1.d-2; % latimea insensibilitatii
xxb=1.d-1; % latimea histerezis
[yI,d1,d2,d3,d4]= bloc2(x1e,x1f,xxa,xxb);
del=[d1 d2 d3 d4];
dlt=(del(2)+del(4)-del(1)-del(3))/2;
% yaw command
uya = -1.*k6*(x8-x8d)-k5*(x5-x5d)+ux*AI(3,1)+uy*AI(3,2)+uz*AI(3,3);
um = -x17/0.1+uya/0.1;
%%
F1=x14*T0; %thrust force with thrust control
Isp=230; %rocket engine specific impulse
dm=-F1/Isp/g; %mass variation formula
C1=[cos(x17) 0 -sin(x17); %thrust rotation matrix around body frame oY axis
0 1 0;
sin(x17) 0 cos(x17)];
C2=[cos(x15) sin(x15) 0; %thrust rotation matrix around body frame oY axis
-sin(x15) cos(x15) 0;
0 0 1];
FE1_b=(C2*C1)*[F1;0;0]; %thrust in body frame
%%F total
Fng=(1/m)*(FE1_b+FA_b); %total body frame translational acceleration without gravitational force
Ft=(AI')*Fng+Fg_b; %total body frame translational acceleration
x1to3dot=Ft; %derivatives vector of body frame translational acceleration
CR=5; %RCS engine thrust [N]
%
Ib=[jx 0 0; %moment of inertia matrix
0 jy 0;
0 0 jz];
A=Ib(1,1); %moment of inertia component
B=Ib(2,2); %moment of inertia component
C=Ib(3,3); %moment of inertia component
LA=0; %aerodnamic drag moment around oX axis
LT=CR*df*x16; %thrust moment around oX axis
l=1; %length of rocket
xT=xcm-l; %arm of thrust moment in body frame
xf=0.4; %centre of pressure position
YT=FE1_b(2); %thrust along oY axis
ZT=FE1_b(3); %thrust along oZ axis
YA=FA_b(2); %aerodynamic force along oY axis
ZA=FA_b(3); %aerodynamic force along oY axis
MA=-xf*ZA; %aerodynamic moment around oZ axis
MT=-xT*ZT; %thrust moment around oZ axis
NA=xf*YA; %aerodynamic moment around oY axis
NT=xT*YT; %thrust moment around oY axis
L=LA+LT; %total moment around oX axis in body frame
M=MA+MT; %total moment around oY axis in body frame
N=NA+NT; %total moment around oZ axis in body frame
p=x4; %angular velocity around oX axis
q=x5; %angular velocity around oY axis
r=x6; %angular velocity around oZ axis
pdot=(L+q*r*(B-C))/A; %angular velocity derivative around oX axis
qdot=(M+r*p*(C-A))/B; %angular velocity derivative around oY axis
rdot=(N+p*q*(A-B))/C; %angular velocity derivative around oZ axis
x4to6dot=[pdot;qdot;rdot];%vector of velocity derivatives around all axes
x7dd=p+q*sin(fi)*tan(ps)+r*cos(fi)*tan(ps); %angular acceleration around oX axis
x8dd=q*cos(fi)-r*sin(fi); %angular acceleration around oY axis
x9dd=q*sin(fi)*sec(ps)+r*cos(fi)*sec(ps); %angular acceleration around oZ axis
x7to9dot=[x7dd;x8dd;x9dd]; %vector of angular accelerations around all axes
x10to12dot=V_l; %vector of translational velocities in inertial frame
dx=[x1to3dot; %state vector derivatives
x4to6dot;
x7to9dot;
x10to12dot;
dm;
ut;
un;
um;
ur
];
end
%% === Drag model ===
function CD = dragcoefvar(Re)
% Coeficiaentul de rezistenta la inaintarea a sferei
% Relatii din lucrarea: Drag coeficient of flow around sphere: Matching asymptotically the wide trend
% Powder Technology 186 (2008) 218-223
% Author Jaber Almedeij - Kuwait
f1=(24.*Re^(-1))^(10)+(21.*Re^(-0.67))^(10)+(4.*Re^(-0.33))^(10)+0.4^(10);
f2=1./((0.148*Re^(0.11))^(-10)+0.5^(-10));
f3= (1.57d8*Re^(-1.625))^(10);
f4=1./((6.d-17*Re^(2.63))^(-10)+0.2^(-10));
CD=(1./((f1+f2)^(-1)+f3^(-1))+f4)^(0.1);
end
%% === meca ===
function [xcm,jx,jy,jz] = meca(m)
global md xcmd jxd jyd jzd
nd=length(md);
for i=1:nd
mid(i)=1./md(i);
end
ami=1./m;
xcm=interp1(mid,xcmd,ami,'pchip'); % centrul de masa
jx= interp1(mid,jxd,ami,'pchip'); % moment de inertie longitudinal
jy= interp1(mid,jyd,ami,'pchip'); % moment de inertie transversal
jz= interp1(mid,jzd,ami,'pchip'); % moment de inertie transversal
end
%% === bloc2 ===
function [y,d1,d2,d3,d4] = bloc2(x,xd,xa,xb)
% rutina de control a unui bloc de 4 elemente de comanda (in opozitie)
% x - semnal de intarare
% dx - semn derivata semnal intrare
% y - semnal de iesire
% d1 - semnal functionare element de comanda 1 ( 1 functioneaza 0 oprit)
% d2 - semnal functionare element de comanda 2 ( 1 functioneaza 0 oprit)
ymax=1.; % valoare maxima semnal de iesire
y=ymod(x,xd,xa,xb,ymax); % semnal de iesire
d1=0.;
d2=0.;
d3=0.;
d4=0.;
if(y == ymax)
d2=1.;
d4=1.;
elseif(y == -ymax)
d1=1.;
d3=1.;
end
end
%% === ymod ===
function y = ymod(x, xd, a, b, ymax)
% element neliniar Triger Schmidt
ab=a+b;
y=0.;
if(xd >0)
if(x <=-a)
y = -ymax;
elseif(x >= ab)
y = ymax;
end
elseif(xd <= 0)
if(x <= -ab)
y=-ymax;
elseif(x >= a)
y= ymax;
end
end
end
Risposte (1)
Hi @Ionel,
Your `FiniteDifferenceStepSize = 1e-6` is way too small for an ODE-based optimization. fmincon is testing gains like 0.000001 vs 0, which produces literally zero detectable difference in your 5-second simulation. It sees "gradient = 0" and quits immediately (that's why you got 19 function calls and stopped).
Quick fix:
'FiniteDifferenceStepSize', 0.1 % instead of 1e-6
**Better fix - use pattern search instead:
options = optimoptions('patternsearch', 'Display', 'iter', 'MaxIterations', 1000);
[opt_gains, ~] = patternsearch(@cost_function, x0, [], [], [], [], lb, ub, options);
Pattern search doesn't rely on gradients so it's much more robust for simulation-based problems like yours.
Also don't start at zero - try `x0 = [1,1,1,1,1,1,1,1,1]` instead.
The MathWorks docs specifically warn about this: " Simulations are often insensitive to small changes in parameters. If you use too small a perturbation, the simulation returns a spurious derivative of 0."
Questions to Consider
1.Does your simulation complete successfully with gains = [1,1,1,1,1,1,1,1,1]?
2. What cost value do you get with manual gain tuning?
3. Are you sure the desired states are achievable? (vx=0, vy=5 m/s, pitch=90 degrees, forward motion from x=0 to somewhere?)
I hope this helps! Let me know if you'd like me to clarify anything or if you encounter new issues after implementing these changes.
P.S. Your 6DOF model looks quite sophisticated with proper Mars atmosphere modeling (temperature, pressure, density), variable mass/inertia, aerodynamic drag with Reynolds number calculation, and RCS thruster control. Once the optimization is working, you should get good results!
Categorie
Scopri di più su Quadratic Programming and Cone Programming in Centro assistenza e File Exchange
Prodotti
il 24 Ott 2025
il 24 Ott 2025
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!
Translated by 
