MHE cost function with xdot variable

Hi :wave:

Firstly, thank you very much for developing such an efficient tool for calculating optimization problems. I am a newcomer to using acados. I try to use acados in Python to solve an MHE problem. The model is an implicit DAE with 12 dynamic states and 3 inputs, like f(q, qd, qdd, p, u)+w=0 (q means distance, qd means velocity, qdd means acceleration). The measurement model is y=h(q, qd, qdd, p, u)+v (This measurement model depends on the accelerations). From the ā€œproblem_reformulationā€ folder in acados acados/docs/problem_formulation at master Ā· acados/acados Ā· GitHub, I know the qdd canā€™t be directly put into the cost function as xdot variable. So I try to configurate 6 new algebraic equations, like 0=z-qdd, (the new model becomes f(q, qd, qdd, p, u)+w=0, z-qdd=0) and then add qdd in cost function as algebraic variable. The measurement model is rewritten into y=h(q, qd, z, p, u)+v.
To make sure qdd can be calculated by acados, I first using ā€œAcadosSimā€ to test the new model (ignore noise v and w), qdd can be successful calculated. Then I use simulation data to generate measurements, which is almost equal to the correct measurement.
But when I configure an OCP to MHE and try to solve it, an error occur

QP solver returned error status 3 in SQP iteration 2, QP iteration 1.
Solver returned status 4 in step 0.

At first, I thought it might be because of the wrong reference value or illness weighting matrix. So I scale all the data and rewrite the weighting matrix and check the reference value. I also try different step size from 0.01 to 10, and solver types (ā€˜PARTIAL_CONDENSING_HPIPMā€™, ā€˜FULL_CONDENSING_QPOASESā€™, ā€˜GAUSS_NEWTONā€™ ). But it still doesnā€™t work properly.
Are there any other causes that could lead to this type of problem? I would appreciate it if you could give me some possibilities. If you are interested in this code I can also clean it up and share it later
Best regards, :pray:
Benjamin