Inav: Quaternion based level PIDs

[HaukeRa]

As from a discussion from cleanflight:

There are benefits when using solely quaternion math for attitude calculations and level PIDs, since they avoid the singularity (gimbal lock) of the Euler angle representation.

Even though these problems are not obvious in everyday usage, it's better to avoid them from the beginning on.

I'm willing to invest time in this, but help is much appreciated!

This issue serves as an initial discussion thread.

One problem that arises is the stick mapping when allowing 90° stick mapping, roll will become yaw for example, and pilots need to discuss if this is intuitive.

[DzikuVx]

272

[digitalentity]

@HaukeRa thanks for joining!
I think we can live with roll/yaw swap, I have a feeling that quaternion representation of orientation will hangle heading correctly (using roll in this case).

[HaukeRa]

I will start setting everything up tomorrow.

And I think it might be possible to implement the quaternion PIDs in Fixed point math. Not sure about the IMU itself. But the quaternion representation guarantees that each element is in the range of -1 to 1 (ideally) so that scaling it to fixed point in high resolution is viable.

[digitalentity]

@HaukeRa cool! Don't think about optimizing this yet, lets make it working using float point math. We can think about fixed point math if we hit performance issues.

[digitalentity]

@HaukeRa how is it going with Quaternion self-leveling code?

[HaukeRa]

Sorry I didn't have as much free time due to work as I thought I would have.

I will work on this over the christmas break!

[digitalentity]

No worries!

[HaukeRa]

FYI: I hacked together a first working version of a PD controller in matlab/simulink just as a proof of concept. Seems to work!

[digitalentity]

@HaukeRa cool! Any change you can show the matlab code to get the idea?

EDIT: I'm trying to get to know quaternion math better, but so far its all somewhat magical for me :smile:

[HaukeRa]

Sure!
Integral controller is also working now.

I'll just so some final testing this evening.

Currently it's written to be easy to understand and easy to work with. There is lots of headroom for optimization. Also there is need for some sanitizing of state variables, as quaternions need to have unit norm, and if they don't calculations are likely to fail.

And don't worry, Quaternion rotation math is just a very small subset of quaternion math!

All you need to know is, that a quaternion represents anot arbitrary rotation. Then if you know how to multiply them you can build a PD-controller. Thenot if you know to calculate a fractional power you can calculate a PID-controller.
It's definitely worth playing around with!

[digitalentity]

@HaukeRa being able to maintail an arbitrary angle even if it's a gimbal lock for Euler angles would be a major breaktrhough - it will open a whole new world of possibilities including tail-sitter VTOL support. Thanks for what you are doing!

[HaukeRa]

Still working on it, but the I-controller still faces some issues.
It tends to wind up on certain control inputs, I hope I can solve this without an hard limit. It performs well in an static environment at least! :D

Current state: https://drive.google.com/file/d/0BxJvibam28UTcEhWTG9aS2ViWkk/view?usp=sharing

Issues:

• When error goes to 180° its impossible to determine which way it will choose to go into the target orientation. This can seen in the video when the vehicle makes a flip instead of rotating back. In my simulation I use -90° and 90° as setpoints, which triggers this. Usually this shouldn't happen in a real scenario, since the inputs won't jump that drastically. But this could be solved by implementing some sort of "planned path", some sort of attitude waypoints.

• When giving rotating control input, the "i-sum-quaternion" always laggs behind, means it will rise constantly. Not sure how to solve this currently.

[digitalentity]

My initial idea was to keep PID as it is now - separate by RPY axis and use Quaternion P-controller only to do self-leveling. Maybe that's the solution?

Overall - I like the progress! Ability to maintain 90 deg attitude is sooo nice, no more weird rotations when leveling from inverted position - quaternion math will always use shortest path

[HaukeRa]

Ah sorry if that wasn't clear, that's exactly what I'm doing. But I implemented P I and D for the leveling. You need I if you want to combat gyro offset/drift.

[digitalentity]

I suggest we go this incrementally - currently it's a plain old P-controller for leveling. Let's just convert it to quaternion logic as a first step and improve it later. What do you think?

[HaukeRa]

Sure, p-controller is the easiest and most robust one. Is there a quaternion IMU in the codebase somewhere already?

[digitalentity]

Yes, imu.c. It's just that orientation quaternion is internal and is not exposed. I'll start a branch and refactor that a little bit - you can base your implementation on that branch.

[HaukeRa]

Great, I hope I can implement the P-controller within one day then.

[digitalentity]

@HaukeRa I've created #1077 and exposed computed orientation quaternion as orientation global.

Self-leveling PID is implemented in pid.c in pidLevel function, but it is working per-axis, so I believe you'll have to considerably change it.

Also, heading hold is handled in a completely different way. Is there a possibility to ignore yaw axis in your controller?

[bk79]

When calculating yaw do not use 0-360 but use radians now since for small
angles sin(x)=~x when calculating orientation use sin(sin(x)) and
cos(sin(x)) since sin() is a continous function with no discontinuity

Il 11/gen/2017 02:14, "Konstantin Sharlaimov" [e-mail] ha
scritto:

@HaukeRa https://github.com/HaukeRa I've created #1077
https://github.com/iNavFlight/inav/pull/1077 and exposed computed
orientation quaternion as orientation global.

Self-leveling PID is implemented in pid.c in pidLevel function, but it is
working per-axis, so I believe you'll have to considerably change it.

Also, heading hold is handled in a completely different way. Is there a
possibility to ignore yaw axis in your controller?

—
You are receiving this because you are subscribed to this thread.
Reply to this email directly, view it on GitHub
https://github.com/iNavFlight/inav/issues/734#issuecomment-271749348,
or mute the thread
https://github.com/notifications/unsubscribe-auth/ALSWijkqRlld1PVT_3QCkxDmAkbE0oPVks5rRC0CgaJpZM4KlCX0
.

[HaukeRa]

@digitalentity

There is no possibility to handle a specific axis seperately, the controller just does not care/know about axes.

Stick inputs need to be interpreted completely new, when allowing for 90° tilt. Heading hold needs to go into the IMU, Just enable mag input into the IMU and disable it if you do not want heading hold.

So the magic here is how to translate stick inputs into reference orientation quaternion. This of course needs to be done differently when implementing some thing like headless mode etc. I'll have a look at the codebase now.

[HaukeRa]

But YAW input will be handled by applying a rate quaternion to the reference orientation quaternion. This rate quaternion will only apply a rotation around the Z-Axis, depending on stick deflection.

[bk79]

You can apply the same on any axis

Il 11/gen/2017 11:24, "HaukeRa" [e-mail] ha scritto:

But YAW input will be handled by applying a rate quaternion to the
reference orientation quaternion. This rate quaternion will only apply a
rotation around the Z-Axis, depending on stick deflection.

—
You are receiving this because you commented.
Reply to this email directly, view it on GitHub
https://github.com/iNavFlight/inav/issues/734#issuecomment-271832529,
or mute the thread
https://github.com/notifications/unsubscribe-auth/ALSWin1KIBUG7mbexs9oiqtmPyAirrLeks5rRK3wgaJpZM4KlCX0
.

[HaukeRa]

Sure, that is possible, but then there would be no need for a level controller, because you would be at a rate controller again, which would make the level controller more or less useless.

[HaukeRa]

Stick input mapping:
Pitch and Roll:
Lets assume that full pitch stick input is linked to a 90° rotation of the aircraft. How should the stick behave on full pitch AND roll input? In a straight forward solution this would give a rotation greater than 90° (aircraft would start to face downwards)

Two options:

• Limit square input space ( +-500 on each axis) into circle, this would leave some dead space in the very extreme corners
• Map square input space into circle (no dead space, but in theory stick input is more sensitive on a single axis compared to the diagonal by a factor of sqrt(2) )

Yaw:
As this controller is unable to just ignore an axis, it will always run in some sort of heading lock mode. One way to define heading would be to use integrated stick input in earth's frame as a reference. Drawback is that if the aircraft flips, yaw direction will stay the same, so won't flip with the vehicle, could be unintuitive, not sure. Positive side: Even after flips etc. Heading would always be the same again. In general I would see this as the navigation solution.

Another option would be to try to define yaw in the aircraft's frame. This is closer to the current solution. I'm not too sure about the implications after flips, or yawing when tilted etc.

In general its more or less a question of order of applying the rotations.

Apart from that the code is ready but completely untested, and in ANGLE mode always overrides all targetRates, which means MagHold etc. are unfunctional.

[digitalentity]

I think if we move forward with this a lot of design choices would have to be made.

I'm not against having heading always locked, but we need to have an option to simply define it in aircraft's frame. In general - this is a good solution for navigation modes, but we'll need to consider other modes and vechicle types as well.

I.e. a flying wing does turns by banking. Heading hold is meaningless for this type of aircraft - it doesn't have rudder to control yaw. So basically yaw stick input will have to be ignored.

Is it possible to extract yaw rotation from current orientation quaternion and substutute it into desired orientation quaternion? If it's possible - it will be a viable solution - we'll be ignoring yaw conpletely and leave only roll & pitch. Does this look like a viable solution?

[HaukeRa]

I didn't tackle the yaw issue yet.

But you can see the curren state here:
https://github.com/HaukeRa/inav/commit/19af68bc6d5a3fb7bfd226a553687156028900d8

As you mention other types of aircrafts: In my oppinion this needs to be done in the mixer. The controller just says: apply velocity around z-axis to yaw right. How the vehicle does this doesn't matter, even if it needs to pitch and roll for this.

[digitalentity]

Probably you are right. Anyway, this is going to be a huge change in the code, so I'm scheduling this for version 2.0 (just to keep it assigned to some milestone)

[HaukeRa]

018eb9abf49797f270ec6db144518f337fecad7e

Do you have a small 250 size test quad? I currently only have a 450 which is not very good for testing (has camera gimbal etc.)

This version is at least dry tested without props and should do the trick. It will always YAW north so start it facing northwards or without MAG.

[digitalentity]

I'll try to do some table-testing before putting this on a real machine.

Code looks so simple at the moment :+1:
Correct me if I'm wrong - I won't have any yaw control at the moment, right?

[HaukeRa]

Right, no YAW currently. So always setup your machine facing north!

Im still figuring out how to implement yaw control in a convenient way

[digitalentity]

Ok, will try to do some table-testing and flight-test in the following days.

[HaukeRa]

Nice, also check 77ab3427659006e655d26119997dfd0c46f22a4e as I accidentially left in a hard coded max_angle_inclination of 90° which is probably not that easy to fly and test.

Also I addressed your remarks to the last commit,

[digitalentity]

@HaukeRa not working. Results:

1. It appears to be doing self-leveling correctly
2. It slowly yaws to one side - probably according to yaw gyro drift
3. Roll & Pitch sticks are mixed up in a weird way
4. Stick input is only accepted if both Roll & Pitch are moved at the same time
5. When doing both Roll and Pitch board also tend to do some yawing

All the tests were done on the tabel and don't involve actual flying

[digitalentity]

https://github.com/iNavFlight/inav/blob/77ab3427659006e655d26119997dfd0c46f22a4e/src/main/flight/pid.c#L157
The code is only executed if both Roll and Pitch are moved at the same time

https://github.com/iNavFlight/inav/blob/77ab3427659006e655d26119997dfd0c46f22a4e/src/main/flight/pid.c#L160-L161
Roll and Pitch are mixed up

[digitalentity]

This code seems to be providing correct results on the bench. Still some yaw when doing both pitch and roll though - probably due to the fact that yaw is calculated in Earth frame.

  if(rcCommand[FD_PITCH] != 0 || rcCommand[FD_ROLL] != 0){
      fpAxisAngle_t rollPitchCommand;
      // Cross-product between stick and z-axis
      rollPitchCommand.axis.V.X = -rcCommand[FD_ROLL] / 500.0f;
      rollPitchCommand.axis.V.Y = -rcCommand[FD_PITCH] / 500.0f;
      rollPitchCommand.axis.V.Z = 0;

[HaukeRa]

That happens when you do things in a hurry without bench testing ...
if(rcCommand[FD_PITCH] != 0 || rcCommand[FD_ROLL] != 0){

That is really strange:

  rollPitchCommand.axis.V.X = -rcCommand[FD_ROLL] / 500.0f;
  rollPitchCommand.axis.V.Y = -rcCommand[FD_PITCH] / 500.0f;
  rollPitchCommand.axis.V.Z = 0;

Is of course right. When you want to pitch, you need to move around the Y axis. I was confused by the index, as I treated the input vector by index (roll, pitch, yaw) and then performing the cross product by the z-axis unit vector. I'll bench test this now and add yaw support.

[digitalentity]

Fortunately I bench-tested before flying :smile:

[HaukeRa]

But hey, its flying! That means that the controller itself is actually working!

[digitalentity]

Anyway, it's good to see new level-controller produce roughly the same rotation rates for the same stick input on roll/pitch axis as the old one. It's far from being ready for prime-time, but that's definitely a good start! Thanks for looking into this!

[HaukeRa]

Actually, on single axis it should behave exactly the same as the old one, thats why I also re-applied the same P-scaling factors as for the old controller.

I also bench tested this to work up to 90°. In theory it should even work above that. Should I implement max_angle_inclination for roll & pitch seperately as before? Could lead to unexpected behaviour if they are not symmetrical at high angles.

The general problem is: Having a controller that does work even in gimbal lock situation gives you the trouble of facing gimbal lock on mapping the control input 😄

[digitalentity]

We have separate inclination limits for airplanes so we need that option. Unexpected behavior at high angles should be expected and I don't think we should bother about preventing it. I'm not expecting people to have 100/200 deg limits on roll/pitch :smile:

[HaukeRa]

Implemented YAW as heading hold in earth's frame here: 7604eb586a5131744fe47a5b168ef108f632bf0f (bench tested this time!)

But still haven't flown this.

[HaukeRa]

@digitalentity I have one strange observation though which I currently can't explain:

When I arm and apply yaw while on the ground motor outputs will go up as intended until hitting the maximum configured rate. That results in a motor output of about 1650. But as soon as I move the yaw stick (doesn't matter if left or right) the motor outputs will go all the way up to 1850, if I release the stick, it will go down to 1650 again.

I overwrite all angle mode rateTargets so that can not be the cause.

Is there some code that scales either p-gain or rate limits depending on yaw stick input? I didn't find anything yet.

[digitalentity]

@HaukeRa that's the yaw_jump_prevention_limit in action - it increases yaw authority when stick is not centered. So this is normal behavior and that's not P-gains or rate-limits - that's mixer.

[HaukeRa]

@digitalentity good to know, thanks for the clarification!

[HaukeRa]

I needed positive stick to axis mapping for correct handling:

 rollPitchCommand.axis.V.X = rcCommand[FD_ROLL] / 500.0f;
 rollPitchCommand.axis.V.Y = rcCommand[FD_PITCH] / 500.0f;
 rollPitchCommand.axis.V.Z = 0;

But now I have flight tested (indoors) this. yaw works as intended! Would be happy to get feedback from others, too!

Should I port the I- and D-controller from my matlab codde, too? But I saw the old level controller had just a p-controller.

[digitalentity]

@HaukeRa sorry, totally forgot about this issue. With 1.6 out I think I'll start testing this code - not sure for quads, but airplanes will benefit a lot from it.

[HaukeRa]

No problem! Let me know if I can do anything.

[digitalentity]

Finally taking steps towards implementing this #2894. @HaukeRa your'code comes in very handy.

[HaukeRa]

Great to hear that it is put to some use 👍