Make this optimization work

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()

 Iter  Func-count            Fval   Feasibility   Step Length       Norm of   First-order  
                                                                       step    optimality
    0          19    1.000000e+06     0.000e+00     1.000e+00     0.000e+00     0.000e+00  

Optimization completed: The final point is the initial point.
The first-order optimality measure, 0.000000e+00, is less than options.OptimalityTolerance =
1.000000e-06, and the maximum constraint violation, 0.000000e+00, is less than
options.ConstraintTolerance = 1.000000e-06.


=== Optimized Gains ===
k4 = 0.0000
 k5 = 0.0000
 k7 = 0.0000
 k8 = 0.0000
 k9 = 0.0000
 k1 = 0.0000
 k2 = 0.0000
 k6 = 0.0000
 k3 = 0.0000

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

Answer 1

[Umar]

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!