def objective_func(q, u, q_goal, Q, R, P):
    """
    Implements the objective function. q is an array of states and u is an array of inputs. Together,
    these two arrays contain all the optimization variables in our problem.
    In particular,
    q has shape (4, N+1), so that each column of q is an array q[:, i] = [q0, q1, q2, q3]
    (i.e. [x, y, theta, phi]), the state at time-step i.
    u has shape (2, N), so that each column of u is an array u[:, i] = [u1, u2], the two control inputs
    (velocity and steering) of the bicycle model.
    This function should create an expression of the form
    sum_{i = 1, ..., N} ((q(i) - q_goal)^T * Q * (q(i) - q_goal) + (u(i)^T * R * u(i)))
    + (q(N+1) - q_goal)^T * P * (q(N+1) - q_goal)
    Note: When dealing with casadi symbolic variables, you can use @ for matrix multiplication,
    and * for standard, numpy-style element-wise (or broadcasted) multiplication.

    """

    n = q.shape[1] - 1
    obj = 0
    for i in range(n):
        qi = q[:, i]
        ui = u[:, i]

        # Define one term of the summation here: ((q(i) - q_goal)^T * Q * (q(i) - q_goal) + (u(i)^T * R * u(i)))
        term = (qi - q_goal).T @ Q @ (qi - q_goal) + (ui.T @ R @ ui)
        obj += term

    q_last = q[:, n]
    # Define the last term here: (q(N+1) - q_goal)^T * P * (q(N+1) - q_goal)
    term_last = (q[:, n] - q_goal).T @ P @ (q[:, n] - q_goal)
    obj += term_last
    return obj