AcadosOcpSolver.set(): mismatching dimension for field "yref" for first node

User Questions
1

Hi :wave:

I want to solve a MHE problem using acados Python interface.
First I have studied the official example: mhe in pendulum on cart.
In the original Code, the measurement are equal to the states. Here y=x := (p,theta, v, dtheta)
ocp_mhe.cost.cost_type_0 == “NONLINEAR_LS”:
ocp_mhe.cost.W_0 = block_diag(R, Q, Q0)
ocp_mhe.model.cost_y_expr_0 = vertcat(x, u, x)
ocp_mhe.cost.yref_0 = np.zeros((ny_0,))

Then I have modified the example. Suppose only p and theta can be measured, means y = (p, theta)

The Code for mhe solver has been modified like this:

ocp_mhe = AcadosOcp()

ocp_mhe.model = model

x = ocp_mhe.model.x
u = ocp_mhe.model.u

nx = x.rows()
nu = u.rows()
nparam = model.p.rows()
nymeas = 2

ny_0 = nymeas + nu + nx
ny   = nymeas + nu                   # h(x), w
    
ymeas = x[0:nymeas]

# Initial cost
ocp_mhe.cost.cost_type_0 == "NONLINEAR_LS"
ocp_mhe.cost.W_0 = block_diag(R, Q, Q0)
ocp_mhe.model.cost_y_expr_0 = vertcat(ymeas, u, x)
ocp_mhe.cost.yref_0 = np.zeros((ny_0,))


# intermediate
ocp_mhe.cost.cost_type = 'NONLINEAR_LS'
ocp_mhe.cost.W = block_diag(R, Q)
ocp_mhe.model.cost_y_expr = vertcat(ymeas, u)
ocp_mhe.parameter_values = np.zeros((nparam, ))
ocp_mhe.cost.yref = np.zeros((ny,))

In the mhe open loop program tested, the modified part is like this:

model_mhe = export_mhe_ode_model()
nx = model_mhe.x.rows()
nw = model_mhe.u.rows()

ny = 2

v_stds = [0.1, 0.2]
…

get solution

for i in range(N):
simX[i,:] = acados_solver_ocp.get(i, “x”)
simU[i,:] = acados_solver_ocp.get(i, “u”)
simY[i,:] = simX[i,[0,1]] + np.transpose(np.diag(v_stds) @ np.random.standard_normal((ny, 1)))

simX[N,:] = acados_solver_ocp.get(N, “x”)
simY[N,:] = simX[N,[0,1]] + np.transpose(np.diag(v_stds) @ np.random.standard_normal((ny, 1)))

set measurements and controls

yref_0 = np.zeros((ny+nw+nx, ))
yref_0[:ny] = simY[0, :]
yref_0[ny+nw:] = x0_bar
acados_solver_mhe.set(0, “yref”, yref_0)

other Code, that not been given, are as same as original code.

The Problem:
When the program runs to the code “acados_solver_mhe.set(0, “yref”, yref_0)”, get an Exception.

Traceback (most recent call last):
File “/path/acados_MHE_exercise/modified_pendulum_on_cart/modified_mhe_mP_openloop.py”, line 85, in
acados_solver_mhe.set(0, “yref”, yref_0)
File “/path/interfaces/acados_template/acados_template/acados_ocp_solver.py”, line 1285, in set
raise Exception(msg)
Exception: AcadosOcpSolver.set(): mismatching dimension for field “yref” with dimension 6 (you have 10)

I have also tried, if only the v and dtheta can be measured. got the same issue

Can Someone help me to solve this?

Thanks a lot.

Lingbing

2

What are nx and nu in your case? Is ny_0 = nx + nu +nymeasured indeed equal to 6?

3

HI,

nx is the dimension of state, nu is the dimension of disturbance. for this case nx = 4, nu = 4.
ny_0 is indeed equal to 10.

for the test, I also give an array with dimension 10.

In the class AcadosOcpSolver, line 1279 and 1280:
dims = self._acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config,
self.nlp_dims, self.nlp_out, stage, field)

the dims got 6. next line:

       if value_.shape[0] != dims:
            msg = f'AcadosOcpSolver.set(): mismatching dimension for field "{field_}" '
            msg += f'with dimension {dims} (you have {value_.shape[0]})'
            raise Exception(msg)

Then I got the Exception.

4

This should be only a single =

Maybe this fixes your problem already . .

5

Hi Kaethe,

thanks a lot. Yes, the issue has been solved for this case.

However I still have a same issue for another case. I have implemented the mhe algorithm for a complex case: double pendulum on cart, which has 12 state variables. x = [p,theta1,theta2, v, dtheta1, dtheta2]

I have just used the same struct but just modified the model and corresponding solver.

for the case y=x
code from mhe_solver

ny_0 = nx + nw + nx         # h(x), w and arrival cost
ny   = nx + nw              # h(x), w

## set inital cost
dp_mhe.cost.cost_type_0 = 'NONLINEAR_LS'
dp_mhe.cost.W_0 = block_diag(R, Q, P)
dp_mhe.model.cost_y_expr_0 = vertcat(x, u, x)
dp_mhe.cost.yref_0 = np.zeros((ny_0,))

# intermediate
dp_mhe.cost.cost_type = 'NONLINEAR_LS'
dp_mhe.cost.W = block_diag(R, Q)
dp_mhe.model.cost_y_expr = vertcat(x, u)
dp_mhe.cost.yref = np.zeros((ny,))

here nx = nw = 6.

code from test file

model_mhe = export_double_pendulum_mhe_model()
nx = model_mhe.x.rows()
nw = model_mhe.u.rows()
ny = nx
.....
acados_solver_mhe = double_pendulum_mhe_solver(model_mhe, N, h, Q_mhe, P_mhe, R_mhe)
 .....
yref_0 = np.zeros((ny+nw+nx, ))  # set measurements and controls
yref_0[:ny] = simY[0, :]
yref_0[ny+nw:] = x0_bar
acados_solver_mhe.set(0, "yref", yref_0)

Same Exception arise, but dimension = 7 is odd.
Exception: AcadosOcpSolver.set(): mismatching dimension for field “yref” with dimension 7 (you have 18)

in the ocp_solver the dimension of output is ny=nx+nu = 7, due to only one input nu=1.

6

could you check the generated .json file for the fields ny_0, W_0, yref_0 and whether they have the expected dimension?

7

I have checked.

in the mhe.json file
“dims”: { … , “nu”: 6, “nx”: 6, “nx_next”: 6, “ny”: 12, “ny_0”: 18,…}
dim(yref_0) = 18, dim(yref)=12, dim(W_0) = 18 * 18, dim(W)= 12*12
I think they are all right.

8

Do you create multiple solvers in the same script? If yes, please ensure that they have different model names.

9

yes, different names is for sure. There are also the different generated .json file for ocp and mhe respectively. The dimensions in every .json file are correct.

10

Could you provide a working example for me to reproduce the error?

11

Certainly, I will provide all the Code. My co-worker helps me to check the code, yet still find no error.
Thanks in advance.
besides: There are some other questions about the modified single pendulum on cart I want to consult.
Do I need create a new topic or do we here discuss later?

  1. code of mpc_model

double pendulum on cart model

from acados_template import AcadosModel
from casadi import SX, vertcat, sin, cos

def export_double_pendulum_ocp_model() -> AcadosModel:

  model_name = 'double_pendulum_on_cart'

  # constants
  g = 9.81  # gravitational acceleration [m/s^2]

  # Pendulum parameters
  M = 2.0                  # Mass of cart
  L1, L2 = 0.4, 0.6        # lengths of pendulum [m]
  l1, l2 = 0.2, 0.3        # distances to CoM [m]
  m1, m2 = 1.0, 1.25       # masses of pendulum [kg]
  I1, I2 = 0.0014, 0.0375  # moments of inertia [N·m·s^2]

  # --- States ---
  theta1  = SX.sym('theta1')     # angle of first pendulum [rad]
  theta2  = SX.sym('theta2')     # angle of second pendulum [rad]
  dtheta1 = SX.sym('dtheta1')    # angular velocity of first pendulum [rad/s]
  dtheta2 = SX.sym('dtheta2')    # angular velocity of second pendulum [rad/s]
  cart_x  = SX.sym('cart_x')     # cart position [m]
  cart_v  = SX.sym('cart_v')     # cart velocity [m/s]

  # Define state vector
  x = vertcat(cart_x, theta1, theta2, cart_v, dtheta1, dtheta2)


  # --- Controls ---
  F  = SX.sym('F')  # cart acceleration (control input) [m/s^2]
  u = vertcat(F)


  # xdot
  cart_x_dot  = SX.sym('cart_x_dot')
  cart_v_dot  = SX.sym('cart_v_dot')
  theta1_dot  = SX.sym('theta1_dot')
  dtheta1_dot = SX.sym('dtheta1_dot')
  theta2_dot  = SX.sym('theta2_dot')
  dtheta2_dot = SX.sym('dtheta2_dot')

  x_dot = vertcat(cart_x_dot, theta1_dot, theta2_dot, cart_v_dot, dtheta1_dot, dtheta2_dot)


  # parameters
  p = []

  # dynamics equations
  dd_x = (F*I1*I2 + F*I1*l2**2*m2 + F*I2*L1**2*m2 + F*I2*l1**2*m1 - F*L1**2*l2**2*m2**2*cos(theta1 - theta2)**2 + F*L1**2*l2**2*m2**2 + F*l1**2*l2**2*m1*m2 + I1*I2*L1*dtheta1**2*m2*sin(theta1) + I1*I2*dtheta1**2*l1*m1*sin(theta1) + I1*I2*dtheta2**2*l2*m2*sin(theta2) + I1*L1*dtheta1**2*l2**2*m2**2*sin(theta1) - I1*L1*dtheta1**2*l2**2*m2**2*sin(theta1 - theta2)*cos(theta2) + I1*dtheta1**2*l1*l2**2*m1*m2*sin(theta1) + I1*dtheta2**2*l2**3*m2**2*sin(theta2) - I1*g*l2**2*m2**2*sin(2*theta2)/2 + I2*L1**3*dtheta1**2*m2**2*sin(theta1) + I2*L1**2*dtheta1**2*l1*m1*m2*sin(theta1) + I2*L1**2*dtheta2**2*l2*m2**2*sin(theta2) + I2*L1**2*dtheta2**2*l2*m2**2*sin(theta1 - theta2)*cos(theta1) - I2*L1**2*g*m2**2*sin(2*theta1)/2 + I2*L1*dtheta1**2*l1**2*m1*m2*sin(theta1) + I2*L1*dtheta2**2*l1*l2*m1*m2*sin(theta1 - theta2)*cos(theta1) - I2*L1*g*l1*m1*m2*sin(2*theta1) + I2*dtheta1**2*l1**3*m1**2*sin(theta1) + I2*dtheta2**2*l1**2*l2*m1*m2*sin(theta2) - I2*g*l1**2*m1**2*sin(2*theta1)/2 + L1**3*dtheta1**2*l2**2*m2**3*(sin(theta1 - 2*theta2) + sin(3*theta1 - 2*theta2))/4 - L1**3*dtheta1**2*l2**2*m2**3*sin(theta1)*cos(theta1 - theta2)**2 + L1**3*dtheta1**2*l2**2*m2**3*sin(theta1) - L1**3*dtheta1**2*l2**2*m2**3*sin(theta1 - theta2)*cos(theta2) + L1**2*dtheta1**2*l1*l2**2*m1*m2**2*(sin(theta1 - 2*theta2) + sin(3*theta1 - 2*theta2))/4 - L1**2*dtheta1**2*l1*l2**2*m1*m2**2*sin(theta1)*cos(theta1 - theta2)**2 + L1**2*dtheta1**2*l1*l2**2*m1*m2**2*sin(theta1) - L1**2*dtheta2**2*l2**3*m2**3*(sin(2*theta1 - 3*theta2) + sin(2*theta1 - theta2))/4 - L1**2*dtheta2**2*l2**3*m2**3*sin(theta2)*cos(theta1 - theta2)**2 + L1**2*dtheta2**2*l2**3*m2**3*sin(theta2) + L1**2*dtheta2**2*l2**3*m2**3*sin(theta1 - theta2)*cos(theta1) + L1**2*g*l2**2*m2**3*sin(theta1)*cos(theta2)*cos(theta1 - theta2) - L1**2*g*l2**2*m2**3*sin(2*theta1)/2 + L1**2*g*l2**2*m2**3*sin(theta2)*cos(theta1)*cos(theta1 - theta2) - L1**2*g*l2**2*m2**3*sin(2*theta2)/2 + L1*dtheta1**2*l1**2*l2**2*m1*m2**2*sin(theta1) - L1*dtheta1**2*l1**2*l2**2*m1*m2**2*sin(theta1 - theta2)*cos(theta2) + L1*dtheta2**2*l1*l2**3*m1*m2**2*sin(theta1 - theta2)*cos(theta1) + L1*g*l1*l2**2*m1*m2**2*sin(theta1)*cos(theta2)*cos(theta1 - theta2) - L1*g*l1*l2**2*m1*m2**2*sin(2*theta1) + L1*g*l1*l2**2*m1*m2**2*sin(theta2)*cos(theta1)*cos(theta1 - theta2) + dtheta1**2*l1**3*l2**2*m1**2*m2*sin(theta1) + dtheta2**2*l1**2*l2**3*m1*m2**2*sin(theta2) - g*l1**2*l2**2*m1**2*m2*sin(2*theta1)/2 - g*l1**2*l2**2*m1*m2**2*sin(2*theta2)/2)/(I1*I2*M + I1*I2*m1 + I1*I2*m2 + I1*M*l2**2*m2 + I1*l2**2*m1*m2 - I1*l2**2*m2**2*cos(theta2)**2 + I1*l2**2*m2**2 + I2*L1**2*M*m2 + I2*L1**2*m1*m2 - I2*L1**2*m2**2*cos(theta1)**2 + I2*L1**2*m2**2 - 2*I2*L1*l1*m1*m2*cos(theta1)**2 + I2*M*l1**2*m1 - I2*l1**2*m1**2*cos(theta1)**2 + I2*l1**2*m1**2 + I2*l1**2*m1*m2 - L1**2*M*l2**2*m2**2*cos(theta1 - theta2)**2 + L1**2*M*l2**2*m2**2 - L1**2*l2**2*m1*m2**2*cos(theta1 - theta2)**2 + L1**2*l2**2*m1*m2**2 - L1**2*l2**2*m2**3*cos(theta1)**2 + 2*L1**2*l2**2*m2**3*cos(theta1)*cos(theta2)*cos(theta1 - theta2) - L1**2*l2**2*m2**3*cos(theta2)**2 - L1**2*l2**2*m2**3*cos(theta1 - theta2)**2 + L1**2*l2**2*m2**3 - 2*L1*l1*l2**2*m1*m2**2*cos(theta1)**2 + 2*L1*l1*l2**2*m1*m2**2*cos(theta1)*cos(theta2)*cos(theta1 - theta2) + M*l1**2*l2**2*m1*m2 - l1**2*l2**2*m1**2*m2*cos(theta1)**2 + l1**2*l2**2*m1**2*m2 - l1**2*l2**2*m1*m2**2*cos(theta2)**2 + l1**2*l2**2*m1*m2**2)
  dd_theta1 = (-2*F*I2*L1*m2*cos(theta1) - 2*F*I2*l1*m1*cos(theta1) - F*L1*l2**2*m2**2*cos(theta1) + F*L1*l2**2*m2**2*cos(theta1 - 2*theta2) - 2*F*l1*l2**2*m1*m2*cos(theta1) - I2*L1**2*dtheta1**2*m2**2*sin(2*theta1) - 2*I2*L1*M*dtheta2**2*l2*m2*sin(theta1 - theta2) + 2*I2*L1*M*g*m2*sin(theta1) - 2*I2*L1*dtheta1**2*l1*m1*m2*sin(2*theta1) - 2*I2*L1*dtheta2**2*l2*m1*m2*sin(theta1 - theta2) - I2*L1*dtheta2**2*l2*m2**2*sin(theta1 - theta2) - I2*L1*dtheta2**2*l2*m2**2*sin(theta1 + theta2) + 2*I2*L1*g*m1*m2*sin(theta1) + 2*I2*L1*g*m2**2*sin(theta1) + 2*I2*M*g*l1*m1*sin(theta1) - I2*dtheta1**2*l1**2*m1**2*sin(2*theta1) + I2*dtheta2**2*l1*l2*m1*m2*sin(theta1 - theta2) - I2*dtheta2**2*l1*l2*m1*m2*sin(theta1 + theta2) + 2*I2*g*l1*m1**2*sin(theta1) + 2*I2*g*l1*m1*m2*sin(theta1) - L1**2*M*dtheta1**2*l2**2*m2**2*sin(2*theta1 - 2*theta2) - L1**2*dtheta1**2*l2**2*m1*m2**2*sin(2*theta1 - 2*theta2) - 2*L1*M*dtheta2**2*l2**3*m2**2*sin(theta1 - theta2) + L1*M*g*l2**2*m2**2*sin(theta1) + L1*M*g*l2**2*m2**2*sin(theta1 - 2*theta2) - L1*dtheta1**2*l1*l2**2*m1*m2**2*sin(2*theta1) + L1*dtheta1**2*l1*l2**2*m1*m2**2*sin(2*theta1 - 2*theta2) - 2*L1*dtheta2**2*l2**3*m1*m2**2*sin(theta1 - theta2) + L1*g*l2**2*m1*m2**2*sin(theta1) + L1*g*l2**2*m1*m2**2*sin(theta1 - 2*theta2) + 2*M*g*l1*l2**2*m1*m2*sin(theta1) - dtheta1**2*l1**2*l2**2*m1**2*m2*sin(2*theta1) + dtheta2**2*l1*l2**3*m1*m2**2*sin(theta1 - theta2) - dtheta2**2*l1*l2**3*m1*m2**2*sin(theta1 + theta2) + 2*g*l1*l2**2*m1**2*m2*sin(theta1) + g*l1*l2**2*m1*m2**2*sin(theta1) - g*l1*l2**2*m1*m2**2*sin(theta1 - 2*theta2))/(2*I1*I2*M + 2*I1*I2*m1 + 2*I1*I2*m2 + 2*I1*M*l2**2*m2 + 2*I1*l2**2*m1*m2 - I1*l2**2*m2**2*cos(2*theta2) + I1*l2**2*m2**2 + 2*I2*L1**2*M*m2 + 2*I2*L1**2*m1*m2 - I2*L1**2*m2**2*cos(2*theta1) + I2*L1**2*m2**2 - 2*I2*L1*l1*m1*m2*cos(2*theta1) - 2*I2*L1*l1*m1*m2 + 2*I2*M*l1**2*m1 - I2*l1**2*m1**2*cos(2*theta1) + I2*l1**2*m1**2 + 2*I2*l1**2*m1*m2 - L1**2*M*l2**2*m2**2*cos(2*theta1 - 2*theta2) + L1**2*M*l2**2*m2**2 - L1**2*l2**2*m1*m2**2*cos(2*theta1 - 2*theta2) + L1**2*l2**2*m1*m2**2 - L1*l1*l2**2*m1*m2**2*cos(2*theta1) + L1*l1*l2**2*m1*m2**2*cos(2*theta2) + L1*l1*l2**2*m1*m2**2*cos(2*theta1 - 2*theta2) - L1*l1*l2**2*m1*m2**2 + 2*M*l1**2*l2**2*m1*m2 - l1**2*l2**2*m1**2*m2*cos(2*theta1) + l1**2*l2**2*m1**2*m2 - l1**2*l2**2*m1*m2**2*cos(2*theta2) + l1**2*l2**2*m1*m2**2)
  dd_theta2 = l2*m2*(-F*I1*cos(theta2) - F*L1**2*m2*cos(theta2)/2 + F*L1**2*m2*cos(2*theta1 - theta2)/2 + F*L1*l1*m1*cos(theta2)/2 + F*L1*l1*m1*cos(2*theta1 - theta2)/2 - F*l1**2*m1*cos(theta2) + I1*L1*M*dtheta1**2*sin(theta1 - theta2) + I1*L1*dtheta1**2*m1*sin(theta1 - theta2) + I1*L1*dtheta1**2*m2*sin(theta1 - theta2)/2 - I1*L1*dtheta1**2*m2*sin(theta1 + theta2)/2 + I1*M*g*sin(theta2) - I1*dtheta1**2*l1*m1*sin(theta1 - theta2)/2 - I1*dtheta1**2*l1*m1*sin(theta1 + theta2)/2 - I1*dtheta2**2*l2*m2*sin(2*theta2)/2 + I1*g*m1*sin(theta2) + I1*g*m2*sin(theta2) + L1**3*M*dtheta1**2*m2*sin(theta1 - theta2) + L1**3*dtheta1**2*m1*m2*sin(theta1 - theta2) + L1**2*M*dtheta2**2*l2*m2*sin(2*theta1 - 2*theta2)/2 + L1**2*M*g*m2*sin(theta2)/2 - L1**2*M*g*m2*sin(2*theta1 - theta2)/2 - 3*L1**2*dtheta1**2*l1*m1*m2*sin(theta1 - theta2)/2 + L1**2*dtheta1**2*l1*m1*m2*sin(theta1 + theta2)/2 + L1**2*dtheta2**2*l2*m1*m2*sin(2*theta1 - 2*theta2)/2 + L1**2*g*m1*m2*sin(theta2)/2 - L1**2*g*m1*m2*sin(2*theta1 - theta2)/2 + L1*M*dtheta1**2*l1**2*m1*sin(theta1 - theta2) - L1*M*g*l1*m1*sin(theta2)/2 - L1*M*g*l1*m1*sin(2*theta1 - theta2)/2 + L1*dtheta1**2*l1**2*m1**2*sin(theta1 - theta2)/2 + L1*dtheta1**2*l1**2*m1**2*sin(theta1 + theta2)/2 + L1*dtheta1**2*l1**2*m1*m2*sin(theta1 - theta2)/2 - L1*dtheta1**2*l1**2*m1*m2*sin(theta1 + theta2)/2 + L1*dtheta2**2*l1*l2*m1*m2*sin(2*theta2)/2 - L1*dtheta2**2*l1*l2*m1*m2*sin(2*theta1 - 2*theta2)/2 - L1*g*l1*m1**2*sin(theta2)/2 - L1*g*l1*m1**2*sin(2*theta1 - theta2)/2 - 3*L1*g*l1*m1*m2*sin(theta2)/2 + L1*g*l1*m1*m2*sin(2*theta1 - theta2)/2 + M*g*l1**2*m1*sin(theta2) - dtheta1**2*l1**3*m1**2*sin(theta1 - theta2)/2 - dtheta1**2*l1**3*m1**2*sin(theta1 + theta2)/2 - dtheta2**2*l1**2*l2*m1*m2*sin(2*theta2)/2 + g*l1**2*m1**2*sin(theta2)/2 + g*l1**2*m1**2*sin(2*theta1 - theta2)/2 + g*l1**2*m1*m2*sin(theta2))/(I1*I2*M + I1*I2*m1 + I1*I2*m2 + I1*M*l2**2*m2 + I1*l2**2*m1*m2 - I1*l2**2*m2**2*cos(theta2)**2 + I1*l2**2*m2**2 + I2*L1**2*M*m2 + I2*L1**2*m1*m2 - I2*L1**2*m2**2*cos(theta1)**2 + I2*L1**2*m2**2 - 2*I2*L1*l1*m1*m2*cos(theta1)**2 + I2*M*l1**2*m1 - I2*l1**2*m1**2*cos(theta1)**2 + I2*l1**2*m1**2 + I2*l1**2*m1*m2 - L1**2*M*l2**2*m2**2*cos(theta1 - theta2)**2 + L1**2*M*l2**2*m2**2 - L1**2*l2**2*m1*m2**2*cos(theta1 - theta2)**2 + L1**2*l2**2*m1*m2**2 - L1**2*l2**2*m2**3*cos(theta1)**2 + 2*L1**2*l2**2*m2**3*cos(theta1)*cos(theta2)*cos(theta1 - theta2) - L1**2*l2**2*m2**3*cos(theta2)**2 - L1**2*l2**2*m2**3*cos(theta1 - theta2)**2 + L1**2*l2**2*m2**3 - 2*L1*l1*l2**2*m1*m2**2*cos(theta1)**2 + 2*L1*l1*l2**2*m1*m2**2*cos(theta1)*cos(theta2)*cos(theta1 - theta2) + M*l1**2*l2**2*m1*m2 - l1**2*l2**2*m1**2*m2*cos(theta1)**2 + l1**2*l2**2*m1**2*m2 - l1**2*l2**2*m1*m2**2*cos(theta2)**2 + l1**2*l2**2*m1*m2**2)

  f_expl = vertcat(cart_v,
                   dtheta1,
                   dtheta2,
                   dd_x,
                   dd_theta1,
                   dd_theta2
                   )
  
  f_impl = x_dot - f_expl
  

  model = AcadosModel()

  model.f_expl_expr = f_expl
  model.f_impl_expr = f_impl
  model.x = x
  model.xdot = x_dot
  model.u = u
  model.p = p
  model.name = model_name

return model
  1. Code of mpc solver
import numpy as np
import scipy
from acados_template import AcadosOcpSolver, AcadosOcp
from casadi import vertcat

def double_pendulum_ocp_solver(model, N, h, Q, R, umax,use_cython=False):

  dp_ocp = AcadosOcp()

  # set model
  dp_ocp.model = model

  # get dimension of variables
  Tf = N*h
  nx = model.x.rows()
  nu = model.u.rows()
  ny = nx + nu
  ny_e = nx

  dp_ocp.solver_options.N_horizon = N

  # set cost
  dp_ocp.cost.cost_type = 'NONLINEAR_LS'
  dp_ocp.cost.cost_type_e = 'NONLINEAR_LS'

  dp_ocp.cost.W = scipy.linalg.block_diag(R, Q)
  dp_ocp.cost.W_e = Q

  x = dp_ocp.model.x
  u = dp_ocp.model.u

  dp_ocp.model.cost_y_expr = vertcat(u, x)
  dp_ocp.model.cost_y_expr_e = x

  dp_ocp.cost.yref = np.zeros((ny,))
  dp_ocp.cost.yref_e = np.zeros((ny_e,))

  # set constraints
  dp_ocp.constraints.idxbu = np.array([0])
  dp_ocp.constraints.lbu = np.array([-umax])
  dp_ocp.constraints.ubu = np.array([umax])

  dp_ocp.constraints.x0 = np.array([0.0, -np.pi, -np.pi, 0.0, 0.0, 0.0])


  # set QP solver
  # dp_ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
  dp_ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
  dp_ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
  dp_ocp.solver_options.integrator_type = 'ERK'

  # set prediction horizon
  dp_ocp.solver_options.tf = Tf
  # dp_ocp.solver_options.nlp_solver_type = 'SQP'
  dp_ocp.solver_options.nlp_solver_type = 'SQP_RTI'
  dp_ocp.solver_options.nlp_solver_max_iter = 200


  if use_cython:
      AcadosOcpSolver.generate(dp_ocp, json_file='acados_dp_ocp.json')
      AcadosOcpSolver.build(dp_ocp.code_export_directory, with_cython=True)
      acados_solver = AcadosOcpSolver.create_cython_solver('acados_dp_ocp.json')
  else:
      acados_solver = AcadosOcpSolver(dp_ocp, json_file='acados_dp_ocp.json')

return acados_solver
  1. Code of mhe model

double pendulum on cart model

from acados_template import AcadosModel
from casadi import SX, vertcat, sin, cos

def export_double_pendulum_mhe_model() -> AcadosModel:

  model_name = 'double_pendulum_on_cart'

  # constants
  g = 9.81  # gravitational acceleration [m/s^2]

  # Pendulum parameters
  M = 2.0                  # Mass of cart
  L1, L2 = 0.4, 0.6        # lengths of pendulum [m]
  l1, l2 = 0.2, 0.3        # distances to CoM [m]
  m1, m2 = 1.0, 1.25       # masses of pendulum [kg]
  I1, I2 = 0.0014, 0.0375  # moments of inertia [N·m·s^2]
  #d1, d2 = 0.005, 0.005       # damping coefficients [N·m·s]

  # --- States ---
  theta1  = SX.sym('theta1')     # angle of first pendulum [rad]
  theta2  = SX.sym('theta2')     # angle of second pendulum [rad]
  dtheta1 = SX.sym('dtheta1')    # angular velocity of first pendulum [rad/s]
  dtheta2 = SX.sym('dtheta2')    # angular velocity of second pendulum [rad/s]
  cart_x  = SX.sym('cart_x')     # cart position [m]
  cart_v  = SX.sym('cart_v')     # cart velocity [m/s]

  # Define state vector
  x = vertcat(cart_x, theta1, theta2, cart_v, dtheta1, dtheta2)

  # state noise
  w_theta1  = SX.sym('w_theta1')
  w_theta2  = SX.sym('w_theta2')
  w_dtheta1 = SX.sym('w_dtheta1')
  w_dtheta2 = SX.sym('w_dtheta2')
  w_cart_x  = SX.sym('w_cart_x')
  w_cart_v  = SX.sym('w_cart_v')

  w = vertcat(w_cart_x, w_theta1, w_theta2, w_cart_v, w_dtheta1, w_dtheta2)

  # xdot
  cart_x_dot  = SX.sym('cart_x_dot')
  cart_v_dot  = SX.sym('cart_v_dot')
  theta1_dot  = SX.sym('theta1_dot')
  dtheta1_dot = SX.sym('dtheta1_dot')
  theta2_dot  = SX.sym('theta2_dot')
  dtheta2_dot = SX.sym('dtheta2_dot')

  x_dot = vertcat(cart_x_dot, theta1_dot, theta2_dot, cart_v_dot, dtheta1_dot, dtheta2_dot)


  # algebraic variables
  z = []

  # controls as parameters
  F  = SX.sym('F')  # cart acceleration (control input) [m/s^2]
  p = vertcat(F)


  # dynamics equations
  dd_x = (F*I1*I2 + F*I1*l2**2*m2 + F*I2*L1**2*m2 + F*I2*l1**2*m1 - F*L1**2*l2**2*m2**2*cos(theta1 - theta2)**2 + F*L1**2*l2**2*m2**2 + F*l1**2*l2**2*m1*m2 + I1*I2*L1*dtheta1**2*m2*sin(theta1) + I1*I2*dtheta1**2*l1*m1*sin(theta1) + I1*I2*dtheta2**2*l2*m2*sin(theta2) + I1*L1*dtheta1**2*l2**2*m2**2*sin(theta1) - I1*L1*dtheta1**2*l2**2*m2**2*sin(theta1 - theta2)*cos(theta2) + I1*dtheta1**2*l1*l2**2*m1*m2*sin(theta1) + I1*dtheta2**2*l2**3*m2**2*sin(theta2) - I1*g*l2**2*m2**2*sin(2*theta2)/2 + I2*L1**3*dtheta1**2*m2**2*sin(theta1) + I2*L1**2*dtheta1**2*l1*m1*m2*sin(theta1) + I2*L1**2*dtheta2**2*l2*m2**2*sin(theta2) + I2*L1**2*dtheta2**2*l2*m2**2*sin(theta1 - theta2)*cos(theta1) - I2*L1**2*g*m2**2*sin(2*theta1)/2 + I2*L1*dtheta1**2*l1**2*m1*m2*sin(theta1) + I2*L1*dtheta2**2*l1*l2*m1*m2*sin(theta1 - theta2)*cos(theta1) - I2*L1*g*l1*m1*m2*sin(2*theta1) + I2*dtheta1**2*l1**3*m1**2*sin(theta1) + I2*dtheta2**2*l1**2*l2*m1*m2*sin(theta2) - I2*g*l1**2*m1**2*sin(2*theta1)/2 + L1**3*dtheta1**2*l2**2*m2**3*(sin(theta1 - 2*theta2) + sin(3*theta1 - 2*theta2))/4 - L1**3*dtheta1**2*l2**2*m2**3*sin(theta1)*cos(theta1 - theta2)**2 + L1**3*dtheta1**2*l2**2*m2**3*sin(theta1) - L1**3*dtheta1**2*l2**2*m2**3*sin(theta1 - theta2)*cos(theta2) + L1**2*dtheta1**2*l1*l2**2*m1*m2**2*(sin(theta1 - 2*theta2) + sin(3*theta1 - 2*theta2))/4 - L1**2*dtheta1**2*l1*l2**2*m1*m2**2*sin(theta1)*cos(theta1 - theta2)**2 + L1**2*dtheta1**2*l1*l2**2*m1*m2**2*sin(theta1) - L1**2*dtheta2**2*l2**3*m2**3*(sin(2*theta1 - 3*theta2) + sin(2*theta1 - theta2))/4 - L1**2*dtheta2**2*l2**3*m2**3*sin(theta2)*cos(theta1 - theta2)**2 + L1**2*dtheta2**2*l2**3*m2**3*sin(theta2) + L1**2*dtheta2**2*l2**3*m2**3*sin(theta1 - theta2)*cos(theta1) + L1**2*g*l2**2*m2**3*sin(theta1)*cos(theta2)*cos(theta1 - theta2) - L1**2*g*l2**2*m2**3*sin(2*theta1)/2 + L1**2*g*l2**2*m2**3*sin(theta2)*cos(theta1)*cos(theta1 - theta2) - L1**2*g*l2**2*m2**3*sin(2*theta2)/2 + L1*dtheta1**2*l1**2*l2**2*m1*m2**2*sin(theta1) - L1*dtheta1**2*l1**2*l2**2*m1*m2**2*sin(theta1 - theta2)*cos(theta2) + L1*dtheta2**2*l1*l2**3*m1*m2**2*sin(theta1 - theta2)*cos(theta1) + L1*g*l1*l2**2*m1*m2**2*sin(theta1)*cos(theta2)*cos(theta1 - theta2) - L1*g*l1*l2**2*m1*m2**2*sin(2*theta1) + L1*g*l1*l2**2*m1*m2**2*sin(theta2)*cos(theta1)*cos(theta1 - theta2) + dtheta1**2*l1**3*l2**2*m1**2*m2*sin(theta1) + dtheta2**2*l1**2*l2**3*m1*m2**2*sin(theta2) - g*l1**2*l2**2*m1**2*m2*sin(2*theta1)/2 - g*l1**2*l2**2*m1*m2**2*sin(2*theta2)/2)/(I1*I2*M + I1*I2*m1 + I1*I2*m2 + I1*M*l2**2*m2 + I1*l2**2*m1*m2 - I1*l2**2*m2**2*cos(theta2)**2 + I1*l2**2*m2**2 + I2*L1**2*M*m2 + I2*L1**2*m1*m2 - I2*L1**2*m2**2*cos(theta1)**2 + I2*L1**2*m2**2 - 2*I2*L1*l1*m1*m2*cos(theta1)**2 + I2*M*l1**2*m1 - I2*l1**2*m1**2*cos(theta1)**2 + I2*l1**2*m1**2 + I2*l1**2*m1*m2 - L1**2*M*l2**2*m2**2*cos(theta1 - theta2)**2 + L1**2*M*l2**2*m2**2 - L1**2*l2**2*m1*m2**2*cos(theta1 - theta2)**2 + L1**2*l2**2*m1*m2**2 - L1**2*l2**2*m2**3*cos(theta1)**2 + 2*L1**2*l2**2*m2**3*cos(theta1)*cos(theta2)*cos(theta1 - theta2) - L1**2*l2**2*m2**3*cos(theta2)**2 - L1**2*l2**2*m2**3*cos(theta1 - theta2)**2 + L1**2*l2**2*m2**3 - 2*L1*l1*l2**2*m1*m2**2*cos(theta1)**2 + 2*L1*l1*l2**2*m1*m2**2*cos(theta1)*cos(theta2)*cos(theta1 - theta2) + M*l1**2*l2**2*m1*m2 - l1**2*l2**2*m1**2*m2*cos(theta1)**2 + l1**2*l2**2*m1**2*m2 - l1**2*l2**2*m1*m2**2*cos(theta2)**2 + l1**2*l2**2*m1*m2**2)
  dd_theta1 = (-2*F*I2*L1*m2*cos(theta1) - 2*F*I2*l1*m1*cos(theta1) - F*L1*l2**2*m2**2*cos(theta1) + F*L1*l2**2*m2**2*cos(theta1 - 2*theta2) - 2*F*l1*l2**2*m1*m2*cos(theta1) - I2*L1**2*dtheta1**2*m2**2*sin(2*theta1) - 2*I2*L1*M*dtheta2**2*l2*m2*sin(theta1 - theta2) + 2*I2*L1*M*g*m2*sin(theta1) - 2*I2*L1*dtheta1**2*l1*m1*m2*sin(2*theta1) - 2*I2*L1*dtheta2**2*l2*m1*m2*sin(theta1 - theta2) - I2*L1*dtheta2**2*l2*m2**2*sin(theta1 - theta2) - I2*L1*dtheta2**2*l2*m2**2*sin(theta1 + theta2) + 2*I2*L1*g*m1*m2*sin(theta1) + 2*I2*L1*g*m2**2*sin(theta1) + 2*I2*M*g*l1*m1*sin(theta1) - I2*dtheta1**2*l1**2*m1**2*sin(2*theta1) + I2*dtheta2**2*l1*l2*m1*m2*sin(theta1 - theta2) - I2*dtheta2**2*l1*l2*m1*m2*sin(theta1 + theta2) + 2*I2*g*l1*m1**2*sin(theta1) + 2*I2*g*l1*m1*m2*sin(theta1) - L1**2*M*dtheta1**2*l2**2*m2**2*sin(2*theta1 - 2*theta2) - L1**2*dtheta1**2*l2**2*m1*m2**2*sin(2*theta1 - 2*theta2) - 2*L1*M*dtheta2**2*l2**3*m2**2*sin(theta1 - theta2) + L1*M*g*l2**2*m2**2*sin(theta1) + L1*M*g*l2**2*m2**2*sin(theta1 - 2*theta2) - L1*dtheta1**2*l1*l2**2*m1*m2**2*sin(2*theta1) + L1*dtheta1**2*l1*l2**2*m1*m2**2*sin(2*theta1 - 2*theta2) - 2*L1*dtheta2**2*l2**3*m1*m2**2*sin(theta1 - theta2) + L1*g*l2**2*m1*m2**2*sin(theta1) + L1*g*l2**2*m1*m2**2*sin(theta1 - 2*theta2) + 2*M*g*l1*l2**2*m1*m2*sin(theta1) - dtheta1**2*l1**2*l2**2*m1**2*m2*sin(2*theta1) + dtheta2**2*l1*l2**3*m1*m2**2*sin(theta1 - theta2) - dtheta2**2*l1*l2**3*m1*m2**2*sin(theta1 + theta2) + 2*g*l1*l2**2*m1**2*m2*sin(theta1) + g*l1*l2**2*m1*m2**2*sin(theta1) - g*l1*l2**2*m1*m2**2*sin(theta1 - 2*theta2))/(2*I1*I2*M + 2*I1*I2*m1 + 2*I1*I2*m2 + 2*I1*M*l2**2*m2 + 2*I1*l2**2*m1*m2 - I1*l2**2*m2**2*cos(2*theta2) + I1*l2**2*m2**2 + 2*I2*L1**2*M*m2 + 2*I2*L1**2*m1*m2 - I2*L1**2*m2**2*cos(2*theta1) + I2*L1**2*m2**2 - 2*I2*L1*l1*m1*m2*cos(2*theta1) - 2*I2*L1*l1*m1*m2 + 2*I2*M*l1**2*m1 - I2*l1**2*m1**2*cos(2*theta1) + I2*l1**2*m1**2 + 2*I2*l1**2*m1*m2 - L1**2*M*l2**2*m2**2*cos(2*theta1 - 2*theta2) + L1**2*M*l2**2*m2**2 - L1**2*l2**2*m1*m2**2*cos(2*theta1 - 2*theta2) + L1**2*l2**2*m1*m2**2 - L1*l1*l2**2*m1*m2**2*cos(2*theta1) + L1*l1*l2**2*m1*m2**2*cos(2*theta2) + L1*l1*l2**2*m1*m2**2*cos(2*theta1 - 2*theta2) - L1*l1*l2**2*m1*m2**2 + 2*M*l1**2*l2**2*m1*m2 - l1**2*l2**2*m1**2*m2*cos(2*theta1) + l1**2*l2**2*m1**2*m2 - l1**2*l2**2*m1*m2**2*cos(2*theta2) + l1**2*l2**2*m1*m2**2)
  dd_theta2 = l2*m2*(-F*I1*cos(theta2) - F*L1**2*m2*cos(theta2)/2 + F*L1**2*m2*cos(2*theta1 - theta2)/2 + F*L1*l1*m1*cos(theta2)/2 + F*L1*l1*m1*cos(2*theta1 - theta2)/2 - F*l1**2*m1*cos(theta2) + I1*L1*M*dtheta1**2*sin(theta1 - theta2) + I1*L1*dtheta1**2*m1*sin(theta1 - theta2) + I1*L1*dtheta1**2*m2*sin(theta1 - theta2)/2 - I1*L1*dtheta1**2*m2*sin(theta1 + theta2)/2 + I1*M*g*sin(theta2) - I1*dtheta1**2*l1*m1*sin(theta1 - theta2)/2 - I1*dtheta1**2*l1*m1*sin(theta1 + theta2)/2 - I1*dtheta2**2*l2*m2*sin(2*theta2)/2 + I1*g*m1*sin(theta2) + I1*g*m2*sin(theta2) + L1**3*M*dtheta1**2*m2*sin(theta1 - theta2) + L1**3*dtheta1**2*m1*m2*sin(theta1 - theta2) + L1**2*M*dtheta2**2*l2*m2*sin(2*theta1 - 2*theta2)/2 + L1**2*M*g*m2*sin(theta2)/2 - L1**2*M*g*m2*sin(2*theta1 - theta2)/2 - 3*L1**2*dtheta1**2*l1*m1*m2*sin(theta1 - theta2)/2 + L1**2*dtheta1**2*l1*m1*m2*sin(theta1 + theta2)/2 + L1**2*dtheta2**2*l2*m1*m2*sin(2*theta1 - 2*theta2)/2 + L1**2*g*m1*m2*sin(theta2)/2 - L1**2*g*m1*m2*sin(2*theta1 - theta2)/2 + L1*M*dtheta1**2*l1**2*m1*sin(theta1 - theta2) - L1*M*g*l1*m1*sin(theta2)/2 - L1*M*g*l1*m1*sin(2*theta1 - theta2)/2 + L1*dtheta1**2*l1**2*m1**2*sin(theta1 - theta2)/2 + L1*dtheta1**2*l1**2*m1**2*sin(theta1 + theta2)/2 + L1*dtheta1**2*l1**2*m1*m2*sin(theta1 - theta2)/2 - L1*dtheta1**2*l1**2*m1*m2*sin(theta1 + theta2)/2 + L1*dtheta2**2*l1*l2*m1*m2*sin(2*theta2)/2 - L1*dtheta2**2*l1*l2*m1*m2*sin(2*theta1 - 2*theta2)/2 - L1*g*l1*m1**2*sin(theta2)/2 - L1*g*l1*m1**2*sin(2*theta1 - theta2)/2 - 3*L1*g*l1*m1*m2*sin(theta2)/2 + L1*g*l1*m1*m2*sin(2*theta1 - theta2)/2 + M*g*l1**2*m1*sin(theta2) - dtheta1**2*l1**3*m1**2*sin(theta1 - theta2)/2 - dtheta1**2*l1**3*m1**2*sin(theta1 + theta2)/2 - dtheta2**2*l1**2*l2*m1*m2*sin(2*theta2)/2 + g*l1**2*m1**2*sin(theta2)/2 + g*l1**2*m1**2*sin(2*theta1 - theta2)/2 + g*l1**2*m1*m2*sin(theta2))/(I1*I2*M + I1*I2*m1 + I1*I2*m2 + I1*M*l2**2*m2 + I1*l2**2*m1*m2 - I1*l2**2*m2**2*cos(theta2)**2 + I1*l2**2*m2**2 + I2*L1**2*M*m2 + I2*L1**2*m1*m2 - I2*L1**2*m2**2*cos(theta1)**2 + I2*L1**2*m2**2 - 2*I2*L1*l1*m1*m2*cos(theta1)**2 + I2*M*l1**2*m1 - I2*l1**2*m1**2*cos(theta1)**2 + I2*l1**2*m1**2 + I2*l1**2*m1*m2 - L1**2*M*l2**2*m2**2*cos(theta1 - theta2)**2 + L1**2*M*l2**2*m2**2 - L1**2*l2**2*m1*m2**2*cos(theta1 - theta2)**2 + L1**2*l2**2*m1*m2**2 - L1**2*l2**2*m2**3*cos(theta1)**2 + 2*L1**2*l2**2*m2**3*cos(theta1)*cos(theta2)*cos(theta1 - theta2) - L1**2*l2**2*m2**3*cos(theta2)**2 - L1**2*l2**2*m2**3*cos(theta1 - theta2)**2 + L1**2*l2**2*m2**3 - 2*L1*l1*l2**2*m1*m2**2*cos(theta1)**2 + 2*L1*l1*l2**2*m1*m2**2*cos(theta1)*cos(theta2)*cos(theta1 - theta2) + M*l1**2*l2**2*m1*m2 - l1**2*l2**2*m1**2*m2*cos(theta1)**2 + l1**2*l2**2*m1**2*m2 - l1**2*l2**2*m1*m2**2*cos(theta2)**2 + l1**2*l2**2*m1*m2**2)

  f_expl = vertcat(cart_v,
                   dtheta1,
                   dtheta2,
                   dd_x,
                   dd_theta1,
                   dd_theta2
                   )
  
  f_expl = f_expl + w
  
  f_impl = x_dot - f_expl

  model = AcadosModel()

  model.f_expl_expr = f_expl
  model.f_impl_expr = f_impl
  model.x = x
  model.xdot = x_dot
  model.u = w
  model.z = z
  model.p = p
  model.name = model_name

return model
  1. Code of mhe solver

import numpy as np
from scipy.linalg import block_diag
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
from casadi import vertcat

def double_pendulum_mhe_solver(model:AcadosModel, N: int, h, Q, P, R) -> AcadosOcpSolver:

  dp_mhe = AcadosOcp()

  dp_mhe.model = model

  x = dp_mhe.model.x
  u = dp_mhe.model.u

  nx = x.rows()
  nu = u.rows()

  nparam = model.p.rows()

  ny_0 = nx + nu + nx         # h(x), w and arrival cost
  ny   = nx + nu              # h(x), w

  dp_mhe.dims.N = N

  ## set inital cost
  dp_mhe.cost.cost_type_0 = 'NONLINEAR_LS'
  dp_mhe.cost.W_0 = block_diag(R, Q, P)
  dp_mhe.model.cost_y_expr_0 = vertcat(x, u, x)
  dp_mhe.cost.yref_0 = np.zeros((ny_0,))

  # intermediate
  dp_mhe.cost.cost_type = 'NONLINEAR_LS'
  dp_mhe.cost.W = block_diag(R, Q)
  dp_mhe.model.cost_y_expr = vertcat(x, u)
  dp_mhe.cost.yref = np.zeros((ny,))

  dp_mhe.parameter_values = np.zeros((nparam, ))
  

  # terminal
  dp_mhe.cost.cost_type_e = 'LINEAR_LS'
  dp_mhe.cost.yref_e = np.zeros((0, ))

  # Solver options
  dp_mhe.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
  dp_mhe.solver_options.hessian_approx = 'GAUSS_NEWTON'
  dp_mhe.solver_options.integrator_type = 'ERK'
  dp_mhe.solver_options.tf = N*h   # set prediction horizon

  # dp_mhe.solver_options.nlp_solver_type = 'SQP'
  dp_mhe.solver_options.nlp_solver_type = 'SQP_RTI'
  dp_mhe.solver_options.nlp_solver_max_iter = 200

  acados_solver_mhe = AcadosOcpSolver(dp_mhe, json_file = 'acados_dp_mhe.json')

return acados_solver_mhe

5 Code for test

  from export_double_pendulum_ocp_model   import export_double_pendulum_ocp_model
  from export_double_pendulum_ocp_solver  import double_pendulum_ocp_solver

  from export_double_pendulum_mhe_model   import export_double_pendulum_mhe_model
  from export_double_pendulum_mhe_solver  import double_pendulum_mhe_solver

  import numpy as np 
  from utils import plot_pendulum

 # general information

 Tf = 1.0    # Prediction horizon
 N = 20      # Number of shooting intervals
 h = Tf/N

# ocp model and solver
model_ocp = export_double_pendulum_ocp_model()

nx = model_ocp.x.rows()
nu = model_ocp.u.rows()

Q_ocp = np.diag([1e4, 1e4, 1e2, 1e-2, 1e-2, 1e-3])
R_ocp = 1e-3 *np.eye(1)

acados_solver_ocp = double_pendulum_ocp_solver(model_ocp, N, h, Q_ocp, R_ocp, umax=50)
# -------------

# mhe model and solver
model_mhe = export_double_pendulum_mhe_model()

nx = model_mhe.x.rows()
nw = model_mhe.u.rows()
ny = nx

P_mhe = 10*np.eye((nx))
Q_mhe  = 0.1*np.eye(nw)
R_mhe  = 0.1*np.eye(ny)

acados_solver_mhe = double_pendulum_mhe_solver(model_mhe, N, h, Q_mhe, P_mhe, R_mhe)

# simulation
v_stds = [0.1, 0.5, 0.5, 0.01, 0.02, 0.01]

simX = np.zeros((N+1, nx))
simU = np.zeros((N,   nu))
simY = np.zeros((N+1, nx))

simXest = np.zeros((N+1, nx))
simWest = np.zeros((N,   nw))


# arrival cost mean
x0_bar = np.array([0.0, -np.pi, -np.pi, 0.0, 0.0, 0.0])

# solve ocp problem
status = acados_solver_ocp.solve()

if status != 0:
    raise Exception(f'acados returned status {status}.')


# get solution
for i in range(N):
    simX[i,:] = acados_solver_ocp.get(i, "x")
    simU[i,:] = acados_solver_ocp.get(i, "u")
    simY[i,:] = simX[i,:] + np.transpose(np.diag(v_stds) @ np.random.standard_normal((ny, 1)))

simX[N,:] = acados_solver_ocp.get(N, "x")
simY[N,:] = simX[N,:] + np.transpose(np.diag(v_stds) @ np.random.standard_normal((ny, 1)))


# set measurements and controls
yref_0 = np.zeros((ny+nw+nx, ))
yref_0[:ny] = simY[0, :]
yref_0[ny+nw:] = x0_bar

acados_solver_mhe.set(0, "yref", yref_0)
acados_solver_mhe.set(0, "p", simU[0,:])


yref = np.zeros((ny+nx, ))
for j in range(1,N):
    yref[:ny] = simY[j, :]
    acados_solver_mhe.set(j, "yref", yref)
    acados_solver_mhe.set(j, "p", simU[j,:])
    acados_solver_mhe.set(j, "x", simX[j,:])


# solve mhe problem
status = acados_solver_mhe.solve()

if status != 0 and status != 2:
    raise Exception(f'acados returned status {status}.')

# get solution
for i in range(N):
    simXest[i,:] = acados_solver_mhe.get(i, "x")
    simWest[i,:] = acados_solver_mhe.get(i, "u")

simXest[N, :] = acados_solver_mhe.get(N, "x")

print('difference |x0_est - x0_bar|', np.linalg.norm(x0_bar - simXest[0, :]))
print('difference |x_est - x_true|', np.linalg.norm(simXest - simX))

#plot_pendulum(np.linspace(0, Tf, N+1), simU, simX, simXest, simY, latexify=False)
12

Both the MPC and the MHE model use model_name = 'double_pendulum_on_cart' as model name. Please choose unique names, e.g. 'double_pendulum_on_cart_ocp' and 'double_pendulum_on_cart_mhe' as this name will be included in the name of the shared library corresponding to the compiled OCP/MHE solver.

13

Hi,

Thanks a lot.

I thought the model name in the “main” script means “model_ocp” and “model_mhe”. I don’t realize the name in the model class is so important. I will keep that in my mind. now the algorithm runs very well. Thanks again.

However I still a two questions about the code of example: pendulum on cart.

  1. Could you explain to me, why R_mhe has to be scaled with time step?

# inverse covariances, R_mhe has to be scaled with time step
Q_mhe = np.diag(1/w_stds_mhe)
R_mhe = 1/Ts*np.diag(1/v_stds_mhe)
  1. I have modified the example, suppose only the velocity and angle velocity be measured, according that, the position and the angle will be estimated. But the estimation of position has a offset. To verify this, I have modified the mhe_solver and the closed loop test file. In the test file, the true value without noise has been give back to mpc controller. Then the offset of estimated Position is very clear.
    Figure_1

Main Code as follows:

from pendulum_model import export_pendulum_ode_model
from export_mhe_ode_model import export_mhe_ode_model

from export_ocp_solver import export_ocp_solver
from export_mhe_mV_solver import export_mhe_mV_solver

from export_ode_mhe_integrator import export_ode_mhe_integrator

from utils import plot_pendulum
import numpy as np


def main():
  Tf_ocp = 1.0
  N_ocp = 40

  Ts = Tf_ocp/N_ocp # time step

  Tf_mhe = 0.5*Tf_ocp  
  N_mhe = int(Tf_mhe/Ts)

  u_max = 80

  # state and measurement noise
  v_stds_mhe = np.array([0.5, 0.3]) # measurement noise stds
  w_stds_mhe = np.array([0.01, 0.001, 0.001, 0.001]) # state noise stds

  v_stds_plant = np.zeros((2,))
  w_stds_plant = np.zeros((4,))
  # w_stds_plant = 0.5*w_stds_mhe

  V_mat = np.diag(v_stds_plant)
  W_mat = np.diag(w_stds_plant)

  # ocp model and solver  to  generate tracjetories  ------------------------------------
  model_ocp = export_pendulum_ode_model()

  nx = model_ocp.x.rows()
  nu = model_ocp.u.rows()

  Q_ocp = np.diag([1e3, 1e3, 1e-2, 1e-2])
  R_ocp = 1 * np.eye(1)

  acados_ocp_solver = export_ocp_solver(model_ocp, N_ocp, Ts, Q_ocp, R_ocp, Fmax=u_max)
  #---------------------------------------------------------------------------------------


  # mhe model and solver
  model_mhe = export_mhe_ode_model()
  nx = model_mhe.x.rows()
  nw = model_mhe.u.rows()
  ny = v_stds_mhe.size  # supporse ymeas = [dx, dtheta]

  # inverse covariances, R_mhe has to be scaled with time step
  #Q_mhe = np.diag(1/w_stds_mhe)
  Q_mhe = np.eye(4)
  #R_mhe = 1/Ts*np.diag(1/v_stds_mhe)
  R_mhe = np.eye(2)

  # arrival cost weighting
  Q0_mhe = 0.1 * Q_mhe

  acados_mhe_solver = export_mhe_mV_solver(model_mhe, N_mhe, Ts, Q_mhe, Q0_mhe, R_mhe)
  

  # integrator/plant
  plant = export_ode_mhe_integrator(model_mhe, Ts)

  # simulation
  Nsim = 100

  simX = np.zeros((Nsim+1, nx))
  simU = np.zeros((Nsim,   nu))
  simY = np.zeros((Nsim+1, ny))

  simXest = np.zeros((Nsim+1, nx))

  # arrival cost mean & initial state
  x0_plant = np.array([0.1, np.pi + 0.5, -0.05, 0.05])
  x0_bar = np.array([0.0, np.pi, 0.0, 0.0])

  u0 = np.zeros((nu,))

  # initial state
  simX[0,:] = x0_plant

  # initialize MHE solver
  for i in range(N_mhe):
      acados_mhe_solver.set(i, "x", x0_bar)

  # simulate for N_mhe steps with zero input
  for i in range(N_mhe):

      # measurement
      simY[i,:] = simX[i,[2,3]] #+ (V_mat @ np.random.standard_normal((ny,1))).T

      # simulate
      w = W_mat @ np.random.standard_normal((nx,))
      simX[i+1,:] = plant.simulate(x=simX[i,:], u=w, p=u0)

  # reference for mhe
  yref = np.zeros((ny+nw, ))
  yref_0 = np.zeros((ny+nw+nx, ))
  

  # closed loop
  for i in range(N_mhe, Nsim):

      ### estimation ###
      k = i - N_mhe

      # set measurements
      yref_0[:ny] = simY[k, :]
      yref_0[ny+nw:] = x0_bar
     

      acados_mhe_solver.set(0, "yref", yref_0)
      # set controls
      acados_mhe_solver.set(0, "p", simU[k,:])

      for j in range(1, N_mhe):
          # set measurements
          yref[:ny] = simY[k+j, :]
          acados_mhe_solver.set(j, "yref", yref)
          # set controls
          acados_mhe_solver.set(j, "p", simU[k+j,:])

      status = acados_mhe_solver.solve()

      if status != 0:
          raise Exception(f'estimator returned status {status} in step {i}.')
      simXest[i,:] = acados_mhe_solver.get(N_mhe, "x")

      # update arrival cost
      x0_bar = acados_mhe_solver.get(1, "x")

      ### control ###
      simU[i:, ] = acados_ocp_solver.solve_for_x0(simXest[i, :])

      ### simulation ###
      # measurement
      simY[i,:] = simX[i, [2,3]] #+ (V_mat @ np.random.standard_normal((ny,1))).T
      w = W_mat @ np.random.standard_normal((nx,))
      simX[i+1,:] = plant.simulate(x=simX[i,:], u=w, p=simU[i,:])

  # plot
  print('estimation error p', np.linalg.norm(simX[:, 0] - simXest[:, 0]))
  print('estimation error theta', np.linalg.norm(simX[:, 1] - simXest[:, 1]))
  print('estimation error v', np.linalg.norm(simX[:, 2] - simXest[:, 2]))
  print('estimation error dtheta', np.linalg.norm(simX[:, 3] - simXest[:, 3]))

  plot_pendulum(np.linspace(0, Ts*Nsim, Nsim+1), u_max, simU, simX, simXest[N_mhe:, :], simY, 1)

if __name__ == '__main__':
    main()

Code for mhe solver

import numpy as np
from scipy.linalg import block_diag
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
from casadi import vertcat

def export_mhe_mV_solver(model: AcadosModel, N: int, h, Q, Q0, R) -> AcadosOcpSolver:

    ocp_mhe = AcadosOcp()

    ocp_mhe.model = model

    x = ocp_mhe.model.x
    u = ocp_mhe.model.u
 

    nx = x.rows()
    nu = u.rows()
    nparam = model.p.rows()
    nymeas = R.shape[0]

    ny_0 = nymeas + nu + nx 
    ny   = nymeas + nu                    # h(x), w
    
    ymeas = x[nymeas:]


    ocp_mhe.dims.N = N

    # Initial cost
    ocp_mhe.cost.cost_type_0 = "NONLINEAR_LS"
    ocp_mhe.cost.W_0 = block_diag(R, Q, Q0)
    ocp_mhe.model.cost_y_expr_0 = vertcat(ymeas, u, x)
    ocp_mhe.cost.yref_0 = np.zeros((ny_0,))
 

    # intermediate
    ocp_mhe.cost.cost_type = 'NONLINEAR_LS'

    ocp_mhe.cost.W = block_diag(R, Q)
    ocp_mhe.model.cost_y_expr = vertcat(ymeas, u)
    ocp_mhe.parameter_values = np.zeros((nparam, ))
    ocp_mhe.cost.yref = np.zeros((ny,))

    # terminal
    ocp_mhe.cost.cost_type_e = 'LINEAR_LS'
    ocp_mhe.cost.yref_e = np.zeros((0, ))

    ocp_mhe.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
    # ocp_mhe.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
    ocp_mhe.solver_options.hessian_approx = 'GAUSS_NEWTON'
    ocp_mhe.solver_options.integrator_type = 'ERK'

    # set prediction horizon
    ocp_mhe.solver_options.tf = N*h

    ocp_mhe.solver_options.nlp_solver_type = 'SQP'
    # ocp_mhe.solver_options.nlp_solver_type = 'SQP_RTI'
    ocp_mhe.solver_options.nlp_solver_max_iter = 200

    acados_solver_mhe = AcadosOcpSolver(ocp_mhe, json_file = 'acados_ocp_mhe.json')

    return acados_solver_mhe
14

Not sure about the scaling, I don’t think you necessarily need it. By default acados scales the stage cost with the time step (the terminal cost is not scaled). Recently, we added the option to provide custom scaling via the option cost_scaling. But I don’t know why you would only want to scale R_mhe.

Regarding the offset: This is expected as you can not estimate a position based on velocity measurements alone. You can not distinguish the starting point of the motion if you only have velocity measurements available. Check for the concept of observability in any control/estimation textbook.

With noise free measurements (and no model-plant mismatch), the best you can get is a perfect fit for the measurements (which is what you get in your example), but you cannot necessarily expect a perfect fit for the states which are not directly measured.

15

Hi,

about the scaling. the code was extracted from official mhe example line 83 to 85, it’s not that I only want to scale R_mhe. I think, if a discrete model be implemented, the scaling will be appeared in the stage cost. But we use a continuous model, the acados will achieve the scaling, I think. I have tested, if the 1/Ts be deleted, the results of error is a little larger.

about the offset: thanks for the explanation. for this case, if set x0_bar = x0_plant, the offset will be eliminated.
in conclusion , I think I have understood, how to using acados to solve the mhe problem.

1 Like