Lower time required for attitude recovery after very hard (involuntary) landings #12937

tbolin · 2023-07-03T20:01:14Z

Work in progress for several small improvements to speed up the recovery after crashing.

Improvements so far

Avoid decreasing accelerometer gain at large errors

The error between the estimated up direction and the upward direction measured by the accelerometer is calculated by a cross product. Both vectors have magnitude 1, so the norm of the error is just the absolute value of sin of the angle between the vectors. For angles with an absolute value of more than 90 degrees the magnitude of the error would decrease, slowing down convergence. This commit introduces a check for those large angles and ensure that the magnitude of the error always increases when the angle increases.
Similar to #12792 but for the accelerometer.

Reliability based accelerometer gain

Basically adds a variable keeps track of how off the current attitude is likely to be. The gain is then calculated based on that variable.
The reliability is implemented as a Kalman filter for a linear system in a single variable, and the reliability is measured as the covariance of the estimated attitude.
The prediction step integrates the gyro rates, and the update step uses the error from the accelerometer.

To save calculations the update step is effectively delayed one cycle. The update and prediction steps can then be carried out in the same quaternion multiplication (this is essentially the same as what was previously done in the Mahoney filter, only now with a different gain for the accelerometer error). The output from the filter is therefore effectively the predicted value.

Settings

imu_gyro_noise_std standard deviation of the gyro noise in [deci-degrees] . Default: 5.
imu_acc_noise_std standard deviation of the accelerometer noise in [deci-degrees] . Default: 50.

Trust based accelerometer covariance

The covariance of the accelerometer noise is increase when the magnitude of the acceleration vector differs from 1g.
When the difference is greater than 0.2g the acceleration vector is not used to update the roll/pitch estimate.

The reasoning behind using this scaling is pretty obvious (and a simpler version is already in BF): if the acceleration is not close to 1g it's probably due to the quad accelerating and the vector is not pointing in the correct direction.

The covariance is also increased at high gyro rates. At rates over 50 deg/s the accelerometer is not used to update the estimate.
The reasons for including the gyroscope rates are that the accelerometer vector is filtered and will lag behind slightly at high rotational rates, and that the centripetal force will create biased readings unless the accelerometer is mounted exactly at the center of gravity.

TODO: add formulas

Trust based gyro covariance

The roll/pitch estimate seems to drift much faster at high gyro rates than when the quad is not moving.
The covariance of the gyro is therefore increased with the gyro rates.

The current formula is gyro_covariance = imu_gyro_noise_std^2 + gyro_rate_magnitude * imu_gyro_noise_std^2 / 10

Gyro rate scaling adjustments

The sensitivity of a gyro (especially the BMI270s) can differ slightly from what is printed in the data sheet. Even a small error in the sensitivity will give large errors when integrated. e.g. on of my BMI270s have an error of about 2.5% on the yaw axis, which will result in a 9 degree error in heading if turned 360 degrees.
The scaling errors also seems to be the main driving factor behind the weird drift of the roll/pitch estimate when either wobbling the quad by hand or when wobbling quickly in flight.

The PR adds a gyro_scaling_adjustment setting that can be used to adjust the sensitivity scaling.
gyro_scaling_adjustment is a 3 value vector, where the values represent the drift in roll, pitch, and yaw resulting from turning the model 360 degrees counter clockwise around the respective axis.

TODO: Add detailed calibration guide.

The drunk quad syndrome

Sometimes a quad will no longer be able to fly straight in angle mode after a crash, making it appear drunk.
The exact reason for this is not entirely known.
One factor could be if the gyro is saturated during the crash. An incorrect value will then be integrated and the attitude will then have to be corrected by the accelerometer.
This hypothesis can be tested by quickly flicking an FC (or a whole quad) and checking for drift in the attitude.
However, in the current code the accelerometer gain is constant when the quad is armed, but uses much higher gains when disarmed, so problems that appear during flight can be hard to observe in the configurator.
By commenting out the gain scheduling I could however make the quad model in the configurator diverge when turning the FC quick enough, and due to the low accelerometer gain it takes a long time to get back to the correct value when the FC is placed back on the table.

I also found that I could make the attitude diverge by just wobbling the quad around for a few seconds at far bellow the saturation rate, and I'm not sure why this is.
The attitude should drift a little bit due to integration of the gyro data.
I would expect about 1 degree/sec at worst just from the static offset from the gyro measurements, but considering how steady the heading is (and that bf does a short calibration of the gyro each power up) the drift should be less. By wobbling I could make the attitude deviate by over 20 degrees in a few seconds.
I increased the attitude update rate to 1kHz (from 100Hz) to see if there was something with the integration, but I couldn't see any notable difference.
I also experimented with changing the filtering for the accelerometer and the gyro. Removing all the filtering from the gyro (including the hard-coded filter for data sent to the attitude calculation), or setting the accelerometer filter to any extreme didn't make a difference as far as I could see. Heavy filtering of the gyro (2 pt3 at 10Hz) made the deviation worse, especially the heading deviated much more than before, but not by much.
My best guess is that the wobble deviation come from accelerometer readings that are included despite being biased.

github-actions · 2023-07-03T20:01:26Z

Do you want to test this code? You can flash it directly from Betaflight Configurator:

Simply put #12937 (this pull request number) in the Select commit field of the Configurator firmware flasher tab (you need to Enable expert mode, Show release candidates and Development).

WARNING: It may be unstable. Use only for testing!

blckmn · 2023-07-03T21:17:18Z

AUTOMERGE: (FAIL)

github identifies PR as mergeable -> FAIL
assigned to a milestone -> PASS
cooling off period lapsed -> PASS
commit count less or equal to three -> FAIL
Don't merge label NOT found -> PASS
at least one RN: label found -> PASS
Tested label found -> FAIL
assigned to an approver -> FAIL
approver count at least three -> FAIL

haslinghuis · 2023-07-05T18:54:46Z

@tbolin please rebase on master.

Please explain the tent function :)

tbolin · 2023-07-05T20:08:07Z

Please explain the tent function :)
https://en.wikipedia.org/wiki/Triangular_function
It can among other things be used as a rough but fast approximation of a gaussian.
I chose to call it tent since it's also a common name and less likely to conflict with something else than triangle or triangular.

I'll rebase tomorrow.

haslinghuis · 2023-07-05T20:12:13Z

🤣 Yes was looking at:

https://en.wikipedia.org/wiki/Tent_map
https://math.stackexchange.com/questions/4669545/help-understanding-discription-of-a-mathematical-tent

tbolin · 2023-07-06T12:08:48Z

Rebased on master and fixed an issue with building the SITL target.
The tests failing is expected, and I will get to that ones I have managed to get the test target to build locally.

ledvinap

Great work, looks very promising.

Can you please add comments/(references) to concepts used in code, where relevant?

src/main/common/maths.c

ledvinap · 2023-07-10T12:34:01Z

src/main/flight/imu.c

@@ -290,10 +291,23 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
        ay *= recipAccNorm;
        az *= recipAccNorm;

+        fpVector3_t acc_upp = {.x = ax, .y = ay, .z = az};


Suggested change

fpVector3_t acc_upp = {.x = ax, .y = ay, .z = az};

fpVector3_t acc_bf = {.x = ax, .y = ay, .z = az};

Not sure what upp means. And better use _ef and _bf suffix, it makes the code slightly more readable

I'll change the suffixes. What does ef and bf stand for here?
And when I think about it it should probably be AccEF and AccBF to comply with Betaflight's coding standards.

BodyFrame and EarthFrame. Rest of code uses underscore suffix, but using camel case ma be good idea. (personally, I slightly prefer underscore version in this case (reference frame is separated from variable name)

upp should be up so acc_up means the direction of up as measured by the accelerometer. In the same way est_up(p) is the previous estimate of the upwards direction.
Unless I'm missing something both vectors are in the body frame here, and should probably have a bf suffix in addition to the direction.
I'll keep the snake case suffixes for now.

ledvinap · 2023-07-10T12:39:08Z

src/main/flight/imu.c

@@ -290,10 +291,23 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
        ay *= recipAccNorm;
        az *= recipAccNorm;

+        fpVector3_t acc_upp = {.x = ax, .y = ay, .z = az};
+        fpVector3_t est_upp = {.x = rMat[2][0], .y = rMat[2][1], .z = rMat[2][2]};


This is up vector, but acceleration is pointing down.
The code needs cleanup, it was hacked together so that all signs ended up correctly in the end, but was wrong during calculation.

If the accelerometer is at rest the measured acceleration will point up, in the opposite direction of gravitation.
I agree with the statement about the code being hacked together and needing a bit of cleanup though.

ledvinap · 2023-07-10T12:42:03Z

src/main/flight/imu.c

@@ -290,10 +291,23 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
        ay *= recipAccNorm;
        az *= recipAccNorm;



(You can rewrite code above to use new vector functions .. )

I'm painfully aware. I've tried to refrain from vectorizing code that I haven't touched for other reasons. Otherwise that would be all I would be doing. I will definitely have another look at it later when the dust have cleared a bit.

ledvinap · 2023-07-10T12:44:42Z

src/main/flight/imu.c

+        // To avoid the gain decreasing for angles > 90 degrees:
+        // set magnitude of error vector to 1 + |cos| of angle between estimated
+        // and measured downwards vectors if the absolute angle of the error is > 90 degrees
+        acc_err = (dot > 0) ? acc_err : *vectorScale(&acc_err, vectorNormalize(&acc_err, &acc_err), 1.0f - dot);


Suggested change

acc_err = (dot > 0) ? acc_err : *vectorScale(&acc_err, vectorNormalize(&acc_err, &acc_err), 1.0f - dot);

acc_err = (dot > 0) ? acc_err : *vectorNormalize(&acc_err, &acc_err);

IMO keeping error vector constant (=1) after 90 deg is enough and may prevent some obscure stability problem.

Possibly, keeping it at 1 above 90 degrees would also simplify the code. However I like having the error increase if the angle gets larger, and I haven't seen any stability issues despite the current code sometimes using very large gains. The magnitude of acc_err is still bounded to 2 instead of 1, so it's not like it can run away to infinity.

I'd prefer simpler version, but it's your call

ledvinap · 2023-07-10T13:04:05Z

src/main/flight/imu.c

@@ -366,80 +380,36 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
    }
 }

-static bool imuIsAccelerometerHealthy(float *accAverage)
+static float imuAccTrust(const float* accAverage, const float* gyroAverage, const float gRecip)


gRecip is really poor name. acc_1gRecip?

Also, it may be better to convert ACC to g's outside of this function (and eventually move it to acc calibration code)

Agreed on both accounts. I'll rename the variable, and take a look at moving the scaling outside.

ledvinap · 2023-07-10T13:07:30Z

src/main/flight/imu.c

@@ -576,7 +564,7 @@ static int calculateThrottleAngleCorrection(void)

 void imuUpdateAttitude(timeUs_t currentTimeUs)
 {
-    if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) {
+    if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce && gyroIsCalibrationComplete()) {


Gyro calibration takes considerable time and offset is not that big. It is IMO possible to converge IMU during this time

I thought so too. However I had problems where the estimated attitude would end up way off if the quad was moved when connecting the power source and for perhaps 5 seconds after. It's almost as if there are some huge jump in the gyro data during or after calibration.
Adding this check removed the issue.
Might be worth looking into further, but currently this check is needed.

Ok. It may also be possible to handle it similarly to gyro overflow case - set assumed gyro noise higher, so that attitude estimate is low weight and filter converges quickly.

The gyroADC and gyroADCf values won't be populated unless gyroCalibrationCompleted is True, see

betaflight/src/main/sensors/gyro.c

Line 417 in d5d3ee8

if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1)) {

The estimate covariance is already initiated to the maximum value at start, and converges quickly once it's getting data. However, the covariance should probably reset each time gyroCalibrationCompleted is false, to avoid weirdness if another calibration is initiated somehow.

src/main/flight/imu.c

ledvinap · 2023-07-10T13:24:14Z

src/main/flight/imu.c

@@ -489,10 +459,19 @@ static void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t in
 }
 #endif

+static float imuUpdateDeviation(const float deviation, const float imuDt, const float durationSaturated)


Suggested change

static float imuUpdateDeviation(const float deviation, const float imuDt, const float durationSaturated)

static float imuUpdateDeviationGyro(const float deviation, const float imuDt, const float durationSaturated)

src/main/flight/imu.c

ledvinap · 2023-07-11T10:38:09Z

@tbolin: Alternate approach may be to use principles from Kalman filter (yes, I know). Estimate (or more likely guess) noise value for accelerometer (expected noise in down vector) / gyro (saturated/calibrating/normal) and then keep track of attitude estimate noise, as single value. The math will be very similar to this PR, but with cleaner mathematical background.

tbolin · 2023-07-11T12:10:27Z

@tbolin: Alternate approach may be to use principles from Kalman filter (yes, I know). Estimate (or more likely guess) noise value for accelerometer (expected noise in down vector) / gyro (saturated/calibrating/normal) and then keep track of attitude estimate noise, as single value. The math will be very similar to this PR, but with cleaner mathematical background.

I've worked a little bit with Kalman filters before, and I will take a look at if anything in this PR can be brought in line with that. However, I think the biggest problem will be how to translate the various noises into something that can be used in the Mahoney filter to make it converge in at a rate appropriate for how fast the estimate noise converges. This is also a core problem with the current PR. There really isn't a well reasoned link between the convergence rates of the attitude estimate and the attitude deviation estimate.

ledvinap · 2023-07-11T13:40:24Z

However, I think the biggest problem will be how to translate the various noises into something that can be used in the Mahoney filter to make it converge in at a rate appropriate for how fast the estimate noise converges

Mahony is in fact complementary filter (or close to one. It is described/implemented as PI regulator, but math behind it is equivalent). So Kp is (almost) equivalent to complementary filter gain. And if I understand Kalman correctly, it is calculating optimum gains based on measurement noise and state noise (and updating state noise). So this should provide gain quite easily.
And input noises are mostly guessed / tuned for given application, and in effect multiply computed gains. So it may be enough only to use correct structure.

tbolin · 2023-07-14T10:42:23Z

However, I think the biggest problem will be how to translate the various noises into something that can be used in the Mahoney filter to make it converge in at a rate appropriate for how fast the estimate noise converges

Mahony is in fact complementary filter (or close to one. It is described/implemented as PI regulator, but math behind it is equivalent). So Kp is (almost) equivalent to complementary filter gain. And if I understand Kalman correctly, it is calculating optimum gains based on measurement noise and state noise (and updating state noise). So this should provide gain quite easily. And input noises are mostly guessed / tuned for given application, and in effect multiply computed gains. So it may be enough only to use correct structure.

In the complementary filters I've seen the gain has been in the range [0, 1], and used for a linear interpolation between the state given by the two sensors e. g. $x_{t+1} = (1 - k)a + kb.$ So a gain of 0 means the integrated values are completely trusted, and a gain of 1 means that the state instantly goes to the latest measurement.
IIRC The Kalman gain in a scalar filter is used the same way and is in the same range (with more states I guess the singular values of the gain matrix are in the same range, but I haven't looked that up).
In the code the gain is just $k > 0,$ and as far as I can tell it's the same in the paper that describes the Mahoney filter.
If there exists a k s.t. the state of the filter instantly goes to the latest acc measurement, then that gain could be used as the end of the range instead of 1, essentially implementing a dead beat controller. However, that value probably is dependent on the error, and even if we find a formula calculating the correct value is likely relatively expensive.

ledvinap · 2023-07-14T12:45:50Z

In the complementary filters I've seen the gain has been in the range [0, 1], and used for a linear interpolation between the state given by the two sensors e. g. xt+1=(1−k)a+kb. So a gain of 0 means the integrated values are completely trusted, and a gain of 1 means that the state instantly goes to the latest measurement.

You can rewrite it as (with
$x_{t+1} = (1 - k)x_t + kb$
$x_{t+1} = x_t + k(b - x_t)$

Then, in Mahony, $b - x_t$ (error estimate) is calculated directly from different error sources and $k$ is dcm_kp. (It is also similar to RC filter, which is useful to estimate Mahony time constants). dcm_kp is set to be quite low ($k\ll1$), so nobody probably considered case where k is close to (or over) one.

In the code the gain is just k>0, and as far as I can tell it's the same in the paper that describes the Mahoney filter. If there exists a k s.t. the state of the filter instantly goes to the latest acc measurement, then that gain could be used as the end of the range instead of 1, essentially implementing a dead beat controller. However, that value probably is dependent on the error, and even if we find a formula calculating the correct value is likely relatively expensive.

Quaternion (rotation in general) and nonlinear, but most of Mahony (at least in BF) is using first-order approximations (or simply operations that point in correct direction), so setting $k=1$ will be very problematic. But in reality, immediate convergence is not necessary, running few tens of iterations (at 500Hz) is perfectly fine. So gain should be limited to some reasonable value (even limiting time constant using equations for RC filter would be quite easy).

Nice thing about Kalman [based derivation] is that tuning will work with noise magnitudes. The have some physical meaning, there are theories how to estimate them (blackbox logs are available). Actual gain calculated will be optimal - at least as far as Kalman theory assumptions are satisfies, which probably means 'not much'.
The code may also make more sense to newcomers, it would be great to have more math people onboard (I wish I did pay more attention at school).

tbolin · 2023-07-20T16:11:43Z

I have pushed a new version with a much more Kalman like dynamic gain. There is some trickery to avoid doing more than one integration, causing the order of updates to be a bit weird compared to a standard Kalman filter.
I'll try to write down a bit more of the math later, but in short linearity is assumed, the noise is assumed to be white and the system is a pure integrator of the gyro rates.
Works very well on my desk.

I also added a check that prevents updates of the attitude if the time delta is greater than 0.1 sec, and that solved an issue with sudden jumps at start up. The roll/pitch estimate covariance should be reset if this happens, but I haven't implemented that yet.

ledvinap · 2023-07-20T17:24:49Z

@tbolin : Great!
Finally, the IMU gain scheduling is starting to make sense. If it works, it can be adapted to compass and GPS, possibly separating prediction and state update, making it closer to normal implementation.

I need to study the math, but code looks fine.

tbolin · 2023-07-26T19:02:54Z

I think I've found the source of the gyro wobble drift: #11894
My boards with ICM-42688-P gyros does not drift nearly as much during a wobble test as my BMI270 boards.

I confirmed the drift when integrating from the BMI270 as in the issue. The scaling adjustment required also seems to be different for different axis.

tbolin · 2023-07-26T21:11:54Z

Added settings to adjust the gyro rate scaling. The settings are in the error per full rotation around that axis in decidegrees.

I turned off the acc and did the wobble test with and without adjustments and the drift was was consistently less than half with the adjustments compared to without.

I also made the gyro covariance scale with high gyro rates (still experimental, and should probably be turned down now), and fixed a bug that caused the error vector to always be normalized.

Edit:
The reason for including the rate scaling adjustment here is that I have had problems with the roll/pitch estimate being slightly off after wobbling around while flying. The quad will be almost stationery with the sticks centered at first, then after a bit of wobbling and flying back and forth it will start leaning slightly in one direction, a bit more wobbling and it might start leaning in another. The quad will usually straighten up after several seconds of hoovering.
I'm trying to find what's causing this problem and the main suspects are the wobble drift and spurious accelerometer readings.

In logs I have seen short bursts of accelerometer readings that are considered valid (the magnitude of the acc vector and gyro rates are within limits), but are off by more than 45 degrees. These readings are likely due to the quad accelerating in just the right way to spoof a valid gravity vector.
However, I can't tell if these spurious readings are what's causing the rp-estimate to drift around without eliminating the other possible source.
If the spurious readings are the main cause of the drift the gyro should be trusted more, but if the wobble drift is the cause the gyro will have to be trusted less at high gyro rates, which will cause any spurious acc readings to have more influence etc.

A standard Kalman filter isn't great at handling outliers. Even if a sensor reading is extremely unlikely given the filters current estimate covariance, the reading can still have a large influence on the estimate. There are ways to handle outliers by e.g. ignoring readings beyond a certain Mahalanobis distance but that's another can of worms if the filter settles on an erroneous value.

I think part of the problem in the current implementation is that the adjustments is proportional to the error.
With a proportional adjustment the error that a few spurious readings cause will take a lot more valid readings to correct.
With quads this problem becomes even more apparent since a small error in the rp-estimate will cause the quad to drift noticeably.
In one way I actually liked the behavior more with the bug that caused the error to always be normalized, but that also caused the estimate to be jittery around the correct value. I saw that there was an heuristic for increasing small errors in the cog gain code, maybe the acc gain code needs a bit of the same treatment.

tbolin · 2023-07-28T20:32:23Z

Did some flight testing with calibrated gyro rate gains, and I think that helped a lot with the drift during flight. I also had some hard bumps that saturated the gyro and the attitude seems to have recovered quickly, and to a correct value.
dynamic_gyro_gain_hard_bumps.zip

tbolin · 2023-08-20T18:17:47Z

src/main/flight/imu.c


 #ifdef USE_MAG
    // Use measured magnetic field vector
    fpVector3_t mag_bf = {{mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}};
    float recipMagNorm = vectorNormSquared(&mag_bf);
-    if (useMag && recipMagNorm > 0.01f) {
+    if (useMag > 0.0f && recipMagNorm > 0.01f) {


Should probably not have been changed, marking so I can get back to it

tbolin · 2023-08-20T18:54:03Z

Finally got the tests to compile locally. Now the old tests compile and passes as they should. Tests for the new code is still a todo.

The error between the estimated up direction and the upward direction measured by the accelerometer is calculated by a cross product. Both vectors have magnitude 1, so the norm of the error is just the absolute value of sin of the angle between the vectors. For angles with an absolute value of more than 90 degrees the magnitude of the error would decrease, slowing down convergence. This commit introduces a check for those large angles and ensure that the magnitude of the error always increases when the angle increases.

- Added tent function to math.h - Decoupled cog calculation from dcmKp gain in imuMahonyAHRSupdate (cog gain shoul be the same as before, but will not be affected by the new dynamicer acceleroemter gain) - Add IMU_GAIN debug mode - Add function (gyroGetDurationSpentSaturated) to gyro.c that returns the time the gyro has been saturated since last heading update - Add estimated attitude deviation to imu.c - Replace imuIsAccelerometerHealthy function with imuAccTrust that calculates a 1.0 - 0.0 scalar instead of just a yes/no. Also checks for high gyroa rates. - Replace old kp gain scheduling function with one that's based on the trust level and assumed deviation instead of armed/disarmed etc. - Only update attitude if the gyro calibration is completed

- Calc duration saturated from gyro looptime instead of PID looptime - Add check that the duration spent saturated is equal to or smaller than the total duration when updating the acc deviation - Lower limit for acceptable gyro rate norm to 100 degrees (was 500 degrees). - Increas limit for acceptable accelerometer norm to 0.2 g (was 0.1 g). - Implement experimental 4th order gyro integration, turned off by define by default - Change normal gyro integration accumulation rate to 1.0 deg/s (was 5.0) - Use base kpgain multiplied by acc trust to update attitude deviation instead of calculated kp

- Implement more kalman like dynamic gain for roll pitch updates from accelerometer. - Add comments to tent function in math.h and remove center parameter - Add guard to prevent IMU heading from being updated if the delta T is greater than 0.1s. Prevents huge timestep after boot. - Remove experimental 4th order quaternion integration - Minor refactoring

- Use durationSaturated instead of durationSaturated^2 when updating the RP estimate covariance in imuUpdateRPEstimateCovariance. The squared duration was a missguided attempt at modeling the gyro drift when saturated as a static bias. The a bias should probably be modeled by having the standard deviation increase linearly, instead of the covariance as it is now. However, that would require taking the square root of the total estimate variance, adding the saturated duration times the assumed drift and then squaring the result. The rate of drift is a crude guess and the extra code complication and perforrmance required are most likely not worth it. - Reset roll/pitch estimate covariance if delta T is greater than 0.1s

- Noise is set in decidegrees, e.g. 5 will lead to a value of 0.5 degrees

- fix bug causing the normalized accDiff vector to always be used - Make it possible to not use acc in attitude update by setting imu_acc_noise_std = 0 - set debug mode IMU_GAIN 2 to display acc angle error in decidegrees - decreas acceleration and gyro rate norm limits to 0.1g and 50 deg/s (from 0.2 g and 100 deg/s) - refactoring

- Turn some magic config variables in functions into static constants on top of file - Remove unused defines related to configuring previous acc gain calculation - Change IMU_ESTIMATE_COVARIANCE_MAXIMUM to 180 (from 360) - Change IMU_ACC_COVARIANCE_CALC_ACC_NORM_LIMIT to 0.2 (from 0.1) - Change IMU_GYRO_COVARIANCE_CALC_RATE_SCALING to 1/10 (from 1/5)

…eviation)

haslinghuis assigned tbolin Jul 3, 2023

haslinghuis added In Development RN: IMPROVEMENT labels Jul 3, 2023

haslinghuis added this to the 4.5 milestone Jul 3, 2023

haslinghuis added the Rebase Required label Jul 5, 2023

tbolin force-pushed the dynamicer_imu_gain branch from 1bdecea to 5b6372e Compare July 6, 2023 10:13

tbolin changed the title ~~Lower time required for heading recovery after very hard (involontary) landings~~ Lower time required for heading recovery after very hard (involuntary) landings Jul 6, 2023

tbolin changed the title ~~Lower time required for heading recovery after very hard (involuntary) landings~~ Lower time required for attitude recovery after very hard (involuntary) landings Jul 6, 2023

ctzsnooze requested a review from ledvinap July 7, 2023 05:29

ledvinap reviewed Jul 10, 2023

View reviewed changes

haslinghuis removed the Rebase Required label Jul 10, 2023

ledvinap reviewed Jul 11, 2023

View reviewed changes

src/main/flight/imu.c Outdated Show resolved Hide resolved

tbolin force-pushed the dynamicer_imu_gain branch from 04b098f to c459cb7 Compare July 20, 2023 16:02

tbolin force-pushed the dynamicer_imu_gain branch from 1d2ab2d to 6ea3bd0 Compare July 26, 2023 21:15

tbolin commented Aug 20, 2023

View reviewed changes

haslinghuis added Testing Required and removed Tests failing In Development labels Aug 20, 2023

pichim mentioned this pull request Sep 18, 2023

rearrange rescue phase enum and keep Mag during landing #13072

Merged

haslinghuis added the Rebase Required label Nov 8, 2023

haslinghuis modified the milestones: 4.5, 4.6 Dec 3, 2023

tbolin force-pushed the dynamicer_imu_gain branch from 348efc7 to 1bf9283 Compare December 27, 2023 18:26

tbolin added 18 commits December 29, 2023 19:04

Mark some imu variables as UNUSED for SITL target

ec4f8b1

Calc duration saturated from gyro looptime instead of PID looptime

96adf30

Add settings for accelerometer and gyro noise standard deviations

859d32a

- Noise is set in decidegrees, e.g. 5 will lead to a value of 0.5 degrees

Add gyro_scaling_adjustment setting

c27acee

Change imu gyro noise unit to power spectral density (from standard d…

d156293

…eviation)

Reset imu estimate covariance on gyro calibration

d03d1de

Update tests for new mahonyAHRS signature

55c649b

Refactor Mahoney filter for easier unit testing

c8cbbb6

Scale acceleration norm to G before using in imuAccCovariance

a246062

Rename imu runtime variables in test

52d57c1

Add imu process dt to IMU_GAIN debug

04ebc9e

tbolin force-pushed the dynamicer_imu_gain branch from 1bf9283 to 04ebc9e Compare December 29, 2023 18:11

ledvinap added the Pinned Pinned items are excluded from automatically being marked as stale label Apr 22, 2024

	fpVector3_t acc_upp = {.x = ax, .y = ay, .z = az};
	fpVector3_t acc_bf = {.x = ax, .y = ay, .z = az};

		@@ -290,10 +291,23 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
		ay *= recipAccNorm;
		az *= recipAccNorm;

	acc_err = (dot > 0) ? acc_err : *vectorScale(&acc_err, vectorNormalize(&acc_err, &acc_err), 1.0f - dot);
	acc_err = (dot > 0) ? acc_err : *vectorNormalize(&acc_err, &acc_err);

	static float imuUpdateDeviation(const float deviation, const float imuDt, const float durationSaturated)
	static float imuUpdateDeviationGyro(const float deviation, const float imuDt, const float durationSaturated)

Lower time required for attitude recovery after very hard (involuntary) landings #12937

Are you sure you want to change the base?

Lower time required for attitude recovery after very hard (involuntary) landings #12937

Conversation

tbolin commented Jul 3, 2023 • edited

Improvements so far

Avoid decreasing accelerometer gain at large errors

Reliability based accelerometer gain

Settings

Trust based accelerometer covariance

Trust based gyro covariance

Gyro rate scaling adjustments

The drunk quad syndrome

github-actions bot commented Jul 3, 2023

blckmn commented Jul 3, 2023 • edited

haslinghuis commented Jul 5, 2023

tbolin commented Jul 5, 2023

haslinghuis commented Jul 5, 2023

tbolin commented Jul 6, 2023

ledvinap left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ledvinap commented Jul 11, 2023

tbolin commented Jul 11, 2023

ledvinap commented Jul 11, 2023

tbolin commented Jul 14, 2023

ledvinap commented Jul 14, 2023

tbolin commented Jul 20, 2023

ledvinap commented Jul 20, 2023

tbolin commented Jul 26, 2023 • edited

tbolin commented Jul 26, 2023 • edited

tbolin commented Jul 28, 2023

Choose a reason for hiding this comment

tbolin commented Aug 20, 2023

tbolin commented Jul 3, 2023 •

edited

blckmn commented Jul 3, 2023 •

edited

tbolin commented Jul 26, 2023 •

edited

tbolin commented Jul 26, 2023 •

edited