hello every one , I am glad to join your forum , I am new with Acados and I want to implement Moving Horizon Estimation (MHE) method and my model has a form of 0 = f(x_dot, x, u , p) . I want to use MHE to not only do the state estimation but also parameter estimation , and I am using Python. My question is that how can I define constraints for parameters that I already define in “model.p” and then solve OCP to get not only estimated states but also estimated parameters ??
Hi,
did you check out this example?
It shows how you can do MHE with noisy parameters:
Best,
Jonathan
1 Like
Hi again, and many thanks Jonathan for your suggestion. I tried to implement the same procedure of MHE as described in the Pendulum example. But I have constraints on parameters that I wanted to estimate although I tried to define them like below , still the initial estimation of these parameters are out of defined constraints.
# linear constraints
# stage bounds on x
ocp_mhe.constraints.lbx = np.array([model.hitch_angle_min, model.delta_f_min, model.m2_min , model.Iz2_min]) # lower bounds on x at intermediate shooting nodes (1 to N-1)
ocp_mhe.constraints.ubx = np.array([model.hitch_angle_max, model.delta_f_max, model.m2_max , model.Iz2_max]) # upper bounds on x at intermediate shooting nodes (1 to N-1)
ocp_mhe.constraints.idxbx = np.array([3,8, 10, 11]) # indices of bounds on x (defines Jbx) at intermediate shooting nodes (1 to N-1)
# terminal bounds on x
ocp_mhe.constraints.lbx_e = np.array([model.hitch_angle_min, model.delta_f_min, model.m2_min , model.Iz2_min]) # lower bounds on x at terminal shooting node N
ocp_mhe.constraints.ubx_e = np.array([model.hitch_angle_max, model.delta_f_max, model.m2_max , model.Iz2_max]) # upper bounds on x at terminal shooting node N
ocp_mhe.constraints.idxbx_e = np.array([3,8 ,10 ,11]) # Indices for bounds on x at terminal shooting node N (defines Jebx)
ocp_mhe.constraints.lbx_0 = np.array([model.hitch_angle_min, model.delta_f_min, model.m2_min , model.Iz2_min]) # lower bounds on x at intermediate shooting nodes (1 to N-1)
ocp_mhe.constraints.ubx_0 = np.array([model.hitch_angle_max, model.delta_f_max, model.m2_max , model.Iz2_max]) # upper bounds on x at intermediate shooting nodes (1 to N-1)
ocp_mhe.constraints.idxbx_0 = np.array([3,8, 10, 11])
# Get the number of non linear constraints in the stage cost
nh = ocp_mhe.model.con_h_expr.shape[0]
# Define which one of the input constraints will be written as slack variables in the stage cost function
# In our case, only one input constraint is defined, so we will use the one
ocp_mhe.constraints.Jsbu = np.eye(0)
# Define which one of the state constraints will be written as slack variables in the stage cost function
# In our case, only one state constraint is defined, so we will use the one
ocp_mhe.constraints.Jsbx = np.eye(4)
# Define which one of the nonlinear constraints will be written as slack variables in the stage cost function
# In our case, we will use all of them
ocp_mhe.constraints.Jsh = np.eye(nh)
# Get the number of non linear constraints in the stage cost
nh_0 = ocp_mhe.model.con_h_expr_0.shape[0]
# Define which one of the input constraints will be written as slack variables in the stage cost function
# In our case, only one input constraint is defined, so we will use the one
ocp_mhe.constraints.Jsbu_0 = np.eye(0)
# Define which one of the state constraints will be written as slack variables in the stage cost function
# In our case, only one state constraint is defined, so we will use the one
ocp_mhe.constraints.Jsbx_0 = np.eye(4)
# Define which one of the nonlinear constraints will be written as slack variables in the stage cost function
# In our case, we will use all of them
ocp_mhe.constraints.Jsh_0 = np.eye(nh_0)
# Define the penalty for the slack variables
# z_1 is the linear penalty for the slack variables in the stage cost function
z_1 = np.ones((nh + 4,)) * L1_pen
# z_2 is the quadratic penalty for the slack variables in the stage cost function
z_2 = np.ones((nh + 4,)) * L2_pen
# Add the penalty to the cost function
# Obs: the vectors Zl, Zu, zl, zu, Zl_e, Zu_e, zl_e, zu_e are defined as the weight of the constraints violation
# When we have slack variables in input, state and nonlinear constraints, the order that these variables are stacked is
# [input_slack, state_slack, nonlinear_slack], according to the link above, last line of page 1.
# Quadratic penalty to when the constraint is violated in lower bound in the stage cost
ocp_mhe.cost.Zl = z_2
# Quadratic penalty to when the constraint is violated in upper bound in the stage cost
ocp_mhe.cost.Zu = z_2
# Linear penalty to when the constraint is violated in lower bound in the stage cost
ocp_mhe.cost.zl = z_1
# Linear penalty to when the constraint is violated in upper bound in the stage cost
ocp_mhe.cost.zu = z_1
# Quadratic penalty to when the constraint is violated in lower bound in the stage cost
#ocp.cost.Zl_0 = np.ones((nh + 0,)) * L2_pen
ocp_mhe.cost.Zl_0 = np.ones((nh_0 + 0,)) * L2_pen
# Quadratic penalty to when the constraint is violated in upper bound in the stage cost
#ocp.cost.Zu_0 = np.ones((nh + 0,)) * L2_pen
ocp_mhe.cost.Zu_0 = np.ones((nh_0 + 0,)) * L2_pen
# Linear penalty to when the constraint is violated in lower bound in the stage cost
#ocp.cost.zl_0 = np.ones((nh + 0,)) * L1_pen
ocp_mhe.cost.zl_0 = np.ones((nh_0 + 0,)) * L1_pen
# Linear penalty to when the constraint is violated in upper bound in the stage cost
#ocp.cost.zu_0 = np.ones((nh + 0,)) * L1_pen
ocp_mhe.cost.zu_0 = np.ones((nh_0 + 0,)) * L1_pen
# Slack variables for the terminal cost constraints
# Get the number of non linear constraints in the terminal cost
nh_e = ocp_mhe.model.con_h_expr_e.shape[0]
# Define which one of the state constraints will be written as slack variables in the terminal cost function
# In our case, only one state constraint is defined, so we will use the one
ocp_mhe.constraints.Jsbx_e = np.eye(4)
# Define which one of the nonlinear constraints will be written as slack variables in the terminal cost function
# In our case, we will use all of them
ocp_mhe.constraints.Jsh_e = np.eye(nh_e)
# Define the penalty for the slack variables
# z_1 is the linear penalty for the slack variables in the terminal cost function
z_1_e = np.ones((nh_e + 4,)) * L1_pen
# z_2 is the quadratic penalty for the slack variables in the terminal cost function
z_2_e = np.ones((nh_e + 4,)) * L2_pen
# Add the penalty to the cost function
# Quadratic penalty to when the constraint is violated in lower bound in the terminal cost
ocp_mhe.cost.Zl_e = z_2_e
# Quadratic penalty to when the constraint is violated in upper bound in the terminal cost
ocp_mhe.cost.Zu_e = z_2_e
# Linear penalty to when the constraint is violated in lower bound in the terminal cost
ocp_mhe.cost.zl_e = z_1_e
# Linear penalty to when the constraint is violated in upper bound in the terminal cost
ocp_mhe.cost.zu_e = z_1_e
Also I noticed for the initial stage , state can not be slacked !! I think this is why we can not enforce penalty to those estimated states which have violated the constraints.
Thanks for your time and helps .