Implementation of an LQR-Controller

Hello everyone

I’d like to implement an LQR control scheme for quadrotors based on the Snapdragon Flight.
The intention behind this is to realise a small agile platform capable of very aggressive manoeuvres.
Overall I would like to contribute the control scheme to the PX4 flight stack so that it can be used by other people.
Therefore I target a clean implementation that goes along with the existing control modules and would like some inputs and hints from the experienced PX4 developer community.

The overall idea is to use the existing EKF2 estimator fed by a vision or motion capture system to get a full state estimate which should include position, attitude, velocity and bodyrates. This part already exists.

The addition is a module that applies an LQR control with a reference point in the form of:
u = u_0 + K * (x - x_0)
where u is the control command, u_0 is the reference input (ex. stable hover point), K is the gain matrix and x and x_0 are the actual and reference state respectively.

The calculation of K and x_0 is a solved problem and left out of discussion. However, it should be possible to update K and x_0 or only x_0 for example over MAVLink. The actual state x should be available from EKF2. State and input convention is also left out of discussion.

The approach so far:
My idea is to implement this as a module and then add a new “experimental” LQR flight mode.
This mode should be exited whenever the state error becomes too large (as a failsafe/failback measure).

If I understand correctly then the commander module handles the modes, whereas the controllers itself are contained in the mc_pos_control and mc_att_control. On activation of the LQR, the two other control modules should therefore stop publishing.


  1. Test EKF2 with external position measurement from vision or motion capture.
  2. Implement the messages for reference state and LQR gain matrix.
  3. Implement the controller itself
  4. Implement the flight mode used to activate the controller in the commander and mixer. (<- this is my biggest concern atm.)
  5. Implement failsafe measures.

My Questions:

  • Is this a meaningful way to implement such an idea?
  • Is there any ongoing work similar or related to this?
  • Do you (experienced developers) see any obvious flaws in my idea that make it incompatible with the existing structure?
  • Is there a chance that such an implementation might find its way into the upstream (master)?
    Or does it sound not useful enough for most user?
  • Since the commander is a rather complex structure, is there a possibility to get some help to implement an additional flight mode in a clean way?

Thanks for your time! I appreciate any advice, hint or comment!
Greetings, Philipp

Hi Phillip,

I suggest you start by doing this in simulation (SITL I would ignore the modes and start by duplicating the MC controller (mc_att_control) and turning it into your new controller.

I understand that if you are going to use a motion capture system you have to use the LPE estimator, or am I wrong?

Ok. before I start to go too deep into changing controllers:
Is it possible to send bodyrates and collective thrust to the PX4 stack with MAVlink (or directly publishing to a certain uORB topic)?
So basically have the flight mode in “ACRO” but receiving rates from a software component/module?
As I understand, in “OFFBOARD” mode it is only possible to send position commands, correct?

@foehnph Yes take a look at SET_ATTITUDE_TARGET ( and the type_mask field.

@Mario1577 I believe you can do this with both LPE and EKF2, but it’s not something I’ve done.

This is a very interesting idea. I have already implemented something similar. LQR is a model-based technique, so could you give an info about your state variables x(e.g. angles) and control inputs u(e.g. motor forces or voltages) ?

It’s very interesting !

Did you finish your work?
If it is done, Could you give the chance to talk about this?

Thank you!