I need to minimize the fisher information determinant in order to get some optimized yaw rates the the AUV model to follow an optimal path.

The minimization function contains some non linear constraints and the fmincon function return the optimal yaw rate and optimal speed for a particular iteration. I don’t know how to define the nonlcon constraints function as it has to take certain parameters that are not returned by the fmincon function but are available from some other matlab file.
(https://i.stack.imgur.com/0y9uw.png)
(https://i.stack.imgur.com/iGf28.png)

This is the MATLAB code I’ve written

% Given parameters
function [optimized_yaw_rates, optimized_speed, max_objective_value] = target_optimize(x_k1, x0, yaw, x_tilde, x_nominal, distances, psi0, v0, m, sigma, target)
    % Adjust as needed
    %     m = length(x_tilde);  % Assuming u_rates is the vector of yaw rates obtained
    % Define equality constraint: p(k+1) = p(k) + sum(u(i))

    % Objective function (negative since fmincon minimizes)
    objective = @(uv) -detFIMCostFunction(uv(1), uv(2), sigma);
    rb = pi/9;

    % Constraints (if any, e.g., bounds on yaw rates)
    lb = [-rb, 0];  % Lower bounds for [u, v]
    ub = [rb, 1.5];  % Upper bounds for [u, v]

    % Initial guess (you can use the obtained yaw rates as the starting point)
    initial_guess = [psi0, v0];  % Initial guess for [u, v]
    % Initialize max_objective_value
    max_objective_value = -Inf;
    % Call fmincon
    options = optimoptions(@fmincon, ...
        'MaxFunctionEvaluations', 1e6, ...
        'MaxIterations', 1e6, ...
        'TolX', 1e-6, ...
        'TolFun', 1e-6, ...
        'Algorithm', 'interior-point', ...
        'StepTolerance', 1e-6, ...
        'ConstraintTolerance', 1e-6);

    [optimized_params, max_objective_value] = fmincon(objective, initial_guess, [], [], [],[], lb, ub, @nonlcon, options);
    optimized_yaw_rates = optimized_params(1);
    optimized_speed = optimized_params(2);
    disp('Optimized Yaw Rates:');
    disp(optimized_yaw_rates);
    disp('Maximum Objective Value:');
    disp(-max_objective_value);  % Note the negative sign here

    % Define the nonlinear constraint function for fmincon
    function [c, ceq] = nonlcon(x_k1,x0,yaw)
        p0 = x0(1:2);
        psi0 = x0(3);
        p_k1 = x_k1(1:2);
        psi_k1 = x_k1(3);
        ceq(1) =  p_k1 - p0 - 1.5*[cos(psi_k1); sin(psi_k1)];
        ceq(2) = psi_k1 - psi0 - sum(yaw);
        c = [];  % Compute nonlinear inequalities.
    end

    % Define the cost function for |FIMp|^2
    function cost = detFIMCostFunction(u, v, sigma)
        x_tilde_func = matlabFunction(x_tilde);
        distances_func = matlabFunction(distances);
        fim = [0, 0; 0, 0];
        P0 = [1, 0; 0, 1];
        x_tilde_numeric = x_tilde_func(u, v);
        distances_numeric = distances_func(u, v);
        for i = 1:m
            fim = fim + 1/(distances_numeric(i)^2) * [x_tilde_numeric(1, i)^2, x_tilde_numeric(1, i)*x_tilde_numeric(2, i); x_tilde_numeric(1, i)*x_tilde_numeric(2, i), x_tilde_numeric(2, i)^2];
        end
        cost = log((1/sigma^2)*det(fim + inv(P0)));
    end
end

I tried to write a nonlinear constraint function with parameters but got an error saying not enough input arguments when I put it in fmincon(..,..,.. nonlcon,..) function.