Difference between revisions of "Subsystem/ahrs"

From PaparazziUAV
Jump to navigation Jump to search
(update to reflect state in v3.9)
Line 3: Line 3:
The AHRS subsystem specifies which attitude estimation filter you are using.
The AHRS subsystem specifies which attitude estimation filter you are using.


Currently possible AHRS subsystems are
Currently possible AHRS subsystem types are
* ''[[Subsystem/ahrs#Complementary_Euler_.28fixed_point.29|int_cmpl_euler]]''
* ''[[Subsystem/ahrs#Complementary_Quaternion_.28fixed_point.29|int_cmpl_quat]]''
* ''[[Subsystem/ahrs#Complementary_Quaternion_.28fixed_point.29|int_cmpl_quat]]''
* ''[[Subsystem/ahrs#Complementary_Quaternion.2FRotation_Matrix_.28floating_point.29|float_cmpl]]''
* ''[[Subsystem/ahrs#Complementary_Quaternion.2FRotation_Matrix_.28floating_point.29|float_cmpl]]''
* ''[[Subsystem/ahrs#DCM_.28floating_point.29|float_dcm]]''
* ''[[Subsystem/ahrs#DCM_.28floating_point.29|float_dcm]]''
* ''[[Subsystem/ahrs#Complementary_Euler_.28fixed_point.29|int_cmpl_euler]]''
* ''[[Subsystem/ahrs#Infrared|infrared]]''


e.g. for the latest complementary filter:
e.g. for the latest complementary filter:
Line 24: Line 25:


'''Please also see [https://github.com/paparazzi/paparazzi/issues/93 issue 93] about proper handling of BODY_TO_IMU in all AHRS algorithms.'''
'''Please also see [https://github.com/paparazzi/paparazzi/issues/93 issue 93] about proper handling of BODY_TO_IMU in all AHRS algorithms.'''
=== Complementary Euler (fixed point) ===
* Not recommended for fixedwings, as this filter doesn't compensate for centrifugal force when flying turns.
* Magnetometer is always only used for heading (yaw).
* Does not handle the accel and mag updates correctly if BODY_TO_IMU is used for more than just adjustment by a few degrees.
* In general, rather use ''[[Subsystem/ahrs#Complementary_Quaternion_.28fixed_point.29|int_cmpl_quat]]''
Optional flags/defines are:
* FACE_REINJ_1 : defaults to 1024
* IMU_MAG_OFFSET : offset to subtract from the heading calculated by the magnetometer
* USE_NOISE_FILTER : apply a simple filter on the rate and accel inputs
* USE_NOISE_CUT : cut rate input at 1 rad/s and accel input at 20m/s²
{{Box Code|conf/airframes/myplane.xml|
<source lang="xml">
  <firmware name="fixedwing or rotorcraft">
    ...
    <subsystem name="ahrs" type="int_cmpl_euler"/>
  </firmware>
  <section name="MISC">
    <define name="FACE_REINJ_1" value="1024"/> <!-- optional, defaults to 1024 -->
  </section>
</source>
}}


=== Complementary Quaternion (fixed point) ===
=== Complementary Quaternion (fixed point) ===
* Estimates the gyro bias.
* Estimates the gyro bias.
* By default uses magnetometer for heading.
* In v3.9 and later:
* In v3.9 and later:
** Compensation of centrifugal force (via AHRS_GRAVITY_UPDATE_COORDINATED_TURN which is set by default for a fixedwing firmware).
** Compensation of centrifugal force via GPS speed (to fly in circles with a fixedwing).<br/>Enabled with AHRS_GRAVITY_UPDATE_COORDINATED_TURN which is set by default for a fixedwing firmware.
** GPS based heading estimation: https://github.com/paparazzi/paparazzi/issues/130
** GPS based heading estimation: https://github.com/paparazzi/paparazzi/issues/130


Line 59: Line 36:
*AHRS_PROPAGATE_LOW_PASS_RATES : apply a low pass filter on rotational velocity
*AHRS_PROPAGATE_LOW_PASS_RATES : apply a low pass filter on rotational velocity
*AHRS_MAG_UPDATE_ALL_AXES : use mag to also update roll/pitch and not only yaw (not recommended in most cases)
*AHRS_MAG_UPDATE_ALL_AXES : use mag to also update roll/pitch and not only yaw (not recommended in most cases)


{{Box Code|conf/airframes/myplane.xml|
{{Box Code|conf/airframes/myplane.xml|
Line 77: Line 53:


Also see the [[Subsystem/ahrs#Local_Magnetic_Field|Local Magnetic Field]] section.
Also see the [[Subsystem/ahrs#Local_Magnetic_Field|Local Magnetic Field]] section.


=== Complementary Quaternion/Rotation Matrix (floating point) ===
=== Complementary Quaternion/Rotation Matrix (floating point) ===
* Estimates the gyro bias.
* Estimates the gyro bias.
* By default uses magnetometer for heading.
* You need to define either ''AHRS_PROPAGATE_RMAT'' or ''AHRS_PROPAGATE_QUAT'' (select if the propagation is done in rotation matrix or quaternion representation).
* You need to define either ''AHRS_PROPAGATE_RMAT'' or ''AHRS_PROPAGATE_QUAT'' (select if the propagation is done in rotation matrix or quaternion representation).
* In v3.9 and later:
* In v3.9 and later:
** Compensation of centrifugal force (via AHRS_GRAVITY_UPDATE_COORDINATED_TURN which is set by default for a fixedwing firmware).
** Compensation of centrifugal force via GPS speed (to fly in circles with a fixedwing).<br/>Enabled with AHRS_GRAVITY_UPDATE_COORDINATED_TURN which is set by default for a fixedwing firmware.
** GPS based heading estimation: https://github.com/paparazzi/paparazzi/issues/130
** GPS based heading estimation: https://github.com/paparazzi/paparazzi/issues/130
Other flags of interest are:
Other flags of interest are:
Line 88: Line 66:
* AHRS_MAG_UPDATE_ALL_AXES : use mag to also update roll/pitch and not only yaw (not recommended in most cases)
* AHRS_MAG_UPDATE_ALL_AXES : use mag to also update roll/pitch and not only yaw (not recommended in most cases)
* AHRS_GRAVITY_UPDATE_NORM_HEURISTIC: lower the gain of the gravity update based on a acceleration norm heuristic (e.g. good for bungee takeoff)
* AHRS_GRAVITY_UPDATE_NORM_HEURISTIC: lower the gain of the gravity update based on a acceleration norm heuristic (e.g. good for bungee takeoff)


{{Box Code|conf/airframes/myplane.xml|
{{Box Code|conf/airframes/myplane.xml|
Line 108: Line 85:


Also see the [[Subsystem/ahrs#Local_Magnetic_Field|Local Magnetic Field]] section.
Also see the [[Subsystem/ahrs#Local_Magnetic_Field|Local Magnetic Field]] section.


=== DCM (floating point) ===
=== DCM (floating point) ===
* No direct gyro bias estimation.
* No direct gyro bias estimation, but also compensates for attitude drift.
* Similar in principle to the ''float_cmpl_rmat'' filter (but less cleanly written).
* Uses GPS speed for heading.
 
* Compensation of centrifugal force via GPS speed (to fly in circles with a fixedwing).
The main aspects are:
* '''Careful, it doesn't handle all BODY_TO_IMU rotations (mounting positions) correctly!'''
# using the centrifugal force by a rotation with the center in  the z axis  for calculating the attitude (you can fly circles for long time without any problems). '''Careful, it doesn't handle all BODY_TO_IMU rotations (mounting positions) correctly!''' If you like to help out improving this part of the sourcecode contact the paparazzi developers via the mailinglist.
# Correcting the matrix elements of the direction cosine matrix for every calculation step (typ. every 25ms) to archive the orthogonality of the matrix (small errors of the integration and calculations are not accumulated, the algorithm gives almost exact results).


The algorithm was developed by William Premerlani and Paul Bizard. The theory can be found here: [[Media:DCMDraft2.pdf|DCMDraft2.pdf]]  The algorithm is also used in the AHS systems of the AdruIMU and SparkFun Razor 6DOF IMU.
The algorithm was developed by William Premerlani and Paul Bizard. The theory can be found here: [[Media:DCMDraft2.pdf|DCMDraft2.pdf]]  The algorithm is also used in the AHRS systems of the AdruIMU.
The name DCM for the algorithm is really a misnomer, as that just means that the orientation is represented as a '''D'''irection'''C'''osine'''M'''atrix (rotation matrix). But since people already know it under that name, we kept it.


Other flags of interest are:
Other flags of interest are:
*USE_MAGNETOMETER : use magnetometer to update yaw (untested ? The magnetometer code has to be improved, since ferromagnetic materials affect the magnetic field. This is currently not implemented.)
*USE_MAGNETOMETER : use magnetometer to update yaw (untested ? The magnetometer code has to be improved, since ferromagnetic materials affect the magnetic field. This is currently not implemented.)


{{Box Code|conf/airframes/myplane.xml|
{{Box Code|conf/airframes/myplane.xml|
Line 131: Line 107:
</source>
</source>
}}
}}
=== Complementary Euler (fixed point) ===
* Not recommended for fixedwings, as this filter doesn't compensate for centrifugal force when flying turns.
* Magnetometer is always only used for heading (yaw).
* Does not handle the accel and mag updates correctly if BODY_TO_IMU is used for more than just adjustment by a few degrees.
* In general, rather use ''[[Subsystem/ahrs#Complementary_Quaternion_.28fixed_point.29|int_cmpl_quat]]''
Optional flags/defines are:
* FACE_REINJ_1 : defaults to 1024
* IMU_MAG_OFFSET : offset to subtract from the heading calculated by the magnetometer
* USE_NOISE_FILTER : apply a simple filter on the rate and accel inputs
* USE_NOISE_CUT : cut rate input at 1 rad/s and accel input at 20m/s²
{{Box Code|conf/airframes/myplane.xml|
<source lang="xml">
  <firmware name="fixedwing or rotorcraft">
    ...
    <subsystem name="ahrs" type="int_cmpl_euler"/>
  </firmware>
  <section name="MISC">
    <define name="FACE_REINJ_1" value="1024"/> <!-- optional, defaults to 1024 -->
  </section>
</source>
}}


=== Infrared ===
=== Infrared ===
For use with infrared sensors and the [[Module/infrared| infrared module]].


== Local Magnetic Field ==
== Local Magnetic Field ==

Revision as of 05:14, 31 May 2012

AHRS subsystem

The AHRS subsystem specifies which attitude estimation filter you are using.

Currently possible AHRS subsystem types are

e.g. for the latest complementary filter:

File: conf/airframes/myplane.xml
  <firmware name="fixedwing or rotorcraft">
     ...
    <subsystem name="ahrs" type="int_cmpl_quat"/>
  </firmware>

Implementations

There is a test program ( sw/airborne/test/ahrs/compare_ahrs.py ) to compare different AHRS implementations on simple test cases.

Please also see issue 93 about proper handling of BODY_TO_IMU in all AHRS algorithms.

Complementary Quaternion (fixed point)

  • Estimates the gyro bias.
  • By default uses magnetometer for heading.
  • In v3.9 and later:
    • Compensation of centrifugal force via GPS speed (to fly in circles with a fixedwing).
      Enabled with AHRS_GRAVITY_UPDATE_COORDINATED_TURN which is set by default for a fixedwing firmware.
    • GPS based heading estimation: https://github.com/paparazzi/paparazzi/issues/130

Other flags of interest are:

  • AHRS_PROPAGATE_LOW_PASS_RATES : apply a low pass filter on rotational velocity
  • AHRS_MAG_UPDATE_ALL_AXES : use mag to also update roll/pitch and not only yaw (not recommended in most cases)


File: conf/airframes/myplane.xml
  <firmware name="fixedwing or rotorcraft">
     ...
    <subsystem name="ahrs" type="int_cmpl_quat"/>
  </firmware>

  <section name="AHRS" prefix="AHRS_">
    <define name="H_X" value=" 0.51562740288882"/>
    <define name="H_Y" value="-0.05707735220832"/>
    <define name="H_Z" value=" 0.85490967783446"/>
  </section>

Also see the Local Magnetic Field section.


Complementary Quaternion/Rotation Matrix (floating point)

  • Estimates the gyro bias.
  • By default uses magnetometer for heading.
  • You need to define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT (select if the propagation is done in rotation matrix or quaternion representation).
  • In v3.9 and later:
    • Compensation of centrifugal force via GPS speed (to fly in circles with a fixedwing).
      Enabled with AHRS_GRAVITY_UPDATE_COORDINATED_TURN which is set by default for a fixedwing firmware.
    • GPS based heading estimation: https://github.com/paparazzi/paparazzi/issues/130

Other flags of interest are:

  • AHRS_PROPAGATE_LOW_PASS_RATES : apply a low pass filter on rotational velocity
  • AHRS_MAG_UPDATE_ALL_AXES : use mag to also update roll/pitch and not only yaw (not recommended in most cases)
  • AHRS_GRAVITY_UPDATE_NORM_HEURISTIC: lower the gain of the gravity update based on a acceleration norm heuristic (e.g. good for bungee takeoff)


File: conf/airframes/myplane.xml
  <firmware name="rotorcraft or fixedwing">
     ...
    <subsystem name="ahrs" type="float_cmpl_rmat">
      <define name="AHRS_PROPAGATE_QUAT"/>
    </subsystem>
  </firmware>

  <section name="AHRS" prefix="AHRS_">
    <define name="H_X" value=" 0.51562740288882"/>
    <define name="H_Y" value="-0.05707735220832"/>
    <define name="H_Z" value=" 0.85490967783446"/>
  </section>

Also see the Local Magnetic Field section.


DCM (floating point)

  • No direct gyro bias estimation, but also compensates for attitude drift.
  • Uses GPS speed for heading.
  • Compensation of centrifugal force via GPS speed (to fly in circles with a fixedwing).
  • Careful, it doesn't handle all BODY_TO_IMU rotations (mounting positions) correctly!

The algorithm was developed by William Premerlani and Paul Bizard. The theory can be found here: DCMDraft2.pdf The algorithm is also used in the AHRS systems of the AdruIMU. The name DCM for the algorithm is really a misnomer, as that just means that the orientation is represented as a DirectionCosineMatrix (rotation matrix). But since people already know it under that name, we kept it.

Other flags of interest are:

  • USE_MAGNETOMETER : use magnetometer to update yaw (untested ? The magnetometer code has to be improved, since ferromagnetic materials affect the magnetic field. This is currently not implemented.)


File: conf/airframes/myplane.xml
  <firmware name="rotorcraft or fixedwing">
     ...
    <subsystem name="ahrs" type="float_dcm"/>
  </firmware>


Complementary Euler (fixed point)

  • Not recommended for fixedwings, as this filter doesn't compensate for centrifugal force when flying turns.
  • Magnetometer is always only used for heading (yaw).
  • Does not handle the accel and mag updates correctly if BODY_TO_IMU is used for more than just adjustment by a few degrees.
  • In general, rather use int_cmpl_quat

Optional flags/defines are:

  • FACE_REINJ_1 : defaults to 1024
  • IMU_MAG_OFFSET : offset to subtract from the heading calculated by the magnetometer
  • USE_NOISE_FILTER : apply a simple filter on the rate and accel inputs
  • USE_NOISE_CUT : cut rate input at 1 rad/s and accel input at 20m/s²


File: conf/airframes/myplane.xml
  <firmware name="fixedwing or rotorcraft">
     ...
    <subsystem name="ahrs" type="int_cmpl_euler"/>
  </firmware>

  <section name="MISC">
    <define name="FACE_REINJ_1" value="1024"/> <!-- optional, defaults to 1024 -->
  </section>


Infrared

For use with infrared sensors and the infrared module.

Local Magnetic Field

To calculate the normalised local magnetic field insert the relevant location coordinates hereand calculate.


Screenshot of noaa page. Noaa mag data.png


Copy the north(x),east(y),vertical(z) component values into scilab and execute "X/norm(X)".


Screenshot of scilab page. Normalised mag fields.png