Drumettaz said:

About gain scaling with CORRECTED_FREQUENCY:

The correction of bias_correction gain with CORRECTED_FREQUENCY is necessary, since the high_rez_bias is summed at CORRECTED_FREQUENCY samples/sec in ahrs_update_accel (line 276).

Rate_correction is accumulated in ahrs_update_accel (line 256) but is then zeroed in ahrs_propagate (line 173). This means that rate_correction must be scaled with the ratio ahrs_propagate frequency / ahrs_update_accel frequency.

Ahrs_update_accel is called when accelerometer reading is available. Ahrs_propagate is called when gyrometer reading is available. I guess the frequency ratio is 1, for aspirin IMU, but is that always the case? If the ratio is always 1, then why accumulate rate_correction in ahrs_update_accel?

The ratio is not always 1, although as you said it's the case for aspirin.
But I don't see the where the problem is with accumulating in the update and applying the correction when you actually compute a new attitude by integrating the angular rates.

Just a (partial) paste from email so we don't loose it:

We want to set the gains in ahrs_update_accel function in such way that rate and bias correction doesn’t change with frequency.

At least to some extent... yes. Not sure over what range this actually can scale.
Keep in mind that if you lower the update frequency, you actually put more confidence in your accel/mag measurements by scaling with frequency.
This is fine as long as your measurement is good...

Assuming we want to calculate a correction that essentially stays the same regardless of the frequency the update is running at, then you basically only care how long ago you added the last correction. At what time this correction is actually applied doesn't matter as long as it is applied when the next attitude is computed and never applied twice...
I think this is already the case and it should be fine.

Different examples:

1. The unusual case: accel freq (correction): 500, gyro freq (propagate): 100
   You accumulate: 5*residual/500 = residual/100, which you apply 100 times per second, so after 1s you applied: residual
2. accel freq: 500, gyro freq: 500
   You accumulate: residual/500 which is applied with 500Hz: total correction per second: residual
3. accel freq: 100, gyro freq 500
   Every 5th propagation you have a correction with residual/100, so in total the same again

The different examples makes it clearer, and the scaling with frequency should be OK like this.

About the filter gains tuning: our aim is to set the filter frequency response with accel_attitude_gain and accel_gyrobias_gain.

int_cmpl_quat simplified schematics

Int_cmpl_quat filter is a second order complementary filter, which very simplified schematics are:

Complementary filter bode diagram

Left (red): angle from accelerometer input, filtered angle output
Right (blue): angle rate from gyrometer input, filtered angle output

To measure attitude angles, gyrometers measurements are integrated. The result of integration is accurate for short term, but gyro bias is accumulated, which results in long term errors (drift). On the other hand, accelerometers can be used to measure angles directly, but they suffer from noise due to vibrations. The measurement is then only accurate when averaged over a long term.
Complementary filter takes advantage of both sensors, using a low-pass filter on accelerometer readings and high pass filter on gyrometers readings, to estimate attitude angles.
Filter cut-off frequency and damping are set with K1 and K2 gains.

Filter transfer function

K1: rate gain
K2: bias (integral) gain

estimated_angle(p)= p²/(p²+K1 p +K2)* angle_rate/p + (K1 p + K2)/(p²+K1 p +K2)* angle

with:
angle_rate: angle rate from gyro measurement
angle: angle from accelerometer measurement

denominator: p² + 2_zeta_w0 p + w0²
w0: filter cut-off frequency (rd/s)
zeta: filter damping factor

K2=w0²
K1=2_zeta_w0

Filter Tuning

From the above equations, to tune the low-pass filter cut-off frequency, the relationship between K1 (rate gain) and K2 (biais gain) is:

w0=K1/(2_zeta)
w0²= K1²/(2_zeta)²=K2

For a given (optimal) damping, K1 (accel_attitude_gain) and K2 (accel_gyrobias_gain) can’t be tuned independently:

To reduce the complementary filter cut-off frequency by a factor X, accel_attitude_gain should be reduced by a factor X, and accel_gyrobias_gain by a factor X².

Heuristic on acceleration norm

The same applies for use_gravity_heuristic: if rate_correction is divided by inv_weight, high_rez_bias should be divided by inv_weight* inv_weight.
For now, rate_correction and high_rez_bias are both divided by inv_weight. The low pass filter frequency response with inv_weight varying from 1 to 50 is then:

The damping factor zeta becomes too low when inv_weight>1. This may introduce low frequency oscillations, accel noise being amplified at resonant frequency?

Possible Fix?

high_rez_bias should be divided by inv_weight² in order to keep the damping zeta together with lowering the cut-off frequency:

Pseudo-code suggestion:
inv_weight = 7 * abs(accel_norm-9.81)/9.81
rate_correction += -residual / inv_rate_scale / inv_weight
high_rez_bias += (residual / inv_bias_gain) * 32 / (2_inv_weight_inv_weight)

Hi Loic,

I don't really have time to implement, let alone test this at the moment...
With inv_weight it was essentially a hyperbolic scaling of the residual depending on the gravity norm.
Maybe a linear scaling makes more sense, as done in commit 5fdbf28?

Scaling bias with weight² sounds right.
Would be great if you could do that...

Hi,

Today, I tested the filter with various settings, to confirm theory above ;-)
I have by-passed ahrs_aligner and set 60°/s initial bias to the gyrometer q, to see how the bias is corrected by accelerometers.
First, the accel_attitude_gain is set to 16, 32 and 64, accel_gyrobias_gain=32:

As expected, the damping increases with rate_correction. Note that when accel_attitude_gain is too low there is some overshoot. There are oscillations at the begining that i cannot explain, maybe overflows?

Then, another setting to increase response time of the filter, keeping the damping high enough:

Finally, I forced weight=0.25, with:
inv_rate_scale = 2 * 4096 * AHRS_CORRECT_FREQUENCY / (weight * ahrs_impl.accel_attitude_gain);
int32_t inv_bias_gain = 3 * AHRS_CORRECT_FREQUENCY / (weight * ahrs_impl.accel_gyrobias_gain);

Compared to weight=0.5 (weight²=0.25)
inv_rate_scale = 2 * 4096 * AHRS_CORRECT_FREQUENCY / (weight * ahrs_impl.accel_attitude_gain);
int32_t inv_bias_gain = 3 * AHRS_CORRECT_FREQUENCY / (weight * weight * ahrs_impl.accel_gyrobias_gain);

This confirms that scaling bias with weight² keeps the damping of the filter. I will implement this...
Maybe it would also be nicer for the end-users to set directly the cut-off frequency w0 and damping zeta, instead of rate_correction and bias_correction gains?

Regards
Loïc

Nice!
Would be very much appreciated if you would implement this and make a pull request to merge it into this branch... (or just push it and tell me and I will add it to this branch).

About the oscillations, might be overflows... we should implement the same in the float version and compare...

And yes, setting cut-off frequency directly would indeed be nicer, someone just needs to implement that ;-)

Hi,

Here it is:
https://github.com/Drumettaz/paparazzi/commits/ahrs_int_cmpl_quat_correction_scaling

• Scaling of rate_correction with weight, and bias_correction with weight*weight.
• Setting of cut-off frequency and damping of the filter for the accel part
• Tested on ground with initial bias = 10°/s
• Tested in flight (indoor)

About oscilations at the begining of the bias correction, I think they come from calculation errors when attitude angles hit singularities (once every 6 seconds at 60°/s...)
But if someone confortable with fixed point arithmetics can check the different scalings and the saturations:

• bound(inv_bias_gain)
• bound(inv_rate_scale)
  It will be very nice!

Further developments:

• send weight to the telemetry
• add weight_factor to the settings?
• Remove inv_zeta from the settings? (no need to tune that in flight?)

Best regards
Loïc

Hm... Singularities? There shouldn't be any with these quaternion computations...

And why call it accel_inv_[omega|zeta] and not just accel_[omega|zeta]?
Same should be done for mag...

Don't think it hurts to be able to set these parameters while the systems is running...

It really depends on the implementation you tested, but the oscillations in
the beginning are quite common with such a big bias (60deg/sec) as the
attitude is thumbling arround so every time it is closer and then again
further away from the correction signal.

-Christophe

On Fri, Mar 8, 2013 at 11:31 PM, Felix Ruess [email protected]:

Hm... Singularities? There shouldn't be any with these quaternion
computations...

And why call it accel_inv_[omega|zeta] and not just accel_[omega|zeta]?
Same should be done for mag...

Don't think it hurts to be able to set these parameters while the systems
is running...

—
Reply to this email directly or view it on GitHubhttps://github.com//pull/371#issuecomment-14649853
.

Would it be possible to have the omega and zeta parameter in real units (i.e rad/s and no units between 0 and 1) ? I think it makes it more easy to tune.
For zeta, it could be settable in flight, but as a correct value is probably between 0.7 and 0.9, it is probably not the most important part to tune.

I'm working on a new version with omega and zeta parameters in universal units.

Good idea. Made the gravity_heuristic_factor configurable (set to zero to disable).
Also made it slightly more granular, so now a factor of 30 (default now) is the same as a previous factor of 3.

Does anyone else have some more input about default settings for the gravity_heuristic_factor? It is currently set to 30, but maybe the default should be lower (probably not completely zero/off)?

Some plots of my test with quad fixed on ground:


Accel_omega is 0.063 rd/s (filter settling time 100 s)

Beginning of the test, gravity_heuristic_factor = 30. when the motors are running it looks like the gyro bias is not well compensated since theta is moving slowly by 5 degres. (the IMU is not calibrated on purpose)

Then gravity_heuristic_factor = 0 at t=500 s. At t=830 s, the imu is cooled with cooling spay. The biais is well compensated. And the settling time is 100 s...

I think that gravity_heuristic_factor = 30, together with Accel_omega = 0.063 rd/s is a quite aggressive setting in case of vibrations for the aspirin. When the accel part of the filter bandwidth is too low, the gyro bias is not compensated fast enough, and the attitude angles get wrong. That's a combination of vibration_level * weight * accel_omega which makes the filter bandwidth.

Where is the limit for the filter accel bandwidth? it depends on the gyro bias stability. The more stable is the gyro bias, the lower you can set the accel bandwidth.

For now, the filter accel bandwidth is set to 0.125 rd/s in ahrs_int_cmpl_quat in master. Maybe we should keep that value as default, letting the users to reduce it if the gyro bias stability is good enough.
Same for gravity_heuristic_factor: it could be set to a lower value by default (10? 20?), and let the users increase it if the imu is well dampened and calibrated.

Thanks a lot for the tests Loic!
From looking at the weight, it seems to me that the value used for the gravity heuristic norm is not filtered enough. We don't really want to lower the gain when having vibrations, but rather when we have trajectory accelerations...
The weight (and hence the gain) actually increases sometimes due to the vibrations...

Also the filter value of this should probably depend on the update frequency as well...
Maybe a better filter (butterworth or so) would make sense instead of the really simple FIR?

Also did you use the latest version? The weight should go exactly to 1.0 when setting the gravity_heuristic_factor to zero.

No, that curves come from the tests I did last Thursday.
It's true that we want to measure trajectory accelerations (bandwidth = 50 Hz?) rather than noise at 512 Hz (for rotorcraft)
I gess that's the same for every accelerometer measurement used in the whole code (i.e. imu.accel). So maybe this filtering should be done in imu subsystem? That's another subject...

In quite a few cases you don't want to do additional filtering the imu subsystem, in order to not introduce lag.
Also some estimators work a lot better if you give them every (noisy) measurement instead of strongly filtering the values before...

In any case, could you maybe test again with a stronger filter on the norm (increase the FIR_FILTER_SIZE) and a factor of 10 or 20?
Hopefully we can then merge this soon...

Hello Felix, i have some time to do tests today.
My lisa_m quadcopter are ready to fly.

I lost the start of this thread.
What parameter i have to change to do this tests ?

I lost the start of this thread.
What parameter i have to change to do this tests ?

git remote update
git checkout ahrs_int_cmpl_quat_correction_scaling

There are several tests you could do:

1. check how well it flies with the current default parameters
2. add the ahrs_int_cmpl_quat settings file and reduce the gravity_heuristic_factor to e.g. 10 and check what difference it makes on your airframe
3. like Loic did, keep the quad on the ground, start the motors and plot the weight (in AHRS_QUAT_INT) message
4. if the weight changes a lot (due to vibrations when the motors are on), increase the FIR_FILTER_SIZE in the the ahrs_int_cmpl_quat.c source file to see if/how much it helps

Hi Felix and Loic, i do a simple flight today and do some fast maneuver.

As i understand this branch is intended to solve the problem of giro bias right ?
I assembled my quad last week using 4 new props
i not balanced the props and the quad flight very well but some time the quad not stay horizontal after the maneuver.

For example, i put full pitch or full roll + throttle and then i put reverse pitch or roll to stop the horizontal speed and when i release the sticks the quad not stay horizontal.

I need to use some pitch / roll to put quad horizontal.

After some time it work fine again and when i release the stick the quad stay perfect horizontal.

My frame a HK flywheel clone without imu damp.
Tomorrow i have more time and i will test again changing the FIR values.

BTW, for reference, i fly other autopilot using unbalanced props and the stabilization work very well (stay horizontal when i release the sticks).

@agressiva No, this doesn't "solve" the problem of gyro bias. That is not really possible with such a simple filter when you have bias drift AND trajectory accelerations.
But it should make this filter applicable over a wide range of propagate/update frequencies and also make it more tunable.

Did you check/log the weight reported in the AHRS_QUAT_INT message?

@Drumettaz I'm still wondering if we should add another gain which to make only the gyro bias correction tuneable... so you can set it according to your expected bias drift or lower it if you expect to do a lot of fast maneuvers...

Yes, the gyro bias is to be corrected. One of the purposes of this branch is to let the user configure the amount of confidence given to the accelerometer to correct the gyro biais.
Too much confidence: the estimated bias will be perturbated by accelerations others than gravity: g=9.81 m/s2. The computed angles will be dynamically wrong (bad dynamic accuracy)
Not enought confidence: the estimated bias will not follow the real bias changes. As the result, the computed angle may get wrong (bad static accuracy)
This "confidence" is set with the complementary filter bandwith. accel_omega = 0.063 rd/s means the accelerometer values will be filtered (or "averaged") during 100 seconds to estimate and correct the gyro bias.
With gravity heuristic factor set different to zero, this bandwidth is also reduced by a factor that can reach 0.15.
So the fliter response will be 100/0.15 = 667 seconds. You need a good gyro bias stability for that...
But the gravity heuristics only reduce the bandwith for a short time: when there are accelerations others than gravity. If the norm of measured accelerations is different to 9.81 m/s2, the confidence in accelerometer (to measure angles) is reduced.
In case of vibrations the gravity heuristic will always see accelerations higher than gravity and will reduce the bandwidth. The FIR filter can reduce the accelerations caused by vibrations, and keep only the "real" accelerations.

You may try higher FIR_FILTER_SIZE to filter the accelerations due to vibrations. If not succesfull, you can rise accel_omega. Then the bias will be corrected faster, but with less dynamic accuracy of the angles. A good compromise can be found between static and dynamic accuracy.

How to find that compromise?

• With a bad static accuracy, you need to correct the attitude permanently to hover.
• With a bad dynamic accuracy of the angles: push the elevator stick for 1 or 2 seconds and then release the stick. The rotorcraft pitch-up instead of coming back to horizontal. The behaviour is bad for horizontal navigation, and you can't rise the horizontal guidance PID gains.

This can also be seen in simulation with nps using quad_lisa_m2 model.

One of the purposes of this branch is to let the user configure the amount of confidence given to the accelerometer to correct the gyro bias.

Not only the bias, but also more importantly the attitude angles.
There is still now way to separately configure how much to correct the bias alone...

flixr said: " I'm still wondering if we should add another gain which to make only the gyro bias correction tuneable... so you can set it according to your expected bias drift or lower it if you expect to do a lot of fast maneuvers..."

If we look at the simplified schematics at the begining of the discussion, where would you put that gain?
Well tuned and filtered, the "gravity heuristics" part of the fiter should do that job?

In the simplified schematics that would probably be the K2 gain... but you can't set that independently of the rate correction, since if you change the cut off freq (w0) you also change K1.

The gravity heuristics lower the correction of the attitude AND the bias... but just for the sake of argument assume that you have no gyro bias drift (in reality probably rather something quite low). It will then falsely also correct the gyro bias, where you would only want to correct the attitude angles...

In case you have a good gyro, with very stable bias. You can reduce accel_omega, which reduces K1 and K2 gains. When K1 and K2 gains are reduced, it doesn't reduce the bias correction, it reduces the bandwitdh of the bias correction. It will take more time for the accelerometer to correct the gyro bias.
In that case it won't falsely correct the attitude angles, since the "residual" will be close to zero...

Reducing omega is not what you want... you can still have lot's of noise on the gyro and hence integrate to a wrong attitude and you want to correct that using the accel. But you don't necessarily want to "correct" the bias.
Your assumption is that bias error and attitude error are always coupled, which is not (necessarily) the case.

Hello felix, i do some flight using FIR_FILTER_SIZE = 16 and AHRS_GRAVITY_HEURISTIC_FACTOR = 50
The quad fly very ... very stable even with unbalanced props.

Tomorrow i will reduce the GRAVITY_HEURISTIC back to 30 and then back to 10 and report the result.

Starting to use int_cmpl_quat in a fixedwing of mine, let's see and keep an eye on PFD and a finger on the mode Manual RC switch ;). Test findings comming month(s)...

@agressiva, any updates?
I would like to merge this rather sooner than later... (meaning in the next days).
If we find better default values we can still change them after merging to master...

Hy felix, my hexacopter was stolen last week.
i performed some tests and its flying very well with this branch with default values

unfortunately i cant do more test for a few mounts.
Date: Thu, 10 Oct 2013 09:56:28 -0700
From: [email protected]
To: [email protected]
CC: [email protected]
Subject: Re: [paparazzi] ahrs_int_cmpl_quat frequency scaling (#371)

@agressiva, any updates?

I would like to merge this rather sooner than later... (meaning in the next days).

If we find better default values we can still change them after merging to master...

—
Reply to this email directly or view it on GitHub.

my hexacopter was stolen last week.

Very sorry to hear that Eduardo, that really sucks...

i performed some tests and its flying very well with this branch with default values

Thanks a lot for testing again!