cost_function.m

function trajectory_cost = cost_function(x, C_u, C_v, C_w, ...
                                         x_0, y_0, z_0, ...
                                         R, R_dot, R_dot_dot, R_int, ...
                                         x_d, y_d, z_d, ...
                                         u_d, v_d, w_d, ...
                                         psi_d, gamma_d, ...
                                         horizon_time, num_local_waypoints, ...
                                         R_obs, ...
                                         lambda_p, lambda_s, ...
                                         lambda_lim, lambda_at, ...
                                         t_array, t_diff, safe_distance, ...
                                         g, rho, e, S, b, mass, AR, k, C_D_0, ...
                                         T_max, T_min, gamma_max, gamma_min, ...
                                         phi_max, phi_min)
    %{
      Calculate trajectory cost (objective function).
    %}

    C_u_ = [C_u(1:3); x(1:4)];
    C_v_ = [C_v(1:3); x(5:8)];
    C_w_ = [C_w(1:3); x(9:12)];
    

	[x_a, y_a, z_a, u_a, v_a, w_a, ...
     u_a_dot, v_a_dot, w_a_dot, ...
     u_a_dot_dot, v_a_dot_dot, w_a_dot_dot, ...
     V_a, V_a_dot, ...
     gamma_a, psi_a, psi_a_dot, gamma_a_dot, ...
     psi_a_dot_dot, gamma_a_dot_dot, ...
     phi_a, n_a, C_L, C_D, D_a, T_a] = calculate_trajectory_states(x_0, y_0, z_0, C_u_, C_v_, C_w_, ...
                                                                   R_int, R, R_dot, R_dot_dot, ...
                                                                   t_array, t_diff, horizon_time, ...
                                                                   g, rho, S, C_D_0, k, mass);

    %% Calculate costs
    position_cost = sum((x_d - x_a).^2 + (y_d - y_a).^2 + (z_d - z_a).^2)/(num_local_waypoints*((max(R_obs) + safe_distance)^2));
    speed_cost = sum((u_d - u_a).^2 + (v_d - v_a).^2 + (w_d - w_a).^2)/(num_local_waypoints*(mean(u_d)^2 + mean(v_d)^2 + mean(w_d)^2));
    
                               
    vehicle_constraints_penalty = (max(0, T_a - T_max) + ...
                                   max(0, gamma_a(1:end-1) - gamma_max) + ...
                                   max(0, gamma_min - gamma_a(1:end-1)) + ...
                                   max(0, phi_a - phi_max) + ...
                                   max(0, phi_min - phi_a)).^2;
    
    vehicle_constraints_penalty = sum(vehicle_constraints_penalty);
    
    terminal_tracking_cost = ((psi_d(num_local_waypoints) - psi_a(num_local_waypoints))/pi)^2 + ((gamma_d(num_local_waypoints) - gamma_a(num_local_waypoints))/pi)^2;
    
    % Saturation of costs to avoid NaN and Inf results 
    % in the objective function
    position_cost = min(position_cost, 1e200);
    speed_cost = min(speed_cost, 1e200);
    vehicle_constraints_penalty = min(vehicle_constraints_penalty, 1e200);
    terminal_tracking_cost = min(terminal_tracking_cost, 1e200);
    
    
    trajectory_cost = lambda_p*position_cost ... 
                    + lambda_s*speed_cost ...
                    + lambda_lim*vehicle_constraints_penalty ...
                    + lambda_at*terminal_tracking_cost;