How can I define the expression for the cost function P and L for a MPC controller

Question

How to define the cost function based on the model variables and in the form of the quadratic cost function, which are similar as: J = J + 10 * ((x[0, k] - 10)**2 + (x[1, k] - 10)**2) + 10 * x[2, k]**2 + 10 * x[3, k]**2 J = J + 1 * (u[0, k]**2 + u[1, k]**2) The model variables are: x = ca.MX.sym('x', (Dim_state, N+1)) u = ca.MX.sym('u', (Dim_ctrl, N))

I tried to define the expression for P and L as: P = ca.diag([10, 10, 10, 0]) L = ca.diag([1, 1, 0, 1]) But the error shows that failed: Expected a dense 'f', but got 4x4,4nz. I think the issue comes from P and L

The requirement are as "## Cost function: C1: Track a desired longitudinal velocity.

C2: Regularize the lateral velocity and yaw rate.

C3: Encourage the car to stay at the same lane as the leading vehicle.

C4: Regularize the control inputs."

Answer 1

You can try using casadi library of python.

import casadi as ca

Dim_state = 4
Dim_ctrl = 2
N = 10 
x = ca.MX.sym('x', (Dim_state, N+1))
u = ca.MX.sym('u', (Dim_ctrl, N))

Q1 = 10  # Weight for tracking desired longitudinal velocity
Q2 = 10  # Weight for lateral velocity regularization
Q3 = 10  # Weight for yaw rate regularization
Q4 = 1   # Weight for control input regularization

desired_velocity = 10  # Example value

J = 0  
J += Q1 * (x[0, 0] - desired_velocity)**2
J += Q2 * (x[1, 0]**2 + x[2, 0]**2)
J += Q4 * ca.mtimes(u, u)
# Now, J represents the total cost function based on the specified weights and objectives.

# You can use J in your MPC optimization problem.