MHE using algebraic variables

User Questions
1

Hi! First of all thanks for providing an MHE example to get started with acados. I am trying to solve the following problem:

I have a model function f(x,z)+w and measurement function h(x,z)+v, where x is the vector of differential states, z is the vector of algebraic states, w is the process noise and v the measurement noise (both aditive). I would like to have an arrival cost both on x and z (each one with its own weighting matrices) and minimize the process and measurement noise. Is there anything that I should take into account in order to formulate this problem? Right now I am stuck with acados telling me that the problem cannot be compiled because the vector of algebraic states is “free”.

I have a working MHE implementation for this problem using CasADi and IPOPT, but since I want to run it in an embedded system, having it in acados would be great.

Thanks in advance!

2

Hi @gmsanchez,

The algebraic variables can be included into the nonlinear least squares cost terms.
Starting in this line and adapting the weighting matrix W later on and the dimension ny before that.

Regarding the CasADi error that algebraic states are free:
You also have to add z to the model, adding the following to the file I pointed above:

    z = ocp_mhe.model.z

I hope this fixes the issue.

Best,
Jonathan

3

@FreyJo

Thanks again for the quick response. I kept getting the same error message despite following your suggestions. After reading the manual, I was wondering if should I use IRK instead of ERK (since I use algebraic states).

After changing the integrator to IRK, the same error message kept appearing regarding the generation of ‘mhe_ode_r_cost’ (my model name is ‘mhe_ode’). I inspected the file generate_c_code_nls_cost.py (it is in the acados_template folder) and modified the code according to this commit.

After adding the algebraic variable to the Jacobian (I don’t know if it would be right) and then to the CasADi function generator (I think this would be right, since my measurement function is h(x,z)) I was able to get past the C code generation.

I generated some test data and tried to see if the MHE works, but after setting the measurements to the yref vector and calling acados_solver_mhe.solve(), I get

Process finished with exit code 139 (interrupted by signal 11: SIGSEGV)

Thanks in advance for any help!

4

Hi @gmsanchez

Indeed models with algebraic variables can not be used with ERK.

I am sorry, what I suggested before is actually not possible yet.

I just realized that currently only the linear least squares cost module can handle the algebraic variables.
The Nonlinear least squares cost module in C (https://github.com/acados/acados/blob/master/acados/ocp_nlp/ocp_nlp_cost_nls.c) has to be adapted to handle also algebraic variables.
This is a bit more complicated then adding it to the code generation.

I made an issue, but I am not sure if I can fix it very soon.

In case that your measurement function h(x,z) is linear, you can actually use the LINEAR_LS cost.
You could also implement h as a slacked constraint and penalize the corresponding slacks with the same weights, which should be equivalent but admittedly is not as clean.

Sorry for being too optimistic earlier!

Best,
Jonathan

5

@FreyJo

My measurement function h(x, z) is linear. Right now I am trying to use the LINEAR_LS cost without any luck.

Let’s suppose that the vector x has size 10, z has size 6, the process noise w (or u, when the OCP is formulated) has size 10 and my measurement vector has size 9: h(x,z) = [x0, x1, x2, z0, z1, z2, z3, z4, z5].

To formulate the LINEAR_LS cost I should:

nx = 10
nz = 6
nu = 10
ny = 9

ocp_mhe.cost.Vx = np.zeros((nx, nx))
ocp_mhe.cost.Vx[:3, :3] = np.eye(3)
ocp_mhe.cost.Vz = np.eye(nz)
ocp_mhe.cost.Vu = np.zeros((nu, nu))
ocp_mhe.cost.W = np.eye(ny)

Is that right or am I missing something? Because after making these changes I cannot get the example to compile the generated code.

Thanks a lot for the help!

6

Thats great!

There is just something wrong with the snippet you wrote.

In the current mhe example, there is:

    ocp_mhe.model.cost_y_expr = vertcat(x, u, x)

where the first entries of y, namely x,u is your h and the second x is for the arrival cost.

Transcribing this into the LINEAR_LS formulation, you have to do something:

ocp_mhe.cost.Vx = np.zeros((ny, nx))
ocp_mhe.cost.Vx[:nx,:nx] = np.eye(nx)
ocp_mhe.cost.Vx[nx+nu:,:nx] = np.eye(nx)

ocp_mhe.cost.Vu = np.zeros((ny, nu))
ocp_mhe.cost.Vu[nx:nx+nu,:] = np.eye(2)

ocp_mhe.cost.W = np.eye(ny)

For your h into the LINEAR_LS formulation, you have to do something

ny = 3 + 6 + nx # len(h) + len(x)

the V* matrices must have ny rows.

The dimension check for the matrices is currently broken on the master, but fixed here:

So these dimension errors should be easy to find then.

7

@FreyJo

Thanks a lot for all the help! After your hints on how to transcribe the current example to the LINEAR_LS formulation, I could get the code to compile and iterate. Sadly, I always get zero as a result.

My current y consists of a “concatenation” of: [h(x,z), w, x, z] (the measurement function, the process noise and the arrival cost for x[0] and z[0], respectively).

In a CasADi SX expression it would be:
[x_0, x_1, x_2, z_0, z_1, z_2, z_3, z_4, z_5, w_0, w_1, w_2, w_3, w_4, w_5, w_6, w_7, w_8, w_9, x_0, x_1, x_2, x_3, x_4, x_5, x_6, x_7, x_8, x_9, z_0, z_1, z_2, z_3, z_4, z_5]

To get this terms arranged this way, I did the following with the Vx, Vu, Vz and W matrices:

    nx = model.x.size()[0]  # 10
    nu = model.u.size()[0]  # 10 (the process noise)
    nz = model.z.size()[0]  # 6

    nparam = model.p.size()[0]  # 0

    n_meas = 9  # the output size of h(x,z)
    ny = 3 + 6 + nu + nx + nz # h(x,z), w and arrival cost for x0 and z0
    ny_e = 0

    ocp_mhe.cost.Vx = np.zeros((ny, nx))
    ocp_mhe.cost.Vu = np.zeros((ny, nu))
    ocp_mhe.cost.Vz = np.zeros((ny, nz))

    ocp_mhe.cost.W = block_diag(R, Q, 0.0* Q0_x, 0.0 * Q0_z)

    ocp_mhe.cost.Vx[:3, :3] = np.eye(3)
    ocp_mhe.cost.Vx[n_meas + nx: n_meas + nx + nx, :] = np.eye(nx)

    ocp_mhe.cost.Vu[n_meas: n_meas + nu, :] = np.eye(nu)

    ocp_mhe.cost.Vz[3: 3+nz, :] = np.eye(nz)
    ocp_mhe.cost.Vz[n_meas + nu + nx:, :] = np.eye(nz)

    acados_solver_mhe.cost_set(0, "W", block_diag(R, Q, Q0_x, Q0_z))

After that, I call the solver with the same data I use in an implementation of MHE that uses CasADi + IPOPT. For example, I initialize the acados MHE in this way:

# Initialize the MHE problem by setting the measurements and arrival cost
for j in range(N):
    yref = np.hstack([simY[j, ], np.zeros((nw,)), x0bar, z0bar])
    acados_solver_mhe.set(j, "yref", yref)

# solve mhe problem
status = acados_solver_mhe.solve()

# print the solution
for j in range(N):
    print(acados_solver_mhe.get(j, "x"))

And all the states that are variables of the optimization problem are equal to zero.

8

Hi,

I think your adaption looks good!
Let me try to ask a few questions that hopefully help you double check some things.

Did you also set the arrival cost matrices to be nonzero

Q0_x, Q0_z

at the initial stage?

Does acados return status 0?

simY[j, ] is nonzero?

Maybe you can checkout this branch and use:
acados_solver_mhe.print_statistics()
after caling the solver.

Best,
Jonathan

9

Hi,

Q0_x and Q0_z are also identity matrix. simY has the same data that I also use with my CasADi + IPOPT MHE solver. Acados returns status 0. Yesterday I changed the print_level option from 0 to 1 and I found out that the SQP only iterates one time (returns at iteration 0).

On the first iteration with the changed print_level, I get that BAbt, b, RSQrq and rq are all zero. I get no printed output for d, idxb, DCt, d_s, idxs, Z and z and the SQP ends (it doesn’t iterate).

After checking the branch you suggested, I get an error while calling to the export_mhe_solver function. The error complains about a calling to jtimes. The full oputput is here:

  File "/usr/lib/python3/dist-packages/IPython/core/interactiveshell.py", line 2882, in run_code
    exec(code_obj, self.user_global_ns, self.user_ns)
  File "<ipython-input-2-c9177c06d313>", line 1, in <module>
    runfile('/home/guiss/fun/mhe_comparison/minimal_example_mhe.py', wdir='/home/guiss/fun/mhe_comparison/pva')
  File "/opt/pycharm-2019.3/plugins/python/helpers/pydev/_pydev_bundle/pydev_umd.py", line 197, in runfile
    pydev_imports.execfile(filename, global_vars, local_vars)  # execute the script
  File "/opt/pycharm-2019.3/plugins/python/helpers/pydev/_pydev_imps/_pydev_execfile.py", line 18, in execfile
    exec(compile(contents+"\n", file, 'exec'), glob, loc)
  File "/home/guiss/fun/mhe_comparison/minimal_example_mhe.py", line 79, in <module>
    acados_solver_mhe = export_mhe_solver(model_mhe, N, h, Q_mhe, Q0_x_mhe, Q0_z_mhe, R_mhe)
  File "/home/guiss/fun/mhe_comparison/export_mhe_solver.py", line 139, in export_mhe_solver
    acados_solver_mhe = AcadosOcpSolver(ocp_mhe, json_file = 'acados_ocp.json')
  File "/home/guiss/.local/lib/python3.6/site-packages/acados_template/AcadosOcpSolver.py", line 376, in __init__
    ocp_generate_external_functions(acados_ocp, model)
  File "/home/guiss/.local/lib/python3.6/site-packages/acados_template/AcadosOcpSolver.py", line 211, in ocp_generate_external_functions
    generate_c_code_implicit_ode(model, opts)
  File "/home/guiss/.local/lib/python3.6/site-packages/acados_template/generate_c_code_implicit_ode.py", line 78, in generate_c_code_implicit_ode
    ADJ = jtimes(f_impl, x_xdot_z_u, multiplier, True)
  File "/home/guiss/.local/lib/python3.6/site-packages/casadi/casadi.py", line 21606, in jtimes
    return _casadi.jtimes(*args)
RuntimeError: Error in SX::jtimes at .../casadi/core/generic_matrix.hpp:1191:
/casadi/core/generic_matrix.hpp:1164: Assertion "v.size1() == ex.size1() && v.size2() % ex.size2() == 0" failed:
'v' has inconsistent dimensions

My model function f is a CasADi expression that has size 10. It is the same I use with the other MHE solver.

Thanks a lot for all the help.

10

The implicit function that you use to define your model should be of size nx + nz, i.e. 16 in your case.
Because both xdot and z have to be implicitly defined by it.

1 Like
11

Thanks for the clarification. The main problem I am facing is that I don’t have either a differential or algebraic expression for the variable z. At first I thought about putting them as control inputs, doing u = vertcat(w, u). But once I do that, I have one less z in the horizon, because the optimization variable u goes from 0 to N-1 in the acados formulation. I need it to go from 0 to N in the problem I want to solve.

12

If the evolution of these variables is not defined by such an equation, the solver can freely choose it and the u control discretization should be appropriate.
The variable is constant over the last shooting interval and can be penalized at the N-1 node.

Does that make sense to you?

13

That makes sense!

Last night I reformulated the problem and changed the algebraic variables to control inputs by doing u = vertcat(w, z) (I keep the variable name z here although it might be misleading). After making these changes, the MHE solver works and the result are consistent with the results obtained using CasADi + IPOPT.

Now I have the MHE working and iterating over my sample data. But at iteration time j after updating yref with the new measurement and arrival cost, when I want to save the solution I am doing

estimated_x[j, :] = acados_solver_mhe.get(N-1, "x")
estimated_z[j, :] = acados_solver_mhe.get(N-1, "u")[nw:, ]

And I think that is linked to what you said, but I don’t understand what you mean.

You mean that I can add a penalization in order to have a acados_solver_mhe.get(N, "u")[nw:, ] (or equivalent) term? If this is what you meant, I don’t know how to do it.

Thanks again for all the help!

14

Great!

I mean, that what you get with
acados_solver_mhe.get(N-1, "u")
is the value which is valid for the last shooting interval, i.e. the time horizon between shooting node N-1 and N.
So if you want to just get the value, this should be good enough.
Or why do you need it at the Nth node?
If you want to penalize the last value of u (your concatenation) differently, you can just set the weighting matrix at shooting node N-1 accordingly.

15

Jonathan,

Thanks to your help I was finally able to get an MHE implementation to solve the problem I am working on. Since I need to use it with C++, I tried to run the example that worked with Python in C++. To my surprise, it didn’t work as expected.

Is the auto generated main*.c file good to use as a starting point for the MHE example or should I take something else in account? It seems that I am doing a different thing in Python to what is done in the autogenerated C file.

I can post this in another question if this is off-topic to this thread.

Thanks in advance!

16

I think it is a good starting point, since it shows how the solver can be used from C.
Note that the main*.c file just creates the solver (in your case MHE) and initializes it with some values from the json file.
The interaction with the solver, i.e. all acados_solver_mhe.set(*), acados_solver_mhe.get(*) kind of calls have to be translated from the Python interface into acados C interface calls.
I guess MHE is only meaningful when the solver interacts with an integrator/OCP solver, which is not done in the generated main file.

I guess if you have further questions on this, a new thread would be good.

Best,
Jonathan