IMU - increase gain on large Course over ground error

[ledvinap]

Test increased gain when CoG error is extreme (> 90ged error)

Untested, intended to improve RTH mode

[github-actions]

Do you want to test this code? Here you have an automated build:
Assets
WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

[ledvinap]

It is possible (but unlikely) that I got some sign wrong. Should be safe to test in normal flight (heading should be corrected according to GPS). Only then test with RTH, it may cause flyaway otherwise,

[ctzsnooze]

I'll test as soon as I can - thank you!

[Quick-Flash]

Test increased gain when CoG error is extreme (> 90ged error)

Untested, intended to improve RTH mode

This will make flying backwards more likely to make things wrong. It would help once flight is forward during a GPS RTH, but during flight this has the possibility of making things off even more as during normal flight we have no idea which direction the drone is actually pointed in.

It should be possible to estimate the direction that thrust should be moving the aircraft in and use CoG to not only assume the drone is moving forwards. For instance if we know our attitude is pitch backwards we can make the CoG correct things so that it knows its pointed backwards. Essentially we should be able to get the direction that the drone is tilted in and use CoG to correct for the direction that the drone is tilted in, rather than correcting it towards forward flight. This would likely just require one extra matrix rotation to make work. I'd have to think about it more to figure out how you would implement it though.

[ledvinap]

@Quick-Flash : Flying backwards will have equal gain as flying sideways. Both are bad.

It should be possible to estimate the direction that thrust should be moving the aircraft in and use CoG to not only assume the drone is moving forwards.

This is quite easy. Just change heading_ef to projected Z axis

const float heading_ef[2] = { rMat[0][2], rMat[1][2] };  // body Z axis. Projected vector magnitude is increased as tilt increases, zero when quad is level

But I'm not sure about BF coordinate systems, maybe it will align heading to wrong direction. It is safe to test as long as RTH is not entered.

[ctzsnooze]

@ledvinap - hi!

I made a PR that lets us investigate exactly what happens to a rescue at different IMU yaw GoG gain values, since I have been struggling a bit to learn exactly how much IMU gain change should be applied, and when.

In normal flight I am becoming convinced that it makes sense to keep IMU gain zero when pitch angle is zero, and to increase the gain in proportion to the pitch angle. I have done this in the descent phase of GPS rescue without any issues. The gain factor seems not very important, but needs investigating, that is all. I anticipate that a small change like that would greatly reduce the chance that we could acquire an IMU error by pilot action in the first place. It will also reduce the rate at which an IMU error could accumulate from drift while flat, or while holding sideways roll input with zero pitch input.

However, I do not want to propose that fix right now, not at this point, anyway, because it is useful to be able to readily generate a 180 degree IMU yaw error (by flying backwards for a period of time) to help us test how best to handle that error in the rescue setting.

In the case of a pre-existing 180 degree IMU yaw error, in the GPS rescue situation, as we correct the IMU error, the yaw heading changes, which then influences the limited means we have to detect the error in the first place.

Too much IMU gain in this case can result in a circling behaviour, particularly if the available pitch angle (and hence available forward speed) is low, and the drift is so high that making headway against the drift is difficult.

Too little IMU gain results in such a prolonged correction that the quad can fly away for hundreds of meters and get a sanity failure or crash into nearby taller objects

So I made a PR that lets us manually configure the IMU gain, so that we can test what happens, exactly, at different IMU gain values, in the presence of a 180 degree IMU error, and at different pitch angles (early in the rescue the pitch angle will always be half max value in these cases) or other situations

Here is that PR:

#12853

I am hoping that once we get some greater clarity on exactly when to increase the IMU gain, and by how much, that we can get a more optimal solution than those I've proposed to date.

[ledvinap]

I made a PR that lets us investigate exactly what happens to a rescue at different IMU yaw GoG gain values, since I have been struggling a bit to learn exactly how much IMU gain change should be applied, and when.

It may be worth playing with SITL a bit. It can provide great testing environment

In normal flight I am becoming convinced that it makes sense to keep IMU gain zero when pitch angle is zero, and to increase the gain in proportion to the pitch angle.

With zero pitch angle (or, better, with zero tilt), you have no information about quad direction. So IMU will only track noise, diluting orientation information it has.

I have done this in the descent phase of GPS rescue without any issues. The gain factor seems not very important, but needs investigating, that is all. I anticipate that a small change like that would greatly reduce the chance that we could acquire an IMU error by pilot action in the first place. It will also reduce the rate at which an IMU error could accumulate from drift while flat, or while holding sideways roll input with zero pitch input.

It is easy to handle roll/combined input. Just use thrust vector projection into ground place as reference direction for IMU (it is in comment of my PR).

However, I do not want to propose that fix right now, not at this point, anyway, because it is useful to be able to readily generate a 180 degree IMU yaw error (by flying backwards for a period of time) to help us test how best to handle that error in the rescue setting.

Another option is to use some switch to set yaw direction immediately. It will cause some transient, but that shall be quite minor. Maybe Headfree orientation reset? It will make testing much easier.

In the case of a pre-existing 180 degree IMU yaw error, in the GPS rescue situation, as we correct the IMU error, the yaw heading changes, which then influences the limited means we have to detect the error in the first place.
Too much IMU gain in this case can result in a circling behaviour, particularly if the available pitch angle (and hence available forward speed) is low, and the drift is so high that making headway against the drift is difficult.
Too little IMU gain results in such a prolonged correction that the quad can fly away for hundreds of meters and get a sanity failure or crash into nearby taller objects

One possibility to handle this would be to track GPS CoG/direction separately from Yaw and mix it into IMU yaw only slowly and when reading is known with good confidence. RTH code can then set desired direction based on GPS, without affecting other code too much.

So I made a PR that lets us manually configure the IMU gain, so that we can test what happens, exactly, at different IMU gain values, in the presence of a 180 degree IMU error, and at different pitch angles (early in the rescue the pitch angle will always be half max value in these cases) or other situations

[ledvinap]

Ok, now it seems to work as intended.
Unittest coming soon

@ctzsnooze : Can you try it? I hope it will not fail catastrophically

[ctzsnooze]

@ledvinap Thanks! I'll cherry-pick into nee to test since it has the needed debugs.

Can I ask you to consider how we might most effectively attenuate the imu yaw to cog gain factor in relation to pitch angle?

I am doing this during a rescue with a line like this, where I subsequently multiply the yaw to COG gain by pitchForwardAngle :

pitchForwardAngle = (gpsRescueAngle[AI_PITCH] > 0.0f) ? fminf(gpsRescueAngle[AI_PITCH] / 3000.0f, 1.0f) : 0.0f;

pitchForwardAngle is zero for negative GPS rescue commanded pitch angles, 1.0 at a commanded pitch angle of 30 degrees, etc.

The idea is simply that if the quad is pitched forward, then the probability that the course over ground represents the attitude of the nose of the quad increases in proportion to the magnitude of the pitch angle of the quad.

I am sure you appreciate that the above code is a fairly primitive solution, but it appears to do no harm :-) and probably does do useful things during the fly home phase and the descent phase of the rescue.

Hence I was wondering if you could perhaps propose a better way to implement this more generally, eg from the point of takeoff. Probably as a separate PR.

The IMU orientation / OSD Arrow would then only relatively quickly point to home if the pilot takes off and pitches forward at some reasonable angle. If they take off and, with flat pitch and just drift with the wind, or if they roll with no pitch, the quad will not acquire any IMU disorientation. This would be a significant improvement.

It's obvious that a more fancy solution would try to orient the IMU generally based on the likely thrust vector, eg if the pilot rolled an equal amount as they pitched, or pitched negatively, then the IMU re-orientation vector would reflect the likely thrust vector. However that is beyond my skills.

If this is already implicit in the above re-coding, I apologise for not recognising that.

However if not, a PR that perhaps implemented just a simple pitch-angle based solution would be much better than the current situation, which is that if a pilot flies backwards for some time, or just hovers and drifts, the IMU will passively inherit a potentially very significant error in its perception of the orientation of the quad. If such a solution was generically implemented then we could remove my interim effort from the GPS Rescue code.

[ledvinap]

It's obvious that a more fancy solution would try to orient the IMU generally based on the likely thrust vector, eg if the pilot rolled an equal amount as they pitched, or pitched negatively, then the IMU re-orientation vector would reflect the likely thrust vector. However that is beyond my skills.

This is actually trivial. Can you test current version, please?
I'll implement thrust based version soon, but I prefer to make smaller steps in this part of code

[ctzsnooze]

@ledvinap

Very quick initial visual test = it works!!

I made a 180 degree IMU error, by reversing backwards, in the standard way.

There was no hesitation, no delay, in rotation. Much faster than without this code. This strongly suggests a non-zero ez_ef value at the start despite a full 180 degree IMU problem.

thank you!!

Log images - this was a bit rushed, relatively small velocities but full IMU error at the start; ez_ef is 0.99 right from the start, leading to immediate yaw correction. The only strange thing was an abrupt sign reversal in the ez_ef value half-way through correcting the IMU error. I haven't seen this previously. It resulted in no abrupt change in yaw rate.

In contrast, this is a log image without this PR, with a well established 180 degree IMU error. ez_ef starts at 0.03, and after a full 2s flying away has only reached 0.10; it takes almost 8s to reach 0.89.

I did several quick tests, all rescues were successful. @ledvinap, at this point my only functional concern is the abrupt reversal of sign of ez_ef in the above test flight.

[ctzsnooze]

The very next flight, immediately after the one above, the log showed noise on the ez_ef values, and zeroes in all other debug fields. The rescue 'flight' itself was perfectly normal, but the debug values appear corrupted, or that the logged data sources are broken somehow.

This also happened on my first test flight, rendering the entire log unusable. I thought perhaps I had made a flashing error, but now I see it happening after re-flashing, and indeed immediately after landing and taking off, after an initially successful log, so something strange is happening. All other logged values are normal; it's only the debug values that appear broken. I haven't seen this happen previously.

So perhaps this is an unrelated issue, I don't know. I stress that the rescue itself was uneventful, I would not have known anything was wrong apart from the bizarre log.

[ctzsnooze]

@ledvinap - I did about 7 more flights, using PR 12853, which incorporated the code from this PR.
All the logs were all OK; I couldn't replicate the strange content that happened previously.

All rescues were quick and successful, however the quad never turned the full 180 degrees at the start, even if it was flown backwards for a long time.

This is a video from my OSD; note:

• the arrow is incorrect at takeoff, and does not correct despite flying out and back several times
• the arrow does rotate when the quad receives yaw inputs from the radio
• the arrow doesn't point the wrong way despite being flown backwards for a long time
• hence the initial error on the first rescue isn't 180 degrees, and the quad does not fly 180 degrees away from home
• the initial response is fast, due to changes in PR 12853, and the log shows ez_ef is close to 1
• after the first rescue, the arrow now points directly and exactly to home!
• subsequent rescues are OK, but still no 180 degree error can be developed.
• at around 2:30 I do a full 360 degree yaw rotation, the arrow follows it perfectly

My conclusion is that with this PR, in current form, the IMU yaw heading isn't updating normally, except during a rescue. Why that is, I don't know. I am confident that I transcribed this code correctly.

Here are two sample logs.

IMU_PL2_Logs.zip

Here is an image from one of those logs, showing again the abrupt reversal of ez_ef, the monotonic (smooth) correction of GPS heading, a maximum pitch angle of about 45 degrees, roll being used instead of at higher pitch angles (as it should), and overall a good rescue. Note that the quad was flying backwards for some time at the start, and see how the GPS heading corrects smoothly from around 100 degrees to 280 degrees, meaning a full 180 degree correction of flight direction occurred.

Overall the new ez_ef behaviour returns a much better rescue, but something is a bit wrong about normal IMU adjustment in normal flight.

[ledvinap]

@ctzsnooze:

• cog correction is disabled unless in GPS_RESCUE_MODE (it defaults to 0.0)
  https://github.com/betaflight/betaflight/pull/12853/files#diff-de6b2b61376cbb30512686ebb362260facb69cf9a4281ebdcd59af61c0306f7aL496

• it prevents some debug logging (CoG in IMU is skipped)

• the roll in start of rescue is strange

Immediate change of sign of ez_ef is strange. It looks like Cog - yaw crossed 180deg (exactly opposite) and IMU decided to correct error other way around.

BTFL_BLACKBOX_LOG_0412290891_20230607_194550.BBL 01:24

GPS course is changing fast.
before jump: yaw = 275.4; cog = 86.4 diff =189
after jump: yaw = yaw = 276; cog = 73.7 diff 202
The jump is a bit late ...

[ledvinap]

@ctzsnooze : Can you add note about debug value meaning to future blackbox logs, please.

[ctzsnooze]

@ledvinap Thanks for explaining the 180 switch causing the abrupt change in ez_ef sign. It didn't seem to do anything bad.

I think the roll changes are due to the 'earth referenced yaw' option in Angle mode, which translates some of yaw into roll with positive pitch angles. The logs above were able to get about 45 degrees pitch angle.

I put a note about the debugs in the opening comment of #12853:

0 = IMU CogYaw gain value * 100
1 = ez_ef value * 100 (incoming value before multiplication by IMU CogYaw gain)
2 = groundspeedErrorRatio * 100 (200 means the error between groundspeed and speed to home is 20ms)
3 = pitchForwardAngle * 100 (value of 100 means 30 degree pitch forward, 200 means 60 degrees)
4 = velocity to home in cm/s
5 = ez_ef * 100 (AFTER multiplying by IMU CogYaw gain, this is the value actually used) 
6 = dcmKpGain * 100

Also in PR #12853 I re-enabled 'normal' IMU adjustment to CoG (ie, in normal, no-GPS Rescue flight) so that we can create the 180 degree IMU error by flying backwards while testing it out.

I think it was just 'lucky' that the ez_ef value reversed sign at approximately the point in time that the GPS heading started to change quickly. In other tests the two were a bit separate in time. I'm sure you're right about it being associated with the heading reaching an arbitrary value.

[ctzsnooze]

@ledvinap - Wow! What a difference that makes! See https://www.dropbox.com/s/fi4h32qmesoqnis/IMU_PL3.AVI?dl=0

I tested the code in this PR, using PR 12853, with normal IMU gain in normal flight, so that the arrow turned correctly, and so that I could generate full 180 degree IMU errors.

All rescues with full 180 degree errors went really well!

Unfortunately I configured the debug incorrectly, so the 'unscaled' ez_ef value is actually ez_ef multiplied by the IMU gain factor. Sorry about that.

This graphic shows the quad accelerating away, but yawing steadily and smoothly, with little in the way of overshoot at the end. The newer boosting algorithm, combined with peak ez_ef from the start, works really well.

If the drift away had been higher, the amount of gain would have been greater.

I only had time for three quick flights and all looked similar to the one below.

I think I can boost the IMU a bit more strongly, as the arc was a bit wider than necessary, but I will need to do more testing.

The immediate yaw response was really impressive. No more waiting forever, it just started turning straight away.

Needs rebase ...

[ledvinap]

When testing this, if I set THRUST_COG 1, and I perform a sustained backwards pitch flight, I am guessing that the arrow should not point the wrong way? And that the arrow should ignore from say a pure roll-only (sideways) flight?

I hope so ;-)

Could the next step possibly be a proportional factor where, for example, if the thrust vector is vertical with respect to the earth frame, and there i no sideways thrust at all, then CoG gain is zero, whereas with increasing angle from 'flat', the magnitude of the Cog yaw gain would be increased?

Already implemented.
Currently, the gain is sqrt(sin(tilt_angle)). sqrt tries to increase gain a bit on small tilt to match old behavior better (odl code used maximum gain when leveled, decreasing it with tilt cos(tilt_angle). But I don't like it, maybe just increase overall gain a bit or increase gain and limit at 45 deg.

This would help give us zero IMU yaw gain during a flat drift due to wind, for example, and that would be really good. Conversely, when quad gained commanded angle in the horizontal plane, it is more likely to acquire more velocity in the line of the thrust vector at higher angles, so it makes sense to to boost the IMU yaw gain more strongly the greater the angle away from horizontal.

Needs testing ... Wind is a problem. Wind estimator may help, but it will be quite complicated.

[ledvinap]

I have updated 12853 with your thrust vector CoG code, and WOW !!! It's now essentially impossible to generate any IMU errors! All rescues turned quickly and directly to home, with no IMU error issue to deal with.

You can use some unused flight mode to generate IMU error.
something like (in https://github.com/betaflight/betaflight/pull/12792/files#diff-de6b2b61376cbb30512686ebb362260facb69cf9a4281ebdcd59af61c0306f7aL226)

if (IS_RC_MODE_ACTIVE(BOXUSER1)) {
   float ez_ef = 3;
    ex += rMat[2][0] * ez_ef;
    ey += rMat[2][1] * ez_ef;
    ez += rMat[2][2] * ez_ef;
}

This will start rotating IMU Yaw as long as RCMODE is activated, generating error as necessary. With smaller ez_ef (or another mode), you can also test RTH with steady error input.

[SteveCEvans]

I'm sure compiler will do this anyway, but shouldn't we precalculate sin(angle / 2)?

[SteveCEvans]

Should this define be here or at the top of the file? Is it for experimentation only and if so, have you decided that the THRUST_COG = 0 option isn't required at all, in which case why keep it?

[ctzsnooze]

I agree, now that we've tested it, and know it works, I don't mind if we remove the THRUST_COG define.

[SteveCEvans]

This cast is safe, but I had to check. Would a conversion function be better?

[ctzsnooze]

@ledvinap - thanks for the explanations. I think that the additional gain from 1/sqrt works well. Without it, sine of a 10 degree pitch angle is close to 0.2, meaning we'd need to fly for almost 30s at that angle to get the IMU oriented. With the 1/sqrt factor, the gain is about 0.4, still slow, but not unreasonably slow. When I tested this it seemed about right. Our advice is to fly forward until the arrow is oriented. The 'arrow' seemed to want me to fly with definite forward angle to orient itself, and that seemed exactly how I would like to have it.

If you have time to rebase to master, that would be great, since the gTest code is already in master.

My only query, now we have thrust vector IMU updating, is whether the <2m/s exclusion that blocks any IMU update in imuCalculateEstimatedAttitude() makes sense.

I've noticed that GPS heading info is quite good down to about 0.5m/s; then it tends to hold the last value for a while. We are not using this information below 2m/s. Also, there can be quite a lot of wind drift at 2.0m/s, that's a very slow speed, walking pace only. I was wondering if we said, perhaps:

• below 0.5m/s, set cogYawGain 0, as we do now below 2m/s
• from 0.5m/s to 10.0m/s we linearly increase the incoming cogYawGain value towards 1.0

I am thinking that the faster we travel, particularly with the thrust vector code that you've done, the more likely it is that the course over ground represents an outcome from the thrust of the machine, rather than simple drift from a poorly calibrated acc, or wind. Of course, with higher wind speeds, there may be significant groundspeed from wind drift, but in that case we are no worse off than we were with the simple on/off at 2m/s.

The 'old' threshold value (going back a long way) was 5m/s.

Anyhow its not difficult to implement, and I wondered about your thoughts about pursuing this idea, generally. It would be in a different PR.

[ledvinap]

@ctzsnooze : I'm afraid that decreasing minimum CoG speed will greatly influence problems with wind. Worst case is when you fly against wind, but drifting backwards - it will result in 180deg error. And maybe filtering will result in similar situation when you fly quite slowly around fixed place.
I'll start with 2-10m/s ramp and see how it works

[ctzsnooze]

@ledvinap I tried a simple minimum of 0.5m/s, and then a linear ramp so that at 5m/s, gain = 1.0; then at 10m/s, gain = 2.0, with a max of 2.0.

This works really well; I flew it today.

Currently we have full gain at 2m/s, which is easy to achieve with simple drift. With the above modification, at 2m/s the gain is only 0.2, so it takes 5 times longer than our current code to develop the same IMU error from low speed drift.

Previously wind drift of just 3m/s was a major problem, not just because the base IMU gain was 1.0, but also because we didn't attenuate that IMU gain on the basis of a low pitch angle. Now, with your code here, we need a thrust angle to be offset from zero before we accumulate IMU error.

Hence with the above modification, or without, a 'flat' drift won't accumulate any IMU error, thanks to this PR.

Following your explanation, above, I measure that at 10 degree pitch angle, the IMU gain is about 0.4. If we only have this factor in play, it would take almost 20 seconds flying dead straight with a 10 degree pitch angle to 'set' the IMU correctly.

With the modification described above, the quad is very likely to get over 5m/s, after a few seconds, increasing the IMU gain.

Even with a 20 degree pitch angle, the IMU gain is about 0.58 with this PR. 20 degrees would reasonably quickly bring groundspeed over 10m/s, increasing IMU gain to 1.16 - faster than our current lag. So a pilot who pitches forward 20 degrees, and gains 10m/s of forward velocity, will probably get their IMU oriented properly in 4-5s.

And, of course, at even higher pitch angle, eg 35 degrees, base IMU gain is around 0.75. Multiply this by 2 from the groundspeed factor and we are getting IMU orientation significantly faster than the normal 6-8 seconds, which makes sense. Could probably allow up to 3x IMU gain at 15m/s I guess.

It did turn out pretty good, I think. I'll push it to the test branch.

I only had about 20kph wind drift today, and it was almost impossible to generate an IMU error. The best I could hope for was to fly incredibly slowly after arming, just exceeding the 0.5m/s minimum, while losing groundspeed despite pitching into the wind. This took quite a lot of effort. But I could eventually get an IMU error, it took 15-20s. But, as soon as I flew 'normally', even for only a few seconds, the IMU got itself oriented quite quickly and cleanly, since my 'normal' speed was around 25-35kph.

I think that IMU errors can be acquired in acro mode by going inverted for a while, not really sure, I did some inverted yaw spins and think the IMU got a bit messed up like that, but as soon as I flew at speed it reset its orientation fairly quickly.

I did try a linear ramp to a maximum of 1.0 at 10m/s, but this caused too much delay in acquiring normal IMU orientation; a slowish forwards flight around 5m/s seemed to take a very long time to acquire a good idea of where the home arrow should go.

[ledvinap]

@ctzsnooze : Thanks for testing.

As for inverted flight - the math shall be fine, both CoG and thrust vector are projected to ground plane. Only problem would be inverted thrust (3d mode), it is trivial to fix (multiply thrust vector with sign(thrust) )

ad gain - with current code, your tuning is best approach, use values as you see fit. But be a bit conservative with respect to old behavior.

Ideally, IMU shall update gain based on previous state ( = Kalman ;-) ). After arming (or gyro overflow), there is no yaw info, so IMU shall use all information available and converge quickly. 5-10s sounds about right.
But then only gyro drift is affecting IMU and it is quite small (at most few dps), so CoG gain shall be decreased and Heading time constant increased to low minutes range. This will in effect use only good portions of flight to remove gyro drift, but not disorient IMU on some strange maneuver.
Some mathematical model is IMO required for this - using heuristics will greatly complicate code and leas to unexpected situations. Even with poor quality model, it is much easier to reason about it's shortcomings and edge cases.

So the idea is to get this code working acceptably with minimum complexity, at least for now.

[ctzsnooze]

@ledvinap This code is very good. It just needs rebasing, so we can merge soon. Thank you for this, it's a massive improvement.

Tested by myself and two others, works really well.

Maybe we can, later, in a different PR, consider a velocity based yaw IMU gain multiplier, instead of the on/off at 2m/s (which used to be 5m/s before 4.1). Once this PR is merged II will collect data and see if we can make improvements. However the biggest improvements are the changes in this PR.

[blckmn]

@ledvinap this should be good to go.