Add LQG controller for rate mode and as attitude inner loop #2037
Conversation
|
Current findings thanks to @ufoDziner is that yaw doesn't work entirely right yet. On my quad it seems OK, on his new 6" one, it goes apeshit. Probably a tuning issue setting Q2 on LQR. |
glowtape
force-pushed
the
glowtape-lqg
branch
3 times, most recently
from
bf5775d
to
d9689b0
Compare
|
OK, I made it somewhat configurable via UAVOs and dicked around some more in MATLAB, and a tiny bit outside. But I changed the defaults since the last artifact. @ufoDziner Whenever you've a bout of boredom and want to test this one, the LQG configuration changed from the defaults in the build. Here's the current bunch of settings, which need to be changed in the UAVO browser: LQGSettings.RTKF.R = 250 LQGSettings.LQR.Q1 = 0.0001 SensorSettings.LowpassOrder = 0 🎉 🎉 🎉 Attention: Don't fly any other mode with LowpassOrder to 0, it'll rattle your motors to pieces. That includes LQG/LQG/Rate. Means, take off in LQG/LQG/LQG, and if it/yaw doesn't rattle the quad like last time, maybe do a bunch of simple manoeuvers (maybe even some flips and punchouts), and then land in LQG. If it rattles anyway right after take-off, abort, I guess. Then hook it up to the GCS and set LowpassOrder back to 1, so you don't get surprised the next time. I have to create a sidechannel for LQG to get the raw gyro data directly. If you want a stronger rate response, you can raise LQGSettings.LQR.Q1 to 0.0002 or 0.0004. |
|
Feels really smooth. Seems to me that it stops flips more accurately. Also tested with LQGSettings.LQR.Q1 to 0.00005 and Q2 to 0.001. Didn't really notice much difference good or bad. |
|
I think I'm going to patch in a gyro filter bypass later today. I'm just sitting at work bored right now and latently figured that your logs seem to be relatively devoid of noise on the regular gyro readings, which they shouldn't. Either you have a super smooth quad, or the gyro filter was accidentally on. Stacking delays probably doesn't help. Also holy shit, GitHub mobile is terrible. |
|
Also, with the bypass you could leave the filter running, so that it's safe to switch between LQG and acro mid-flight. Will probably help better for direct comparison. |
|
Keep in mind that my FC is mounted on rubber bobbins. They seem to smooth things out a ton. |
|
Wow, that seems like they do take out a lot of the noise pretty effectively then. On my logs, the gyros look noisy as hell with hard mounting. Now I'm wondering whether the bobbins have an effect on the bounce. However instead of taking them out, dropping the R value by a lot is an option. Like this, it'll start relying way more on the actual gyro than the internal model. I've already suggested R=100 on IRC, maybe 25 or less is an option. I have to run these through MATLAB this afternoon. |
|
I changed to hard mounts on a different build to test if the bobbins had an effect. Didn't seem to stop the flips any more accurately with the hard mounts. I haven't seen any negative effects with the bobbins. |
|
Okay. Dropping R is however still worthwhile to explore, if your quad is relatively noise free regardless, to correct mispredicts. In the logs, the estimate of the RTKF got ahead of the gyro reading on the huge swings of the flips, both starting and stopping. It seems to think that your quad has more power than it actually reacts with. That said, I have to figure out why it is happening for you. |
|
This was reported via IRC, but figured I should add it here as well. The biggest thing I noticed is that it's like the quad is in a soft leveling mode. It tries to level itself sooner than in acro/rate mode. Also the roll/pitch rates seem a bit more linear in acro, whereas this seems to have a steeper expo curve. Still flew smoothly though. |
is CG low on the vehicle (e.g. do you have an underslung battery?) If it's tending towards self-leveling with a low battery, it means that long term drift isn't being combated effectively. |
|
@mlyle |
glowtape
force-pushed
the
glowtape-lqg
branch
from
df9e1a6
to
76d07ef
Compare
|
Updated the LQG default settings based on experimentation. The current set is: LQGSettings.RTKF.R = 100 These changes will make sure the actual gyro will be considered more. Maybe this helps reducing the wandering (I have not yet had it, so I'm guessing here). Especially the process noise for bias got raised significantly, which should help more fighting the autoleveling. (The problem with the RTKF is, there's a point where the estimates get too noisy and cause more drama than necessary, due to how aggressive the LQR can react to it.) LQGSettings.LQR.Q1 = 0.0001 These changes for one disable the yaw fudging that was introduced because our autotune doesn't evaluate a direct torque beta that the original math wants. Seems to work fine on my quad, and still causes less of a ruckus than #2019. Also, the Q2 weight got raised, which will try to drive torque to zero some more, but not so much that it impacts performance noticeably. It should also keep the yaw axis under control some more. (I was having a bunch of roll axis oscillations, but they could be easily reproduced with the PID controller in the same situation, so it seems the axis estimates aren't that correct.) Not spinning a new artifact, since these are UAVO changes only. I was considering LQI, but the matrices go from 2x2 to 4x4, which raises the computation time way too much. And it looks like it doesn't really do much more than what the RTKF spits out. |
|
Separate sensor noise value for the yaw axis. Allows to drop R for pitch and roll considerably, so that even more gyro applies to the estimate. |
|
So about LQG as inner loop for attitude. This one is tricky. Situation is this, I've the default 55° on roll and my P-term on the outer loop is 7.24, and the I-term is 4. Initially, assuming I hammer the stick fast enough, the quad is supposed to reach around 400°/s. The main actuation from the inner loop seems like like 50ms-ish, so I figure the outer I-term doesn't do anything worthwhile. The initial impulse from both loops seems to be just as strong with the current settings, the slopes overlap pretty nicely. However, the PID-loop actually reaches the 400°/s, whereas the LQG-loop only goes to around 320-330°/s. But... Considering the AttitudeActual starts moving, the proportional term starts pulling back. Around when the LQG stops accelerating, the attitude is at least 5° from level (should have screenshot it), maybe more since it may be a smidge behind due to logging AttitudeActual at only half the logging rate here currently (assuming it is a reason for delay, anyway). Anyhow, when LQG stops accelerating, the rate demand would be around 360°/s or a little less, because the error would be 50° or a little less instead of 55°. On the first bit, PID is a tiny bit faster at reaching the attitude, then it crosses over and LQG keeps reaching target attitude faster. The argument that could be made that LQG is more accurate at following the outer loop's rate demands, while PID rides on overshoot at the very beginning, but then it swings back and hinders the outer loop. Two things, I did hammer the stick, not put it on momentary switch, and second, it was windy. White is PID, yellow is LQG, top gyro, bottom attitude. Tried to find events matching as close as possible. The LQG pulls back sooner compared to PID every time, on either side of the axis. |
|
Flew a couple of packs on this build today with a Bolt RC Kraken Worx 5". Wandering is gone. It's more floaty than PID in turns. Also, in high speed high power turns there is a high frequency oscillation. The LQG control rates also feel a bit more linear, so they're more aggressive near the center stick than I prefer. Major improvement in flyability over the last build. Still needs work for sure, but it's definitely going the right direction. Great work! |
glowtape
force-pushed
the
glowtape-lqg
branch
from
479cdf0
to
d6fe08d
Compare
|
Added RTKFEstimate UAVO, which dumps what the estimator thinks. Adjusted defaults some more after testing today. Now for the fun part, this is gonna be long. I've noticed the motors going kind of gung-ho on aggressive maneuvers, some sort of grindy noise, which I always thought was noise caused by aggressive braking and acceleration (to be honest, after further testing, it sounds almost exactly like the yaw rattle on low throttle drops). Attitude on LQG testing kind of made me rethink that. The estimator works like that: Update state estimate based on gain matrix, then add control input from the last loop cycle to it. The wrinkle in this is that RC updates at a fraction of the rate than the stabilization loop runs, so RC commands look pretty much like a stair to the control law. If you swing the stick fast enough, the steps are pretty damn high, and in return, the initial error on value change is just as high. This causes the control law to demand instantly a huge change from the actuators. When you log ActuatorDesired, and the command rate is low enough or the logging rate is high enough, you can actually see it manifest as saw tooth pattern on the PID. Means it is pulsing the motors speeds, so long prop inertia allows it. Now that the estimator and the LQR are in the loop, the unit delay causes some issues. The huge setpoint change creates a huge control demand, because of the rate error, to which the first LQR coefficient reacts. We assume nothing much is happening on torque (yet/anymore). Then the next loop, the estimator gets updated with the huge control demand, which creates a huge peak in estimated torque, to which the second coefficient of the LQR suddenly reacts and creates a huge opposing control demand (torque is always driven to zero, so it counters the rate part). That demand again influences the estimate, which again futzes with the controller response. Creating some sort of short feedback loop that dies down relatively quickly. The torque is pretty much a differential term just like the D in PID. On the PID controller, we filter the D-term to avoid similar issues. While it's probably worthwhile to investigate, I'm not too fond of sticking a filter on the relevant part of the LQR, because both coefficients seem relatively finetuned to each other. The Kalman filter itself has a filtering function, but solely on the gyro input. It also tries to predict the rate change on estimated torque. The control input is what upsets it all, because it adds to it unfiltered. The alternative is smoothing the control input. There's been a version of it in the firmware already, but deactivated by default. That version however is still stair-steppy, however literally takes the edge off of the outer corners of the stairs, by chamfering them. That however doesn't seem enough. No smoothing, normal and extended all cause the rattling motors here. I've added most of #1950 to this, and with either Linear or Psychic mode, the rattling is pretty much gone. Either when doing huge stick swings in Attitude on LQG, and as well as entering and exiting flips and such in LQG acro. I kind of suspect that the unsmoothed control input potentially also causes drama during hard FFF maneuvers, because the motors are getting held up in short bursts of grinding instead of properly doing their job, when doing fast and large stick motions. I've enabled Psychic by default in this PR, because it served me pretty well in Summer 2017 with no noticeable issues. It uses the sort of predicts the existing Extended mode creates, but then interpolates between them. The predicts are to lower delay by the interpolation. It has theoretically the capability to overshoot, but from all logs I've looked at, human aren't able to swing or stop the sticks fast enough to create a hard edge, so this shouldn't be an issue. Linear mode just interpolates by trailing the control signal, this is the safest option, if you feel funny about the other mode. @ufoDziner If you're going to install this, export the UAVO before flashing, then after flashing, reimport it. The stabilization settings get trashed due to small changes to the UAVO, reimporting the settings fixes them. Then reset the LQGSettings and make sure at least StabilizationSettings.RCControlSmoothing.Axes is set to either Linear or Psychic. You can do that for thrust, too, but the LQG isn't really concerned with that. And I suppose using the Custom logging profile would be nice. It does 250hz with just +12% CPU. (--edit: The control smoothing also affect PIDs.) |
|
This latest bunch of settings come from experimentation. I'm going to freeze them for a while. They give generally PID like performance on my current acro quad and the 3S crapquad of mine, and don't cause any ruckus on large stick throws. For whoever wants to give this a try. |
glowtape
force-pushed
the
glowtape-lqg
branch
from
a0532e8
to
a04a8c0
Compare
|
Rewritten history to compact all that mess, and rebased. |
|
Current settings seem to work generally fine across the whole throttle band over here. Found that hangtime doesn't really like them. It also creates a rebound, but that's more like a fast jerk. |
|
I just posted them like that to not force push things again. If everything is A-OK, I will squash them into the appropriate commits, before merge. |
|
Going to add optional control term filtering on the RTKF, based on either global lowpass delay or tau. Since the whole shebang is a first order system, I think it might help, if we did something like that on the control term. If there's a huge setpoint change, there's going to be a huge instant control demand, and if it's fed as is to the RTKF, it'll leap ahead in its estimate accordingly (because it adds torque directly scaled to u), even though the actual system still needs to get moving (tau et al). Visible for instance in this graph posted a while ago. Purple is the RTKF, note how the slope is always ahead of actual gyro:
|
|
Seems like I need to work some more on the default settings. When things seem really off balance for whatever reason, the torque estimate can go way overboard and make things terrible. Here's an example. Top of the graph is @ufoDziner's problematic quad with T5050C props, doing flips on default RTKF weights. Bottom is the same quad with the Q2 (process noise on torque) bumped from 0.000003 to 0.00001, i.e. 3.3x. Notice the cross-axis wobbling after flips, and the wobbling at the top rate of flips. Severely reduced.
(In the logs, there's still cross-coupling to the yaw axis, though. But it looks way saner.) I need to find another set of weights and costs for the LQG, that improves the torque estimate a bit more than above, while still maintaining the noise advantage to some degree. The Q2 bump was cross-checked on another quad, things stayed good over there. Noise magnitude of LQG vs. PID was 50% on the eye. |
|
Any objections to merging this? |
|
Not objection from me. It's great for most of my flights. In fact it's the only flight mode I'm using now. Only anomaly is the weird back flip wobble with blheli_32 and aggressive props. It's not horrible, but there on my builds anyway. |
|
Apart from that one specific quad from ufo, the few guys who tried it, the experience ranged from neutral to mildly positive. I'm gonna squash the fix commits later today. The tricopters worked with surprisingly without much issues. Whenever this one got merged, I'll run a quick optimization on the LQR covariance. The A11 term in the A matrix is always zero, the generated code doesn't consider that (doing so would save a bunch of multiplications and additions). I don't expect it to change anything, but it's a bit risky to fold it into this PR. |
|
Squashed some stuff to reduce commit history size, verified things still work practically. |
|
Ack dude, don't rewrite history right at the point I'm considering merging it. It means I can't tell what's changed! |
|
Skimmed diff from 53e3743 |
|
Whoops. Thought the review was over with the consideration of a merge. I can push the previous version again, if that helps. |
|
Nah, it's OK, I've re-read. Rewriting history is great (makes git log prettier, makes the repo use less space, rebases are better than merging back and forth), but it means that the ability for other people to compare and make sure the right things go in goes away. It's not always possible, but it's much better if it happens before review (especially for big nasty things). |
mlyle
changed the title
Posted for @ufoDziner and anyone else that is brave.
LQG stabilization mode added. Need to configure a Stabilized1/2/3 with LQG per axis, it isn't added as flight mode currently. It is a rate controller only. Tried LQG/LQG/Rate outside and LQG/LQG/LQG inside, both work.
The LQR state weighting matrix is currently fixed to experimental values. My test platform is a 5" miniquad that currently tunes to 10.6-ish beta and ~14ms tau, and it gets a proper solution there. Anything similar to that should fly somewhat OK I'd figure.
I touched the ManualControlSettings UAVO, so if you flash it and boot it, the settings are gone, so export them before hand to reimport them later.
--edit: Also, you need to run autotune, so that SystemIdent fields get filled in. Adjusting Previous Tune is OK, if nothing changed on the quad.
--edit: Might also be an idea to smooth the RC signal. Done by setting via UAVO browser, StabilizationSettings.RCControlSmoothing.Axes/Thrust to Extended (setting I've been testing with.).