Change ecl controllers to not transform body angular rates

[scott-eddy]

Updated ecl_roll/pitch/yaw_controller.cpp to not transform estimator angular rate measurements to the body frame as these are output in the body frame by the ekf.

Also updated and explained the logic for coordinated turns in the ecl_pitch_controller, which currently has a comment "//xxx needs explanation and conversion to body angular rates or should be removed"

[LorenzMeier]

Awesome, very much appreciated! We did an internal review of the same code location this afternoon and came to the same conclusion.

[LorenzMeier]

The roll feed forward is required to prevent the nose from dipping down during turns. Its not "right" but "works" very effectively. Do you have a proposal how to retain this compensation?

[scott-eddy]

I'm going to be testing this with and without feed forward compensation this week as well. Even if feed forward compensation is necessary I think _pitch_ff should be applied as I'm guessing people will not think to adjust roll gains after seeing the nose pitch down or bad pitch tracking from log files.

[bansiesta]

So this is the same compensation as above but without _roll_ff param? Is the param not needed anymore to tune this?

[scott-eddy]

Correct, this is the same compensation above without the feed forward term though you will notice that it then gets transformed from a body fixed offset (e.g angular velocity) to an euler rate offset later. I'll be looking into the affect of the lack of feed forward this week, though theoretically it should not be applied here. _ptich_ff would of course still drastically affect the performance, as this is only adding in compensation pitch when the vehicle is rolling.

[scott-eddy]

For anyone interested I have run a few sims with the proposed changes. I am going to try to draw some conclusions from them with MATLAB analysis but here are the munched logs:
Stable branch performance with QGC noisy sim: http://dash.oznet.ch/view/3p2b9TahCMpLBR4uCwgdXD
Stable branch performance #2: http://dash.oznet.ch/view/QSs9VEgqUXd3f6y9TUBUah
Proposed ecl changes as is: http://dash.oznet.ch/view/326RiuxnW4RAp9tPm7VNJf
proposed ecl changes with pitch feedforward: http://dash.oznet.ch/view/Qpgj6GCXtupHjD7FVaAJXG
proposed ecl changes without jacobian: http://dash.oznet.ch/view/BhmhZUqXBHEWhwdyh8mQbJ
ecl changes without any jacobians (even on rate controllers): http://dash.oznet.ch/view/FavpdnEeQdtvekxE3KegAX

I know its a lot to look at, but I'm hoping data reduction will tell us more.

[LorenzMeier]

Thanks! Looks like the proposed ECL changes as-is reduce the jitter in the roll reference and have similarly good tracking as the version without jacobians. This is the opposite of what I expected. I have some idea why this could still make sense, but definitely worth more discussion.

[kd0aij]

I agree that the roll/pitch rates look less noisy in the runs with jacobians, but I don't understand what those transforms are supposed to do.
But something seems wrong with the pitch angle tracking at 15:15:29 in the run with pitch feedforward.

Also, it looks like the inverted flag should be set in the inverted branch of the roll constraint block for turn_offset to work correctly in ecl_pitch_controller.

[boosfelm]

Hi @scott-eddy Thanks a lot for the pr, it was definitely wrong to transform the angular rates from the ekf. I had a look at the data and it is tough for me to have a clear favorite, althgough personally I would go for your changes + pitch feedforward.
I fully agree that the pitch_ff does not belong here from an architectural approach, but I believe we have to account for different wing shapes (i.e. that loose more or less lift in turns) and this has been done with the pitch_ff param so far.
Else than that I was wondering why we do not move the whole compensation in turns into control_bodyrates and add it to body_rate_sp right away. Thsi way we would not have to transform back and forth or am I mistaking?
Cheers, Max

[RomanBapst]

No transformation happening:
Pitch controls

[RomanBapst]

Transformation happening:
Pitch controls

[RomanBapst]

I did a testflight flying the same auto mission with two versions:

1. the desired bodyrates are calculated by transforming the desired euler rates.

2. the desired bodyrates are approximated to be equal to the desired euler rates.

[LorenzMeier]

ok - so how is the attitude and rate tracking? Please just upload them to log muncher for a more complete picture.

[RomanBapst]

Log files:

1. Transformation happening:
   http://dash.oznet.ch/view/6hByj94YZepRetP8SAWzg

2. Transformation not happening
   http://dash.oznet.ch/view/Emp4J9yBYFE76WBorP5QEC

[RomanBapst]

@LorenzMeier Attitude tracking looks very similar.

[scott-eddy]

@Tumbili Thanks for actually flight testing this, unfortunately getting actual flights in for me is very difficult so I'm stuck to the simulation environment for most development. I'm doing some additional MATLAB processing on my logs and yours will provide some more insight.

Looking the aggressive roll case at 8:59:32-9:01:02 and 1:04:14-1:05:44 respectively, I see the transformation achieving better pitch angular rate tracking and the setpoint looks like it is slightly less noisy.

@boosfelm Interesting idea handling the turn offset in the bodyrate controller. I think this is a really good idea and I will run a sim with that logic. I'm hesitant to keep applying the _pitch_ff term to this turning offset. If a user needs a relatively large feed forward gain in order to achieve pitch tracking with no roll angle when in a turn they will now be applying that large gain they tuned for pitch tracking to a term they probably aren't aware of. However, I'd be willing to include it if the majority of devs think it best to keep it in.

[scott-eddy]

Okay, hopefully this will put the issue to bed. I will post images of the MATLAB graphs I generated below this comment and list the statistics here and finally follow with a conclusion. Sorry for being verbose but I suppose more info is always better than less
Looking at the roll cases (as this is when the turn offset matters) in @Tumbili flight data we can see that the flight with the proposed transforms perform better.

1. Stats for no transform:

---

mean pitch error std pitch error mean rate error std rate error
-7.0865 2.0411 -4.8092 5.5556
⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻
2)Stats for proposed changes:

---

mean pitch error std pitch error mean rate error std rate error
-5.589 1.5303 -0.7366 4.2659
⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻

Now, I then ran two simulations with the proposed changes as they stand and with the turn offset being handled in the rate controller. Here the sims suggest that handling the turn offset in the rate controller is better. This intuitively makes sense as you are no longer adding noise to the system by multiplying by the yawrate, but you are handling the offset as a body angular rate instead of as an euler angle rate

3. Stats for turn offset in rate controller

---

mean pitch error std pitch error mean rate error std rate error
-1.3083 3.506 -9.0343 14.0262
⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻
4)Stats for proposed changes as is:

---

mean pitch error std pitch error mean rate error std rate error
-3.6213 3.7519 -12.1105 15.206
⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻⁻

[scott-eddy]

---

---

---

[scott-eddy]

Conclusions:
I think we should handle the turn offset in the rate controller as this seems to have the best performance, though this is clearly not a make-or-break the system change. I also think that leaving the feed forward gain out of the turn compensation is fine and from a controls standpoint more correct.

I will push the changes with the turn offset in the rate controller if everyone agrees with this, I'd also be happy to wait to merge the changes until someone can flight test the new code.

Let me know if there are more questions or issues.

[LorenzMeier]

Sounds good. I'm very, very happy about the level of analysis done here, this is really pushing things forward. Happy to merge the changes as proposed.

[scott-eddy]

Okay, I will clean up some whitespace and comments and push the implementation with turn offset handeled in the rate controller after lunch. It will be up in less than one hour.

[LorenzMeier]

Rebased and applies, thanks!