Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

proper handling of BODY_TO_IMU in all AHRS algos #93

Closed
flixr opened this issue Nov 28, 2011 · 21 comments

Comments

@flixr
Copy link
Member

commented Nov 28, 2011

Currently you can't really use IMU_TO_BODY to set a totally different
orientation, only to correct for small errors.

e.g. for ahrs_float_dcm:

Luke Ionno wrote on the mailing list:

I solved the problem... There is a small bug in the DCM body-to-IMU
rotation implementation, when the USE_GPS flag is enabled. The GPS heading
(which is obviously not dependent on the orientation of the IMU) is
incorporated into the state matrix, before it is rotated as defined by the
body-to-IMU variables... This of course results in an error in the reported
heading.

Was very briefly discussed already a while ago:
http://lists.gnu.org/archive/html/paparazzi-devel/2011-10/msg00021.html

My quick solution was to add IMU_BODY_TO_IMU_PSI to the ground_course, to
compensate for it later being subtracted by the body-to-IMU rotation:
float ground_course = ((float)gps.course)/1.e7 - M_PI + IMU_BODY_TO_IMU_PSI;

This only solves part of the problem, e.g. in ahrs_update_accel the
correction of centrifugal force is not correct if the imu is not
aligned with the body.
Also that quick solution only works if BODY_TO_IMU doesn't have any
significant rotation on phi and theta.

This can all be fixed of course, but I guess the question still is
whether to run the ahrs in body or in imu frame, both has it's
advantages and drawbacks....

@dewagter

This comment has been minimized.

Copy link
Member

commented Nov 28, 2011

some more constraints to the problem:

e.g.1: on Lisa-M, the long side of the board happens to be the roll axis,
while many mount the board with the long side along the fuselage. The
easiest, most logical, most straight forward option is simply to define the
IMU X as Y and vice versa (and reverse the Y sign). This way, raw and
calibrated data correspond again (very confusing to ask raw imu and get X
toward the wing while calibrated data has X towards the nose), rolling the
plane moves the p gyro making everything easy and understandable etc...

(sample of a case where you really want to run the ahrs in body)

In essence: any filter with more than pure kinematic model only (e.g. the
aircraft filter with turnrate x V_total, must be run in the axis of this
model: in this case: body axis)

e.g.2: on the quadrotor the imu is mounted at 45 degrees ... rotating the
gyro measurements to body before integration sounds weird because you will
then mix the gyro biases. Theoretically this is no problem though, as in
the 45 deg case the roll-rate is (p-gyro + q-gyro) / sqrt(2). Running this
filter in body (rotating the raw imu data) simply gives a filter that
estimates combinations of biases... from there on everything is simple
again.

e.g.3: we update the imu-2-body during a tuning flight: I do not think
changing the imu2body during flight is possible anymore if the raw
measurements are rotated. that would make the filter inconsistent?

Conclusion:

what about introducing an AHRS frame;

imu2ahrs (hardcoded) is done on RAW(*footnote1) IMU data
ahrs2body (inflight tunable) is done after AHRS

In most cases imu2ahrs will be as simple as channel allocation and sign
(and optimized away by the compiler if it is close enough to the other sign
and scaling code.)
e.g. 1. lisa-m RAW p gyro reacts to roll... all ahrs filters make sense now
e.g. 2. if you use a pure kinematic filter you can choose to run the ahrs
in imu coordinates, or run it in 45 deg rotated. Overhead after compiling:
6 fixedpoint summations. (instead of gyro_p * scale * sign you now have
(gyrop+gyroq) * differentscale * sign. (*footnote1)
e.g. 3: inflight you can only change the ahrs2body which does not affect
filter consistency

Moreover I think in the future this approach offers most advantages for
everyone from the "run all in body" and from the "run all in ahrs" group.
E.g. you could still run it all in imu axis if the ahrs allows this.

footnote1: I would prefer to rotate imu2ahrs on raw data in order to have
the raw p-gyro value reacting to roll: if your gyros are not perfectly
perpendicular or have different scales, relative scaling and cross terms
need to be removed first before rotating. this means I probably have to
abandon the wish to have raw p-gyro react to roll. I can live with this if
the RAW sensor data in the telemetry is renamed to be more clear: RAW
gx-imu gy-imu ... CALIBRATED: gx-ahrs, gy-ahrs, ....

-Christophe

On Mon, Nov 28, 2011 at 7:56 PM, Felix Ruess <
reply@reply.github.com

wrote:

Currently you can't really use IMU_TO_BODY to set a totally different
orientation, only to correct for small errors.

e.g. for ahrs_float_dcm:

I solved the problem... There is a small bug in the DCM body-to-IMU
rotation implementation, when the USE_GPS flag is enabled. The GPS
heading
(which is obviously not dependent on the orientation of the IMU) is
incorporated into the state matrix, before it is rotated as defined by
the
body-to-IMU variables... This of course results in an error in the
reported
heading.

Was very briefly discussed already a while ago:
http://lists.gnu.org/archive/html/paparazzi-devel/2011-10/msg00021.html

My quick solution was to add IMU_BODY_TO_IMU_PSI to the ground_course, to
compensate for it later being subtracted by the body-to-IMU rotation:
float ground_course = ((float)gps.course)/1.e7 - M_PI +
IMU_BODY_TO_IMU_PSI;

This only solves part of the problem, e.g. in ahrs_update_accel the
correction of centrifugal force is not correct if the imu is not
aligned with the body.
Also that quick solution only works if BODY_TO_IMU doesn't have any
significant rotation on phi and theta.

This can all be fixed of course, but I guess the question still is
whether to run the ahrs in body or in imu frame, both has it's
advantages and drawbacks....


Reply to this email directly or view it on GitHub:
#93

@flixr

This comment has been minimized.

Copy link
Member Author

commented Nov 29, 2011

As @dewagter said:

e.g.1: on Lisa-M, the long side of the board happens to be the roll axis,
while many mount the board with the long side along the fuselage.

I get what you are trying to say, but if the roll axis = long side of
board = along fuselage, things are fine...
Or do you mean the long side of aspirin? Actually judging from the silkscreen
aspirin 1.3 and aspirin 1.5 have different axes.

The easiest, most logical, most straight forward option is simply to define the
IMU X as Y and vice versa (and reverse the Y sign). This way, raw and
calibrated data correspond again (very confusing to ask raw imu and get X
toward the wing while calibrated data has X towards the nose), rolling the
plane moves the p gyro making everything easy and understandable etc...

While this is basically also what I do in my trainer, we want to avoid to tell
users to redefine channels and signs.
And it is quite confusing to have different axes for raw and scaled values...

In essence: any filter with more than pure kinematic model only (e.g. the
aircraft filter with turnrate x V_total, must be run in the axis of this
model: in this case: body axis)

I disagree here, you can also just transform into the imu frame...

e.g.2: on the quadrotor the imu is mounted at 45 degrees ... rotating the
gyro measurements to body before integration sounds weird because you will
then mix the gyro biases. Theoretically this is no problem though, as in
the 45 deg case the roll-rate is (p-gyro + q-gyro) / sqrt(2). Running this
filter in body (rotating the raw imu data) simply gives a filter that
estimates combinations of biases... from there on everything is simple
again.

I'm not really fond of doing integration and bias estimation in body frame.

E.g. if you integrate at a high rate in body frame, you

  1. need to transform every measurement which just costs computing power
  2. introduce more errors since you loose a bit of precision with every
    transform
    (especially with fixed point)

For bias estimation it's hard to specify a proper bias model
(random walk, first order markov,.. on each axis independently)
and estimate them, if your axes are not the actual sensor axes.

e.g.3: we update the imu-2-body during a tuning flight: I do not think
changing the imu2body during flight is possible anymore if the raw
measurements are rotated. that would make the filter inconsistent?

As you normally only make small adjustments in the air, I guess that in most
cases that would actually not be a problem, but definitely depends on
the filter.

what about introducing an AHRS frame;

imu2ahrs (hardcoded) is done on RAW(*footnote1) IMU data
ahrs2body (inflight tunable) is done after AHRS

Hm... maybe not such a bad idea, but I'm not totally convinced...

In most cases imu2ahrs will be as simple as channel allocation and sign
(and optimized away by the compiler if it is close enough to the other sign
and scaling code.)
e.g. 1. lisa-m RAW p gyro reacts to roll... all ahrs filters make sense now

Raw values are only ever used for debugging and calibration... ahrs
only uses scaled values
but yes, it would be nice of course if that would correspond to the body axes.

e.g. 2. if you use a pure kinematic filter you can choose to run the ahrs
in imu coordinates, or run it in 45 deg rotated. Overhead after compiling:
6 fixedpoint summations. (instead of gyro_p * scale * sign you now have
(gyrop+gyroq) * differentscale * sign. (*footnote1)

I still don't really see the advantage of running the ahrs in body
frame instead of
transforming the results to the body frame when they are actually used.

e.g. 3: inflight you can only change the ahrs2body which does not affect
filter consistency

As stated above I think that if you run the whole ahrs in imu frame there will
only be very few cases where this would actually affect filter consistency.
But maybe I'm missing something?

Moreover I think in the future this approach offers most advantages for
everyone from the "run all in body" and from the "run all in ahrs" group.
E.g. you could still run it all in imu axis if the ahrs allows this.

To me that is actually the only advantage of running the ahrs in body frame,
we don't have to change/fix some of the current ahrs algos to properly
take this into account.
But is that really worth it?

footnote1: I would prefer to rotate imu2ahrs on raw data in order to have
the raw p-gyro value reacting to roll: if your gyros are not perfectly
perpendicular or have different scales, relative scaling and cross terms
need to be removed first before rotating. this means I probably have to
abandon the wish to have raw p-gyro react to roll. I can live with this if
the RAW sensor data in the telemetry is renamed to be more clear: RAW
gx-imu gy-imu ... CALIBRATED: gx-ahrs, gy-ahrs, ....

jep..

So IMHO the proper way to do things is to run the filter in IMU frame
and transform other measurements into the imu frame if needed.
But of course that also means some work for the current ahrs algos...

Raw and scaled values would have the same coordinate frame, the actual
sensor/imu frame...
But for 90deg rotations we probably could "automatically" switch the
channels and signs?
A problem then is that the IMU calibration has to be adjusted/switched
as well, since that refers to the actual hardware sensors.

Cheers, Felix

@dewagter

This comment has been minimized.

Copy link
Member

commented Nov 29, 2011

On Tue, Nov 29, 2011 at 1:01 PM, Felix Ruess <
reply@reply.github.com

wrote:

As @dewagter said:

e.g.1: on Lisa-M, the long side of the board happens to be the roll axis,
while many mount the board with the long side along the fuselage.

I get what you are trying to say, but if the roll axis = long side of
board = along fuselage, things are fine...
Or do you mean the long side of aspirin? Actually judging from the
silkscreen
aspirin 1.3 and aspirin 1.5 have different axes.

sorry badly phrased. in any case with aspirin 1.5 things are not good in
our planes :-)

The easiest, most logical, most straight forward option is simply to
define the
IMU X as Y and vice versa (and reverse the Y sign). This way, raw and
calibrated data correspond again (very confusing to ask raw imu and get X
toward the wing while calibrated data has X towards the nose), rolling
the
plane moves the p gyro making everything easy and understandable etc...

While this is basically also what I do in my trainer, we want to avoid to
tell
users to redefine channels and signs.
And it is quite confusing to have different axes for raw and scaled
values...

Now in all our quadrotors RAW and CALIBRATED data are NOT in the same
orientation. Apparently calibrated data is rotated to body.

In essence: any filter with more than pure kinematic model only (e.g. the
aircraft filter with turnrate x V_total, must be run in the axis of this
model: in this case: body axis)

I disagree here, you can also just transform into the imu frame...

Filters like the dcm_float use a "model" (namely: gps_v_total is in ahrs-x
direction). you can not rotate the gps to imu frame in this case but need
to do the rotation somewhere inside the ahrs (beween the propagation and
feedback).

That was the motivation to call the intermediate coordinate system:
ahrs_frame

e.g.2: on the quadrotor the imu is mounted at 45 degrees ... rotating the
gyro measurements to body before integration sounds weird because you
will
then mix the gyro biases. Theoretically this is no problem though, as in
the 45 deg case the roll-rate is (p-gyro + q-gyro) / sqrt(2). Running
this
filter in body (rotating the raw imu data) simply gives a filter that
estimates combinations of biases... from there on everything is simple
again.

I'm not really fond of doing integration and bias estimation in body frame.

E.g. if you integrate at a high rate in body frame, you

  1. need to transform every measurement which just costs computing power

6 additions only for 90/45 deg

  1. introduce more errors since you loose a bit of precision with every
    transform
    (especially with fixed point)

only in 45 deg mode

For bias estimation it's hard to specify a proper bias model
(random walk, first order markov,.. on each axis independently)
and estimate them, if your axes are not the actual sensor axes.

true. this is an issue: the idea of splitting imu2body into imu2ahrs and
ahrs2body still lets you choose

e.g.3: we update the imu-2-body during a tuning flight: I do not think
changing the imu2body during flight is possible anymore if the raw
measurements are rotated. that would make the filter inconsistent?

As you normally only make small adjustments in the air, I guess that in
most
cases that would actually not be a problem, but definitely depends on
the filter.

what about introducing an AHRS frame;

imu2ahrs (hardcoded) is done on RAW(*footnote1) IMU data
ahrs2body (inflight tunable) is done after AHRS

Hm... maybe not such a bad idea, but I'm not totally convinced...

In most cases imu2ahrs will be as simple as channel allocation and sign
(and optimized away by the compiler if it is close enough to the other
sign
and scaling code.)
e.g. 1. lisa-m RAW p gyro reacts to roll... all ahrs filters make sense
now

Raw values are only ever used for debugging and calibration... ahrs
only uses scaled values
but yes, it would be nice of course if that would correspond to the body
axes.

e.g. 2. if you use a pure kinematic filter you can choose to run the ahrs
in imu coordinates, or run it in 45 deg rotated. Overhead after
compiling:
6 fixedpoint summations. (instead of gyro_p * scale * sign you now have
(gyrop+gyroq) * differentscale * sign. (*footnote1)

I still don't really see the advantage of running the ahrs in body
frame instead of
transforming the results to the body frame when they are actually used.

Well, let me put it this way: if you have simple 90 degree rotations, then
its purely a matter of channel allocations and signs and then things
suddenly are clean, logical, and it costs no computation power if hardcoded.

And I think it's better to define a IMU2AHRS of -90 deg than to define
P-GYRO_CHANNEL 2 Q-GYRO_CHANNEL 1 etc...

e.g. 3: inflight you can only change the ahrs2body which does not affect
filter consistency

As stated above I think that if you run the whole ahrs in imu frame there
will
only be very few cases where this would actually affect filter consistency.
But maybe I'm missing something?

Moreover I think in the future this approach offers most advantages for
everyone from the "run all in body" and from the "run all in ahrs" group.
E.g. you could still run it all in imu axis if the ahrs allows this.

To me that is actually the only advantage of running the ahrs in body
frame,
we don't have to change/fix some of the current ahrs algos to properly
take this into account.
But is that really worth it?

footnote1: I would prefer to rotate imu2ahrs on raw data in order to have
the raw p-gyro value reacting to roll: if your gyros are not perfectly
perpendicular or have different scales, relative scaling and cross terms
need to be removed first before rotating. this means I probably have to
abandon the wish to have raw p-gyro react to roll. I can live with this
if
the RAW sensor data in the telemetry is renamed to be more clear: RAW
gx-imu gy-imu ... CALIBRATED: gx-ahrs, gy-ahrs, ....

jep..

So IMHO the proper way to do things is to run the filter in IMU frame
and transform other measurements into the imu frame if needed.
But of course that also means some work for the current ahrs algos...

Well I almost 100% agree, except for the 1 detail: I would like to make it
possible to rotate sensor data easily to make many simple scenarios (like
90 deg rotation) much much easier...

Idea: when possible run the ahrs in body. (I can not think of a reason NOT
to unrotate the RAW data in the extremely common 90 degree mounting problem
which causes no runtime overhead when hardcoded)

*
*
Note: RAW is not RAW anyway as the sensor driver header also swaps channels
etc... so the extra 90/180 deg rotation to body before you call it RAW is
not that terrible after all.

Raw and scaled values would have the same coordinate frame, the actual
sensor/imu frame...
But for 90deg rotations we probably could "automatically" switch the
channels and signs?
A problem then is that the IMU calibration has to be adjusted/switched
as well, since that refers to the actual hardware sensors.

interesting issue: how to prevent people from first calibrating and then
setting the imu2ahrs angles?

Cheers, Felix


Reply to this email directly or view it on GitHub:
#93 (comment)

@flixr

This comment has been minimized.

Copy link
Member Author

commented Nov 29, 2011

@dewagter said:

Now in all our quadrotors RAW and CALIBRATED data are NOT in the same orientation. Apparently calibrated data is rotated to body.

Huh? If you look in the ImuScaleX macros (subsystems/imu.h) , you can clearly see that currently there is no rotation/transformation, just the scaling/neutral. The only exception I know about is for mag when you have IMU_MAG_45_HACK defined.

Filters like the dcm_float use a "model" (namely: gps_v_total is in ahrs-x direction). you can not rotate the gps to imu frame in this case but need to do the rotation somewhere inside the ahrs (beween the propagation and feedback).

I don't see the fundamental problem here... the filter should just not assume that ahrs-x direction is the same as body-x direction, and since you know the body2imu rotation. Of course you have to change the ahrs code and it's not so easy/clear anymore...

  1. need to transform every measurement which just costs computing power

6 additions only for 90/45 deg

Well, but 90/45 degrees are special cases, and why should we limit ourselves to these special cases again?

Well, let me put it this way: if you have simple 90 degree rotations, then its purely a matter of channel allocations and signs and then things suddenly are clean, logical, and it costs no computation power if hardcoded. And I think it's better to define a IMU2AHRS of -90 deg than to define P-GYRO_CHANNEL 2 Q-GYRO_CHANNEL 1 etc...

Sure, totally agree on the premise, but if we do that for IMU2AHRS why can't we do the same for the BODY_TO_IMU?
Or did you just want to allow 90 deg rotations for IMU2AHRS?

I totally agree that we need a simple way to deal with the common 90 deg mounting problem, without having to redefine channels manually. But I'm just not totally convinced that another coordinate frame (limited to 90 deg rotations if I understood correctly) for this and then adjust something "on top of that" is the way to go...
Also the IMU calibration potentially is a big problem if you start changing channels... e.g. you can't just use the calibration file you got from joby...

@flixr

This comment has been minimized.

Copy link
Member Author

commented Dec 2, 2011

I did a very brief review of some of the AHRS algos:

  • "int_cmpl_quat" seems to handle BODY_TO_IMU correctly in all cases
  • "int_cmpl_euler" does not handle the accel and mag updates correctly with non-negligible BODY_TO_IMU rotations
  • "float_dcm" does not handle the accel, gps and mag updates correctly with non-negligible BODY_TO_IMU rotations
@flixr

This comment has been minimized.

Copy link
Member Author

commented Dec 7, 2011

Update:
"int_cmpl_quat" currently does not deal with body_to_imu correctly when doing the gravity update, should be fixed with pull request #100. There AHRS_GRAVITY_UPDATE_COORDINATED_TURN is also already enabled for fixedwings.
In pull request #98 subsystem makefiles to use ahrs type "float_cmpl_rmat" are added and the gravity update is fixed in the same way.

@ghost ghost assigned flixr and dewagter Dec 7, 2011
@dewagter

This comment has been minimized.

Copy link
Member

commented Feb 15, 2012

idea?

airframe.xml

section name=imu
orientation_servo_connectors value="back/front/left/right/up/down"
orientation_microcontroller value="back/front/left/right/up/down"
/section

at the end of imu code:

#include "imu_alignment.h"

imu_alignment.h

if LISA_M // 24 mounting options per board
if SERVO=back
if MCU=up
x,y,z = y, -x, z
elif MCU=down
x,y,w = -y,x,-z
...

@flixr

This comment has been minimized.

Copy link
Member Author

commented Feb 17, 2012

hm.... I really don't know how to best do this.. this will screw up your calibration again.

Looking at this from the other side:

  • I don't think anything actually uses the imu values directly except for the ahrs algoritms of course.
    • Ok, and when you want to look at them for debugging.
  • the only ahrs algo that doesn't deal with this BODY_TO_IMU correctly is float_dcm.
  • since imu_frame = body_frame should not be assumed anyway, why not let it deal with the whole rotation?

So while I certainly agree that it can be annoying to have the imu values not rotated, is it really such a big issue?
I mean after all that is the orientation you have the imu mounted in... e.g. on a lot of rotorcrafts you have it mounted with 45deg to the front...
Or is the only issue that you can't use float_dcm atm?
Then maybe we should rather fix that, or use int_cmpl_quat?

Of course a nice solution to set the imu orientation for these common cases of 90,180 deg rotations would be good, but IMHO there are more drawbacks to this than benefits:

  • + imu data in a more intuitive frame
  • +- if BODY_TO_IMU is really set to zero, then it is more efficient, but if not (probably most cases?) you have to do the rotation anyway....
  • - you have to start switching the axes/signs of the calibration data and you have to be very careful with what you do your calibration (e.g. your airframe with these rotations or the general test_imu target which doesn't have these?)
  • - now you have multiple configuration options to define the orientation of your IMU, which probably makes it harder instead of easier for the user
  • - the code get's harder to read and if you want to find out what actually happens, you have to go through a lot more defines and ifdefs

All that being said, if anyone really wants/needs this we can still add a hack, and people can use that hack if they want or not. Personally I'm just not convinced it is worth it... but you are free to disagree and add that of course!

@flixr

This comment has been minimized.

Copy link
Member Author

commented Feb 20, 2012

oh: I totally forgot about "int_cmpl_euler": since that wasn't fixed since the brief review I posted earlier, it still applies:

  • "int_cmpl_euler" does not handle the accel and mag updates correctly with non-negligible BODY_TO_IMU rotations.
  • I honestly don't see any point in putting energy into this one, better use "int_cmpl_quat" anyway...
@dewagter

This comment has been minimized.

Copy link
Member

commented Feb 20, 2012

The int_cmpl_quat has significantly different filter gains and thereby
behaves differently, especially in turbulent weather, as accelerometers are
thrusted more. We are working on it as adding input filters help a huge
amount. I'll also try to fix dcm_float some day and will stop complaining
;-)

-Christophe

On Mon, Feb 20, 2012 at 2:02 PM, Felix Ruess <
reply@reply.github.com

wrote:

oh: I totally forgot about "int_cmpl_euler": since that wasn't fixed since
the brief review I posted earlier, it still applies:

  • "int_cmpl_euler" does not handle the accel and mag updates correctly
    with non-negligible BODY_TO_IMU rotations.
  • I honestly don't see any point in putting energy into this one, better
    use "int_cmpl_quat" anyway...

Reply to this email directly or view it on GitHub:
#93 (comment)

@flixr

This comment has been minimized.

Copy link
Member Author

commented Feb 20, 2012

True, I get the problem... as I said, feel free to add a hack for the time
being, but IMHO that is not a long term solution...

AHRS_GRAVITY_UPDATE_NORM_HEURISTIC like in float_cmpl would probably help
here as well?
It lowers the gain of the gravity update based on acceleration norm.
Have you tested with float_cmpl and the norm heuristic instead of
int_cmpl_quat?

Also maybe we should make the gains configurable anyway? At least for the
float version....

Also regarding input filters, it would probably be nice to not re-implement
the filtering in each ahrs algorithm, but just have some filters that you
can apply.

On Mon, Feb 20, 2012 at 2:37 PM, Christophe De Wagter <
reply@reply.github.com

wrote:

The int_cmpl_quat has significantly different filter gains and thereby
behaves differently, especially in turbulent weather, as accelerometers are
thrusted more. We are working on it as adding input filters help a huge
amount. I'll also try to fix dcm_float some day and will stop complaining
;-)

-Christophe

On Mon, Feb 20, 2012 at 2:02 PM, Felix Ruess <
reply@reply.github.com

wrote:

oh: I totally forgot about "int_cmpl_euler": since that wasn't fixed
since
the brief review I posted earlier, it still applies:

  • "int_cmpl_euler" does not handle the accel and mag updates correctly
    with non-negligible BODY_TO_IMU rotations.
  • I honestly don't see any point in putting energy into this one, better
    use "int_cmpl_quat" anyway...

Reply to this email directly or view it on GitHub:
#93 (comment)


Reply to this email directly or view it on GitHub:
#93 (comment)

@dewagter

This comment has been minimized.

Copy link
Member

commented Feb 20, 2012

The biggest "issue" we have is with faster planes and fast-period motions
like dutch-roll etc... Planes like the microjet seem to show quite a bit of
those. The feedback being the gyro's multiplied with the GPS-speed that
easily ends up giving lateral accelerations of 10 g. A low-pass of the gyro
(only in the feedback, not in the integration) that is slower than the fast
eigenmotions removes this trouble.

-Christophe

On Mon, Feb 20, 2012 at 3:28 PM, Felix Ruess <
reply@reply.github.com

wrote:

True, I get the problem... as I said, feel free to add a hack for the time
being, but IMHO that is not a long term solution...

AHRS_GRAVITY_UPDATE_NORM_HEURISTIC like in float_cmpl would probably help
here as well?
It lowers the gain of the gravity update based on acceleration norm.
Have you tested with float_cmpl and the norm heuristic instead of
int_cmpl_quat?

Also maybe we should make the gains configurable anyway? At least for the
float version....

Also regarding input filters, it would probably be nice to not re-implement
the filtering in each ahrs algorithm, but just have some filters that you
can apply.

On Mon, Feb 20, 2012 at 2:37 PM, Christophe De Wagter <
reply@reply.github.com

wrote:

The int_cmpl_quat has significantly different filter gains and thereby
behaves differently, especially in turbulent weather, as accelerometers
are
thrusted more. We are working on it as adding input filters help a huge
amount. I'll also try to fix dcm_float some day and will stop complaining
;-)

-Christophe

On Mon, Feb 20, 2012 at 2:02 PM, Felix Ruess <
reply@reply.github.com

wrote:

oh: I totally forgot about "int_cmpl_euler": since that wasn't fixed
since
the brief review I posted earlier, it still applies:

  • "int_cmpl_euler" does not handle the accel and mag updates correctly
    with non-negligible BODY_TO_IMU rotations.
  • I honestly don't see any point in putting energy into this one,
    better
    use "int_cmpl_quat" anyway...

Reply to this email directly or view it on GitHub:
#93 (comment)


Reply to this email directly or view it on GitHub:
#93 (comment)


Reply to this email directly or view it on GitHub:
#93 (comment)

@flixr

This comment has been minimized.

Copy link
Member Author

commented Apr 6, 2012

So if I understand you correctly these issues arise due the assumption we make:
The norm of the 3D GPS velocity measurement is always in the direction of the body x-axis
Which is not true in case of side-slip, tail waggling, pitch oscillations, dutch roll, etc...

If your velocity vector is correct, there should be no such problems.
If you roll and your velocity is tangential to your x-axis (the case of a clean roll) then everything is fine: rate (cross) velocity is zero and you don't need to correct the acceleration measurement.

@dewagter

This comment has been minimized.

Copy link
Member

commented Apr 7, 2012

Hi Felix,

It is not so much the speed that is wrong, it is the turn rate. The "model"
says centrifugal force is speed x turn rate.

With no dutch roll or pitch oscillations the gyro measures just that. E.g.
360 deg in 30 sec = 12deg/sec which at 30m/s gives 6m/s2 centrifugal force
or roughly 35 degree of roll.

But if you now add a fast period motion eigenmotion of only 150 deg/sec
this gives 60 m/s2 errors. Although this is an oscillation and thus on
average is correct the kalman filters tends to get in trouble when the roll
measurement is -5.4 g left = -178 degree and then 6.6g right = + 179deg
while the average should be around 35.

The same happens in an aiframe with 20m/s2 vibrations from the
accelerometers. The non-linearity of vector feedback in the attitude
filters results in highly underestimating the angles.

When vibrations are higher than gravity the filter even could temporarily
get measurements that are upside down. Atan(x,z) with z having noise larger
than gravity is a disaster. While a simple low pass BEFORE the atan that
e.g. reduces the vibrations from 1.1 g with atan errors of 180deg to 0.6 g
with atan errors of only 40 degree.

Filtering the accelero + GPS*gyro to get rid of eigenmotions and airframe
vibrations before giving it to the kalman filter would help here if the
delay is acceptable.
On Apr 6, 2012 4:54 PM, "Felix Ruess" <
reply@reply.github.com>
wrote:

So if I understand you correctly these issues arise due the assumption we
make:
The norm of the 3D GPS velocity measurement is always in the direction
of the body x-axis

Which is not true in case of side-slip, tail waggling, pitch oscillations,
dutch roll, etc...

If your velocity vector is correct, there should be no such problems.
If you roll and your velocity is tangential to your x-axis (the case of a
clean roll) then everything is fine: rate (cross) velocity is zero and you
don't need to correct the acceleration measurement.


Reply to this email directly or view it on GitHub:
#93 (comment)

@flixr

This comment has been minimized.

Copy link
Member Author

commented Apr 7, 2012

It is not so much the speed that is wrong, it is the turn rate. The "model" says centrifugal force is speed x turn rate.

Hm.. I still don't understand why the turn rate would be the problem. Isn't it the speed vector that is wrong? I mean if you had the correct speed vector (NOT assuming speed was only in x-direction), then it should be correct.
So is not the problem that the model is [vel_norm 0 0]' x [p q r]' instead of [vx vy vz]' x [p q r]'?

With no dutch roll or pitch oscillations the gyro measures just that.
E.g. 360 deg in 30 sec = 12deg/sec which at 30m/s gives 6m/s2 centrifugal force or roughly 35 degree of roll.
But if you now add a fast period motion eigenmotion of only 150 deg/sec this gives 60 m/s2 errors. Although this is an oscillation and thus on average is correct the kalman filters tends to get in trouble when the roll measurement is -5.4 g left = -178 degree and then 6.6g right = + 179deg while the average should be around 35.

Apart from noise or vibrations the gyro should measures the turn rate just fine. 150 deg/sec should be no problem at all, no matter if they are eigenmotions and oscillating or not. The dutch roll or pitch oscillations are slow enough to measure properly with the gyros...

I get what you are saying about the resulting error you get... But as long as you don't run into the gyro limits (or have serious vibration issues), it is not really the the turn rate that is wrong, but it is actually the model using to compute the centrifugal acceleration.

Filtering the accelero + GPS*gyro to get rid of eigenmotions and airframe vibrations before giving it to the kalman filter would help here if the delay is acceptable.

I agree it could help if you low-pass gyro measurements a lot for our filters (complementary and the like).
But in case of dutch roll or pitch oscillation movements of the airplane it only masks the actual problem: velocity vector = [vel_norm 0 0] in body frame is not true anymore.

If you write a proper INS Kalman filter you don't (directly) have that problem. You keep an estimate of the velocity and you don't need to explicitly compensate for centrifugal acceleration. So there is no need for the simplified model from above.

Vibrations are a different issue...

@dewagter

This comment has been minimized.

Copy link
Member

commented Apr 10, 2012

On Sat, Apr 7, 2012 at 2:49 PM, Felix Ruess <
reply@reply.github.com

wrote:

It is not so much the speed that is wrong, it is the turn rate. The
"model" says centrifugal force is speed x turn rate.

Hm.. I still don't understand why the turn rate would be the problem.
Isn't it the speed vector that is wrong? I mean if you had the correct
speed vector (NOT assuming speed was only in x-direction), then it should
be correct.
So is not the problem that the model is [vel_norm 0 0]' x [p q r]' instead
of [vx vy vz]' x [p q r]'?

The vector not pointing the correct way is one source of error but not the
worst. You need significant effecitive sideslip before a p-rol-motion for
instance causes a g-force.
With the turn-rate measured from fast period motions, (especially lateral)
the used model estimates huge lateral accelerations. One would hope that
these transient lateral accelerations would be compensated for by the
accelerometers. In our logfiles this is not the case (even when saturation
issues are solved). After low-passing away eigenmotions the model is quite
accurate again. So going back to theory: I'm not 1000% sure but I THINK
the [vx vy vz]' x [p q r] - [ax ay az] model we use says: that the force
needed to bend a speed [vx vy vz]' over an angle [p q r] * DeltaT is a
force of [vx vy vz]' x [p q r]. And in our planes the problem is the fact
that the speed is not actually bent this angle. I mean: there is sideslip:
so that means that the speed vector did not turn as much as the heading
(during short period transients). So the model is (VERY) wrong here.

With no dutch roll or pitch oscillations the gyro measures just that.
E.g. 360 deg in 30 sec = 12deg/sec which at 30m/s gives 6m/s2
centrifugal force or roughly 35 degree of roll.
But if you now add a fast period motion eigenmotion of only 150 deg/sec
this gives 60 m/s2 errors. Although this is an oscillation and thus on
average is correct the kalman filters tends to get in trouble when the roll
measurement is -5.4 g left = -178 degree and then 6.6g right = + 179deg
while the average should be around 35.

Apart from noise or vibrations the gyro should measures the turn rate just
fine. 150 deg/sec should be no problem at all, no matter if they are
eigenmotions and oscillating or not. The dutch roll or pitch oscillations
are slow enough to measure properly with the gyros...

OK, I agree: I should have phrased it differently: it is the turn-rate part
in the used filter-model that is wrong. (the measurements are good, except
for a little more integration errors during fart period motions when
captured at too low sampling rate)

I get what you are saying about the resulting error you get... But as long
as you don't run into the gyro limits (or have serious vibration issues),
it is not really the the turn rate that is wrong, but it is actually the
model using to compute the centrifugal acceleration.

Correct!

Filtering the accelero + GPS*gyro to get rid of eigenmotions and
airframe vibrations before giving it to the kalman filter would help here
if the delay is acceptable.

I agree it could help if you low-pass gyro measurements a lot for our
filters (complementary and the like).

Not to confuse: I would NOT filter the gyro's before integration, since
that causes more integration errors. But I would filter the specific-force

  • V *omega.

But in case of dutch roll or pitch oscillation movements of the airplane
it only masks the actual problem: velocity vector = [vel_norm 0 0] in body
frame is not true anymore.

This is another problem. That problem is not handled here and will remain.
But since it's another problem I do not agree that we mask it. I need to
look into this problem further, but the first idea is that this one is not
actually such a big deal for airplanes. Example: if you fly with 45 degree
of crab angle: you would expect a speed vector of [1 1 0]. But that would
mean you now also have p x vy terms that cause vertical accelerations.
These accelerations do not occur as the air is not coming from the side.
And for estimating the centrifugal the total ground speed is also good:
with 15m/s speed and 12m/s wind for instance, the turn upwind is very flat
and downwind very strong, well captured in the equations. Only problems
arrise is vertical acceleration estimation with a lot of wind: e.g. 10deg
pitch up command when flying nose in the wind: GPS-speed is very small. a
normal Vx * q (and Vx * r) highy underestimates the actual g-force, and
overestimates it when flying with the wind. But here again: any q and r
rotation that does not lead to a heading change should be neglected, and
here the lowpass-filtering could also help to reduce the amplitude of the
feedback error.

If you write a proper INS Kalman filter you don't (directly) have that
problem. You keep an estimate of the velocity and you don't need to
explicitly compensate for centrifugal acceleration. So there is no need for
the simplified model from above.

If you write a proper Kalman filter that estimates the complete speed
vector, you must also be very careful to use a good airplane model: It is
NOT because the ground speed is at 45 of crab that rolling causes high
vertical g-forces just like pitch would do. That is because of referece
frame issues. There are 2 steps: from body to air (causing aerodynamic
forces) and from body to ground (causing inertial forces). Not to be
confused :-)

Vibrations are a different issue...

Different indeed, but related. Worst about vibrations is the effect on the
gyro bias. That is the scariest part. The rest (I mean oscillations) can be
filtered away, but filtering WILL MASK the gyro bias stability problem, so
this is the biggest hidden danger if we would make filters that can cope
with more an more vibrations.


Reply to this email directly or view it on GitHub:
#93 (comment)

@dewagter

This comment has been minimized.

Copy link
Member

commented Apr 10, 2012

If you write a proper INS Kalman filter you don't (directly) have that

problem. You keep an estimate of the velocity and you don't need to
explicitly compensate for centrifugal acceleration. So there is no need for
the simplified model from above.

If you write a proper Kalman filter that estimates the complete speed
vector, you must also be very careful to use a good airplane model: It is
NOT because the ground speed is at 45 of crab that rolling causes high
vertical g-forces just like pitch would do. That is because of referece
frame issues. There are 2 steps: from body to air (causing aerodynamic
forces) and from body to ground (causing inertial forces). Not to be
confused :-)

The [vx vy vz]' x [p q r]' as estimate for the kinematic acceleration
[dvx/dt dvy/dt dvz/dt] in [ax ay az] = [0 0 g] + [dvx/dt dvy/dt
dvz/dt] should contain:

    [vx vy vz] = velocities in some inertial frame.

    [p q r] = rate of change of that velocity.

-GPS estimates the inertial speed very well. (only terms in the order of
earth rotation&orbit and solar system accelerations are neglected)
-Body-mounted gyroscopes estimate the rate of change of the inertial speed
poorly (ranges from potentially poorly to extremely terribly poorly
depending on the vehicle). First it simply completely neglects ANY linear
acceleration. Moreover they measure attitude changes while attitude changes
are only partially aerodynamically related to speed changes (to illustrate:
take for instance an X-wing that can fly a straight line while constantly
rolling, or the 3-othogonal-prop-pair-quad at imav2011, and turbulence also
appears to affect attitude terribly, but I'm not sure where it destroys
this model the most, maybe linear accelerations, probably heading changes
without acceleration...)

In conventional fixedwings these errors are minimized when you estimate
that p is pointing in the direction of the total GPS-V. And removing large
oscillations (vibrations + short-period motions) appear to be CRUCIAL to
reduce the non-linear consequences of attitude equations ( atan( 0.5 + 2,
0.5) = 179deg followed by atan(0.5 -2, 0.5) = -178 not being atan ( 0.5,
0.5) = 45 ) especially when combined with a linear 1st order kalman filter
(big error in = big error out) like the DCM/quad/complementary code we have
now.

@flixr

This comment has been minimized.

Copy link
Member Author

commented Apr 10, 2012

So is not the problem that the model is [vel_norm 0 0]' x [p q r]'
instead
of [vx vy vz]' x [p q r]'?

The vector not pointing the correct way is one source of error but not the
worst. You need significant effecitive sideslip before a p-rol-motion for
instance causes a g-force.
With the turn-rate measured from fast period motions, (especially lateral)
the used model estimates huge lateral accelerations.

You only get unwanted huge lateral accelerations if either the linear or
the angular velocity vector is not correct.

One would hope that
these transient lateral accelerations would be compensated for by the
accelerometers. In our logfiles this is not the case (even when saturation
issues are solved). After low-passing away eigenmotions the model is quite
accurate again. So going back to theory: I'm not 1000% sure but I THINK
the [vx vy vz]' x [p q r] - [ax ay az] model we use says: that the force
needed to bend a speed [vx vy vz]' over an angle [p q r] * DeltaT is a
force of [vx vy vz]' x [p q r]. And in our planes the problem is the fact
that the speed is not actually bent this angle. I mean: there is sideslip:
so that means that the speed vector did not turn as much as the heading
(during short period transients). So the model is (VERY) wrong here.

What we want to do in the accel update (in the filers we currently use) is
essentially to compute an attitude (ok, roll & pitch only) estimate from
the acceleration measurements and use that to correct for the drift in gyro
integration. For that we need to measure acceleration due to gravity, but
accels measure specific force, so we need to subtract our trajectory
induced accelerations. We approximate that by computing the centrifugal
acceleration whilst assuming a coordinated turn and neglecting the rest..
centrifugal acceleration: omega x omega x radius = omega x vel_tangential
So if the assumption that you are in a coordinated turn is not true
anymore, you would have to somehow use the correct velocity or filter it so
much that the assumption is approximately true again (e.g. in case of dutch
roll movements).

I agree it could help if you low-pass gyro measurements a lot for our
filters (complementary and the like).

Not to confuse: I would NOT filter the gyro's before integration, since
that causes more integration errors. But I would filter the specific-force

  • V *omega.

Sure, got that and definitely agree with you.

But in case of dutch roll or pitch oscillation movements of the airplane
it only masks the actual problem: velocity vector = [vel_norm 0 0] in
body
frame is not true anymore.

This is another problem. That problem is not handled here and will remain.
But since it's another problem I do not agree that we mask it. I need to
look into this problem further, but the first idea is that this one is not
actually such a big deal for airplanes. Example: if you fly with 45 degree
of crab angle: you would expect a speed vector of [1 1 0]. But that would
mean you now also have p x vy terms that cause vertical accelerations.
These accelerations do not occur as the air is not coming from the side.
And for estimating the centrifugal the total ground speed is also good:
with 15m/s speed and 12m/s wind for instance, the turn upwind is very flat
and downwind very strong, well captured in the equations. Only problems
arrise is vertical acceleration estimation with a lot of wind: e.g. 10deg
pitch up command when flying nose in the wind: GPS-speed is very small. a
normal Vx * q (and Vx * r) highy underestimates the actual g-force, and
overestimates it when flying with the wind. But here again: any q and r
rotation that does not lead to a heading change should be neglected, and
here the lowpass-filtering could also help to reduce the amplitude of the
feedback error.

As I wrote above, I don't think this is a different problem. Imho the
problem with these maneuvers is that you assume a coordinated turn, which
is just not true if you have dutch roll, etc. movements...
But as you said (and I also wrote above) if you low pass it enough to get
rid of the oscillations, you approximately have a coordinated turn again,
and you can use that model.
And yes, the assumption is that the only other accelerations (apart of
gravity) come from the coordinated turn.
The other assumption of course is that the accel measurement you use is the
actual current acceleration of the airframe (not low passed to much).

If you write a proper INS Kalman filter you don't (directly) have that

problem. You keep an estimate of the velocity and you don't need to
explicitly compensate for centrifugal acceleration. So there is no need
for
the simplified model from above.

If you write a proper Kalman filter that estimates the complete speed
vector, you must also be very careful to use a good airplane model: It is
NOT because the ground speed is at 45 of crab that rolling causes high
vertical g-forces just like pitch would do. That is because of referece
frame issues. There are 2 steps: from body to air (causing aerodynamic
forces) and from body to ground (causing inertial forces). Not to be
confused :-)

No, you don't need a model at all! A strapdown INS is a purely kinematic
system and hence no model is needed.

Vibrations are a different issue...

Different indeed, but related. Worst about vibrations is the effect on the
gyro bias. That is the scariest part. The rest (I mean oscillations) can be
filtered away, but filtering WILL MASK the gyro bias stability problem, so
this is the biggest hidden danger if we would make filters that can cope
with more an more vibrations.

Full ack

@flixr

This comment has been minimized.

Copy link
Member Author

commented Jun 18, 2013

@dewagter are you OK with closing this issue. As documented some algorithms properly deal with this, others don't...

@dewagter

This comment has been minimized.

Copy link
Member

commented Jun 19, 2013

OK

-Christophe

On Tue, Jun 18, 2013 at 6:26 PM, Felix Ruess notifications@github.comwrote:

@dewagter https://github.com/dewagter are you OK with closing this
issue. As documented some algorithms properly deal with this, others
don't...


Reply to this email directly or view it on GitHubhttps://github.com//issues/93#issuecomment-19623065
.

@flixr

This comment has been minimized.

Copy link
Member Author

commented Jun 19, 2013

Closing this for now, as it is documented at http://paparazzi.enac.fr/wiki/Subsystem/ahrs which algos properly deal with this.

@flixr flixr closed this Jun 19, 2013
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
2 participants
You can’t perform that action at this time.