Output reference tracking

[Matyjehouba]

I've tried to use the tiny MPC to follow some output reference, which is basically just one of the states, but I've encountered a problem. Even if I add the Q matrix set to zeros on all just the one state it still seemed like the other states influenced the tracking.

Ive also changed the linear cost update function to only take into account the reference of the output state but it started to fall apart completely. Is there part of the ADMM I have to change too?

[ishaanamahajan]

The rho or the ADMM penalty parameter specifically might be causing issues here. So even if you set Q to be 0, the pre-computation uses the rho penalty parameter that might not actually make your Q to be 0.

About changing the linear cost update function, if you only consider your output state, the solver likely became unstable because you broke the mathematical consistency of the ADMM formulation since the slack variables, dual variables, and their updates in update_slack and update_dual still operate on the full state space.

I would suggest using really small values for Q (like 1e-6) for the states you don't want to track, while keeping your desired weight on the output state. You should also ensure your reference trajectory Xref has reasonable values for all states (not just the output), since the dynamics couple everything together.

[Matyjehouba]

Thank you very much for answering, I assumed the rho parameter might cause some trouble but is it solvable?

I tried to put small values on the diagonal but it didnt really matter whether the Q = diag(1e-6, 1e-6, 1e-6, 1e-6,1000) or if there were zeros in the other states. I also tried to calculate the pre-computation with this Q matrix instead of the one that has the ADMM penalty added to it. That didn't work either.

How I solved it is I made the other state references to equal to the current states values... I imagine since the controller wants to maintain in the four states and track the reference of the fifth, it slows down the control but at least it works. It is not a nice solution tho.

Is there a way to uncouple the system from the other states or make also the slack variables only receptive to the fifth state or does the solver become unstable? I also suspect that the weight on the output state is too big, and the controller sometimes outputs the control inputs out of bounds. With smaller weight than 1000 the controller doesnt track the state well.

I also dont use any input reference, but that might also be the issue rigth? Since its trying to track the input reference that is set to zero.

[plancherb1]

Fundamentally TinyMPC is solving a problem and considering the linearized dynamics Ax_k + Bu_k = x_{k+1} which will always couple across state dimensions depending on your physics model. For example, lets say you have a quadrotor and you only put a cost on the X position. Well to move in any direction but Z we need to tilt the quadrotor and thus this will lead to changes in other state dimensions. As such, for most systems you can't purely decouple one dimension (nor do you want to if you want an optimal solution). Does that help clarify things?