Quaternion based attitude control #8003
Conversation
| /* calculate weight for yaw control */ | ||
| float yaw_w = R_sp(2, 2) * R_sp(2, 2); | ||
| using namespace matrix; | ||
| float yaw_w = .4f; |
There was a problem hiding this comment.
Yes, I can recommend you to read section 4.2 on page 16 of the paper I linked to about what this weight actually should represent and some comparison of resulting behaviours. I personally found 0.4 a good rule of thumb for a typical quadrotor setup in my past experience, that's why I put it there for the time being. I think this should be incorporated with the attitude yaw gain parameter in the end. Still to do.
It would be interesting where the current yaw weight calculation comes from and how it's justified from a control perspective. @bkueng told me already that it's done wrong and has his improvement suggestion here: https://github.com/PX4/Firmware/pull/7997/files#diff-81c4f50bdf837c4183b5d3cb81eb40eaR898
|
|
||
| /* calculate weight for yaw control */ | ||
| float yaw_w = R_sp(2, 2) * R_sp(2, 2); | ||
| using namespace matrix; |
There was a problem hiding this comment.
You can do this at the top for all of mc_att_control.
| /* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) | ||
| * also taking care of the antipodal unit quaternion ambiguity */ | ||
| Vector3f eq = 2.f * math::sign(qe(0)) * qe.imag(); | ||
| math::Vector<3> e_R(eq.data()); |
There was a problem hiding this comment.
Can you do this without mixing math libraries?
There was a problem hiding this comment.
I got asked this before in reviews but in my opinion the matrix library is like the name suggests no general math library. I see the matrix library covering linear algebra and the math library more general math functions like sin, cos, sign, expo. That's why I also added all my useful mathematical helper functions here: https://github.com/PX4/Firmware/blob/13e64d00a8b8afa7cedd201ee85842de4feeff6a/src/lib/mathlib/math/Functions.hpp
Let me know if I got something wrong there.
There was a problem hiding this comment.
I'm fine with the existence of the math library, but I don't like that we have multiple types of Vector. Vector3f is from Matrix and math::Vector<3> is from the PX4 math library.
There was a problem hiding this comment.
I totally agree! I, like everyone else I talked to, clearly favor the matrix library initiated by @jgoppert and that's why I contributed to it and look at math::matrix as deprecated.
Like in this pr I port every instance I come along to matrix. I also plan to get rid of the parameters:
https://github.com/PX4/Firmware/pull/8003/files#diff-81c4f50bdf837c4183b5d3cb81eb40eaR875
and outputs:
https://github.com/PX4/Firmware/pull/8003/files#diff-81c4f50bdf837c4183b5d3cb81eb40eaR872
in math::vector.
This is not to confuse with the call to math::sign which is a nonlinear scalar function that is defined in the math library because I felt it fits there best.
There was a problem hiding this comment.
I am bit confused as well. What is the reason for using math::Vector<3> instead of matrix?
There was a problem hiding this comment.
It's there to stay compatible with the rest while not replacing all data types.
But I see the consensus is that I should not only replace the attitude control functionality but totally switch the entire module to the matrix library.
|
just a question about the implementation order. |
|
Yes @Stifael you understood it completely right. Confusion: I think the only problem with your confusion was my imprecise phrasing with the name "full attitude control" with which I refered to the current algorithm effect and not the exact formulas written in the paper under the corresponding section. Sorry for that I've seen these terms for too long now and start to use them vague. I edited the description now. Setpoint improvements: We definitely should not only improve the control law but also the setpoint generation.
But these changes are bigger and could interfere also with fixed wing because of the required interface changes... We should tackle that step by step. |
|
confusion solved |
MaEtUgR
force-pushed
the
pr-quaternion-control
branch
from
8d4d3dc to
a4c99ef
Compare
MaEtUgR
force-pushed
the
pr-quaternion-control
branch
from
a4c99ef to
8ba90c5
Compare
|
Rebased, it still flies super nicely in SITL. I already wrote some scripts to do test calculations with the yaw adjustments and now I just need to port the correct implementation into the module. |
to prioritize yaw compared to roll and pitch by combining the shortest rotation to achieve a total thrust vector with the full attitude respecting the desired yaw not by scaling down the control output with the gains
MaEtUgR
force-pushed
the
pr-quaternion-control
branch
from
be1a86a to
83b5d32
Compare
Because the parameter does not make sense from a control theory perspective. Either you have a gain with the unit 1/s or an inverse gain or time constant with the unit s. But the time constant parameter was neither bound to any exact unit nor did it apply instead of a gain. Rather it adjusted multiple gains from rate and attitude control according to an arbitrary scale. This can only by accident lead to good tuning.
According to the paper the quaternion controller is built on the yaw weight represents the ratio between the roll/pitch and the yaw attitude control time constant. It also states that as a thumb rule a value of ~0.4 works alright for most multicopter platforms. The default attitude gains of PX4 which were determined independent of the paper from experimental results have a ratio of 2.8/6.5 = 0.43 which matches.
MaEtUgR
force-pushed
the
pr-quaternion-control
branch
2 times, most recently
from
8da9137 to
ee97dc0
Compare
- Delete left over identity matrix. - Corner case with a zero element when using the signum function: We always need a sign also for zero. - Corner case with arbitrary yaw but and 180 degree roll or pitch: Reduced attitude control calculation always rotates around roll because there's no right choice when neglecting yaw. In that small corner case it's better to just use full attitude contol and hence rotate around the most efficient roll/pitch combination.
|
@bkueng I took your branch and debugged how NaNs get produced.
The old controller just ignores the invalid setpoint and generates useless valid output and the new controller produces NaN in the same case. It's good that you found it, let's come up with a better general solution for this case. But it's not about the changes in this PR, they just reveal the problem that was always there. |
|
You're right. I think we should just initialize But there's another NaN case, which is due to numerical inaccuracies. See bkueng@e47017a, which contains a fix as well. |
|
@bkueng Yes, you're right, I found multiple corner cases I'm just too slow in writing, the one with numerical values exceeding the domain of acos adn asin I also found as well thanks to your analysis branch and fixed exactly the same way. Now I wanted to initialize the EDIT: With this branch https://github.com/MaEtUgR/Firmware/tree/pr-quaternion-control-loop-stop SITL stops the entire attitude control loop reproducable within seconds/some minutes when it's just idle. |
|
I did some flight testing with the added
I tested that branch and let it run for around 10 minutes, but didn't have any issues. Maybe it's just too many printf's to the console? |
While operating on exactly normalized float quaternions it can aparently still happen that one of the elements gets just slightly above 1 or below -1 and hence out of the domain of the acosf and asinf functions resulting in NaN. The constrain function uses stricly smaller/bigger comparisons and catches all tested cases.
|
@bkueng Thanks for all your super useful help! I just pushed fixes for the domain problem and initialization of the messages. I intentionally ommited checks for valid input quaternions on every loop iteration like MaEtUgR@a85c97c#diff-81c4f50bdf837c4183b5d3cb81eb40eaR947 because that's too much overhead and possible warnings in my eyes. What do you think? |
That's fine. Can you check CI, it fails due to style problems. |
- On initialization _v_att_sp got filled with zeros leaving invalid quaternions - While not armed mc_pos_control did not publish any attitude setpoint which makes no sense - The attitude control just uses the data in _v_att_sp if it was (ever) updated or not
MaEtUgR
force-pushed
the
pr-quaternion-control
branch
from
60ad986 to
8694819
Compare
|
Astyle on my windows toolchain passed... I need to check what's going wrong. |
|
@PX4/testflights Can you test this please? On different multicopters in Manual, Position & Mission. |
|
Couple flights on the Pixracer (V4): good flights; nice flight performance; no perceivable difference from master |
|
flight with pixhawk 1 V2. good flight, but i had some issues with baro. flight with pixhawk 2.1 V3. good flight no isuuses. flight with pixhawk 3 Pro V4pro. good flight, no issues. |
|
v3 pixhawk mini flight great. |
There was a problem hiding this comment.
This looks good now. I checked the CPU load, it marginally increases by ~0.05%, which is acceptable.
Let's wait for @RomanBapst to have a look at this as well, and then I think it's good to merge.
|
@bkueng I was expecting CPU load to decrease 😲 I actually concentrated to make all the quaternion calculations as efficient as possible... Probably this line https://github.com/PX4/Firmware/pull/8003/files#diff-81c4f50bdf837c4183b5d3cb81eb40eaR868 uses most cycles... but it's necessary to have linearisation for the yaw weight to have a correct implementation according to the paper. |
I've found the actual math of the controllers is a tiny portion. #8731 (comment) |
…term The meaning of the yaw weight changed with #8003: - before, the yaw weight decreased with increasing tilt angle error, so it was mostly 1 - now, it is constant and depends on the tuning gains (around 0.4 by default) It means that #8003 reduced the feedforward term, and we get the closer behavior as before with this change. It also reduces coupling between different parameters.
…term The meaning of the yaw weight changed with #8003: - before, the yaw weight decreased with increasing tilt angle error, so it was mostly 1 - now, it is constant and depends on the tuning gains (around 0.4 by default) It means that #8003 reduced the feedforward term, and we get the closer behavior as before with this change. It also reduces coupling between different parameters.
Introduction: I had the plan to port and contribute the quaternion based attitude controller I wrote for my master thesis running on PX4 for too long now. I guess it was a big mistake to wait for PX4/PX4-Matrix#34 to be merged which was pending for almost a year now. I was just too pigheaded for the incorrect quaternion convention because of a dramatic experience in the past. It's fixed and merged to PX4 with #7908 now and hence I started my work.
Documentation/Motivation: Here's a really good paper documenting the underlying math behind this type of attitude control which is very similar but still far from equal to the well known rotation matrix based algorithm by Kumar et al..
https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf
I'm also unhesitant to share my master thesis for further documentation. It contains a hopefully even easier to read in depth text description of my point of view and includes my findings. Just let me know if and where it should be hosted.
**State:**
I started now by writing and testing the general quaternion attitude control law which with the current setpoint generation should generate the same result as running only the full attitude control of the paper and does not have any notion of reducing yaw authority. This was a lot simpler to implement as a first step and should already allow unrestricted flying. I checked stable multicopter flight in SITL. I will add the quaternion based roll pitch reduced attitude control and the mixing with the yaw now to enhance performance in extreme conditions.Refactor attitude controller with matrix library (as suggested in review comments)EDIT: I did only some refactoring and I think we should split it up here and make a separate PR for further such changes to keep it more focused and reviewable.
Note:Be very careful with tests on real hardware right now.Context: This pr can soon be tested/compared with @bkueng's nice recent attitude enhancements:#7940
#7997
EDIT: fixes #9077