Difference between revisions of "Subsystem/ahrs"

From PaparazziUAV
Jump to navigation Jump to search
m (removed software category, as modules is a subcategory of software)
m
 
(40 intermediate revisions by 13 users not shown)
Line 1: Line 1:
<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages hideprefix=always>Subsystems</categorytree>
== AHRS subsystem ==
== AHRS subsystem ==
The AHRS subsystem specifies which attitude estimation filter you are using.
The '''A'''ttitude and '''H'''eading '''R'''eference '''S'''ystem 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]]''
{| class="wikitable" style="text-align:center" border="1"
* ''[[Subsystem/ahrs#Complementary_Quaternion.2FRotation_Matrix_.28floating_point.29|float_cmpl]]''
! Type !! Point type !! Firmwares !! Notes
* ''[[Subsystem/ahrs#DCM_.28floating_point.29|float_dcm]]''
|-
|'''[[Subsystem/ahrs#Complementary_Quaternion_.28fixed_point.29|int_cmpl_quat]]''' || fixed || all ||
|-
|'''[[Subsystem/ahrs#Complementary_Quaternion.2FRotation_Matrix_.28floating_point.29|float_cmpl_quat]]''' || floating || all ||
|-
|'''[[Subsystem/ahrs#DCM_.28floating_point.29|float_dcm]]''' || floating || fixedwing ||
|-
|'''[[Subsystem/ahrs#Complementary_Euler_.28fixed_point.29|int_cmpl_euler]]'''  || fixed || rotorcraft ||
|-
|'''[[Subsystem/ahrs#Kalman_Filter_Quaternion|float_mlkf]]''' || floating || rotorcraft ||
|-
| '''[[Subsystem/ahrs#Infrared|infrared]]''' ||  || all ||
|-
| '''[[Subsystem/ahrs#Vectornav|vectornav]]''' || || all || external
|}


e.g. for the latest complementary filter:
e.g. for the latest complementary filter:
{{Box Code|conf/airframes/myplane.xml|
{{Box Code|conf/airframes/myplane.xml|
<pre>
<source lang="xml">
   <firmware name="fixedwing or rotorcraft">
   <firmware name="fixedwing or rotorcraft">
     ...
     ...
     <subsystem name="ahrs" type="int_cmpl_quat"/>
     <subsystem name="ahrs" type="int_cmpl_quat"/>
   </firmware>
   </firmware>
</pre>
</source>
}}
}}


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


'''Please also see [https://github.com/paparazzi/paparazzi/issues/93 issue 93] about proper handling of BODY_TO_IMU in all AHRS algorithms.'''
<span style="color:#FF0000">'''Caution!'''</span> '''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:
=== Complementary Quaternion (fixed point) ===
* 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|
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).
<pre>
On the other hand, accelerometers can be used to infer pitch/roll angles utilizing gravity vector measurement, but they suffer from noise due to vibrations. The measurement is then only accurate when averaged over a long term. Also, accelerometers alone are unable to give accurate angles when the vehicle is accelerating.
  <firmware name="fixedwing or rotorcraft">
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.
    ...
* No danger of [http://en.wikipedia.org/wiki/Gimbal_lock gimbal lock], since quaternions are used.
    <subsystem name="ahrs" type="int_cmpl_euler"/>
* The arithmetic is [http://en.wikipedia.org/wiki/Fixed-point_arithmetic fixed point] and is thus suitable if the processor (on your board) has no [http://en.wikipedia.org/wiki/Floating_point_unit FPU].
  </firmware>
* Estimates the gyro bias.
 
* For rotorcrafts: uses magnetometer for heading. Needs calibration!<br/>You can add <source lang="xml" enclose="none"><configure name="USE_MAGNETOMETER" value="TRUE"/></source> to be explicit about it.
  <section name="MISC">
* Also suitable for fixedwings, but you need GPS.
    <define name="FACE_REINJ_1" value="1024"/> <!-- optional, defaults to 1024 -->
** 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.
  </section>
** For fixedwing firmware, GPS course is used for heading estimation by default (and magnetometer disabled), add <source lang="xml" enclose="none"><configure name="USE_MAGNETOMETER" value="FALSE"/></source> if you want to be explicit.
</pre>
** To use magnetometer for heading: <source lang="xml" enclose="none"><configure name="USE_MAGNETOMETER" value="TRUE"/></source> and <source lang="xml" enclose="none"><configure name="AHRS_USE_GPS_HEADING" value="FALSE"/></source>
}}


=== Complementary Quaternion (fixed point) ===
* Estimates the gyro bias.
* The version on the '''master branch is currently not recommended for fixedwings''', as this filter doesn't compensate for centrifugal force when flying turns.
* In the dev branch:
** Compensation of centrifugal force (via 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:
Other flags of interest are:
*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_YAW_ONLY : use mag to update yaw only (recommended in most cases, already default in dev branch)
*AHRS_MAG_UPDATE_ALL_AXES : use mag to also update roll/pitch and not only yaw (not recommended in most cases)


* NEW since [https://github.com/paparazzi/paparazzi/commit/b40da0ae56e861e088017c01426d0c4ed7c43350 v5.1_devel-455-gb40da0a]:
** Proper scaling of corrections for 100Hz fixedwing or 500Hz for rotorcraft.
** Allow tuning of the accel and mag correction natural frequency and damping.
** Tunable gravity_heuristic_factor to reduce accelerometer influence only when the vehicle is accelerating (norm of ax,ay,az ~ 9,81 m/s2).


* Flags of interest in master branch are:
** AHRS_PROPAGATE_FREQUENCY: IMU gyrometer reading frequency ( Hz, depend on IMU subsystem used and its configuration)
** AHRS_CORRECT_FREQUENCY: IMU accelerometer reading frequency (Hz)
** AHRS_MAG_CORRECT_FREQUENCY: IMU magnetometer reading frequency (Hz)
** AHRS_ACCEL_OMEGA: Complementary filter accelerometer cut-off frequency (rd/s). Default is 0.063 rd/s, the accelerometer reading are "averaged" over 100 seconds (= 2*pi/0.063) to correct gyro bias. Lower the cut-off frequency reduce the influence of accelerometers. WARNING: if ACCEL_OMEGA is set at a lower frequency, the gyro bias variations may not be corrected fast enough. As a result, the computed attitude may show significant static (or low frequency) errors. If accel_omega is set higher, the gyro bias will be well corrected and the static accuracy of the computed angle will be very good, but the dynamic error of the computed angle may be bad.
** AHRS_ACCEL_ZETA: Complementary filter accelerometer damping. Default is 0.9
** AHRS_MAG_OMEGA:Complementary filter magnetometer cut-off frequency (rd/s). Default is 0.04 rd/s. Acts the same as accelerometer but on the yaw axis.
** AHRS_MAG_ZETA:Complementary filter magnetometer damping. Default is 0.9
** AHRS_GRAVITY_HEURISTIC_FACTOR: Default is 30. Reduce accelerometer cut-off frequency when the vehicle is accelerating: norm(ax,ay,az) ~ 9,81 m/s2. WARNING: when the IMU is not well damped, the norm of accelerometers never equals to 9,81 m/s2. As a result, the GRAVITY_HEURISTIC_FACTOR will reduce the accelerometer bandwith even if the vehicle is not accelerating. Set AHRS_GRAVITY_HEURISTIC_FACTOR = 0 in case of vibrations.
{{Box Code|conf/airframes/myplane.xml|
{{Box Code|conf/airframes/myplane.xml|
<pre>
<source lang="xml">
   <firmware name="fixedwing or rotorcraft">
   <firmware name="fixedwing or rotorcraft">
     ...
     ...
     <subsystem name="ahrs" type="int_cmpl_quat">
     <subsystem name="ahrs" type="int_cmpl_quat"/>
      <define name="AHRS_MAG_UPDATE_YAW_ONLY"/>
    </subsystem>
   </firmware>
   </firmware>


Line 75: Line 85:
     <define name="H_Z" value=" 0.85490967783446"/>
     <define name="H_Z" value=" 0.85490967783446"/>
   </section>
   </section>
</pre>
</source>
}}
}}


Also see the [[Subsystem/ahrs#Local_Magnetic_Field|Local Magnetic Field]] section.
Also see the [[Subsystem/ahrs#Local_Magnetic_Field|Local Magnetic Field]] section.
To use '''int_cmpl_quat''' for fixedwings without magnetometer (GPS based heading estimation instead):
{{Box Code|conf/airframes/myplane.xml|
<source lang="xml">
  <firmware name="fixedwing">
    ...
    <subsystem name="ahrs" type="int_cmpl_quat"/>
    <configure name="USE_MAGNETOMETER" value="FALSE"/>
  </firmware>
</source>
}}


=== Complementary Quaternion/Rotation Matrix (floating point) ===
=== Complementary Quaternion/Rotation Matrix (floating point) ===
* Estimates the gyro bias.
* Estimates the gyro bias.
* You need to define either ''AHRS_PROPAGATE_RMAT'' or ''AHRS_PROPAGATE_QUAT'' (select if the propagation is done in rotation matrix or quaternion representation).
* By default uses magnetometer for heading for rotorcrafts.
* The version on the '''master branch is currently not recommended for fixedwings''', as this filter doesn't compensate for centrifugal force when flying turns.
* Choose either ''type="float_cmpl_rmat"'' or ''type="float_cmpl_quat"'', which define ''AHRS_PROPAGATE_RMAT'' or ''AHRS_PROPAGATE_QUAT'' respectively to select if the propagation is done in rotation matrix or quaternion representation.
* In the dev branch:
* For fixedwings:
** 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:
* 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_YAW_ONLY : use mag to update yaw only  (recommended in most cases, already default in dev branch)
* 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_HEURISTIC_FACTOR: Default is 30. Reduce accelerometer cut-off frequency when the vehicle is accelerating: norm(ax,ay,az) ~ 9,81 m/s2. WARNING: when the IMU is not well damped, the norm of accelerometers never equals to 9,81 m/s2. As a result, the GRAVITY_HEURISTIC_FACTOR will reduce the accelerometer bandwith even if the vehicle is not accelerating. Set AHRS_GRAVITY_HEURISTIC_FACTOR = 0 in case of vibrations
 


{{Box Code|conf/airframes/myplane.xml|
{{Box Code|conf/airframes/myplane.xml|
<pre>
<source lang="xml">
   <firmware name="rotorcraft or fixedwing">
   <firmware name="rotorcraft or fixedwing">
     ...
     ...
     <subsystem name="ahrs" type="float_cmpl_rmat">
     <subsystem name="ahrs" type="float_cmpl_quat">
      <define name="AHRS_PROPAGATE_RMAT"/>
      <define name="AHRS_MAG_UPDATE_YAW_ONLY"/>
     </subsystem>
     </subsystem>
   </firmware>
   </firmware>
Line 108: Line 126:
     <define name="H_Z" value=" 0.85490967783446"/>
     <define name="H_Z" value=" 0.85490967783446"/>
   </section>
   </section>
</pre>
</source>
}}
}}


Also see the [[Subsystem/ahrs#Local_Magnetic_Field|Local Magnetic Field]] section.
Also see the [[Subsystem/ahrs#Local_Magnetic_Field|Local Magnetic Field]] section.
<p>
No danger of [http://en.wikipedia.org/wiki/Gimbal_lock gimbal lock], since quaternions are used.
<br/>
Suitable for fixedwings. Needs GPS!
<br/>
Suitable for rotorcraft if the magnetometer is calibrated.
<br/>
The arithmetic is [http://en.wikipedia.org/wiki/Floating_point floating point] and is thus '''not''' suitable if the processor (on your board) has '''no''' [http://en.wikipedia.org/wiki/Floating_point_unit FPU].
</p>


=== 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).
* '''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: [[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:
*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|
<source lang="xml">
  <firmware name="fixedwing">
    ...
    <subsystem name="ahrs" type="float_dcm"/>
  </firmware>
</source>
}}


The main aspects are:
# 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.
=== 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]]''


Other flags of interest are:
Optional flags/defines 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.)
* 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="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>
}}
 
<p>
Possible danger of [http://en.wikipedia.org/wiki/Gimbal_lock gimbal lock], since quaternions are not used.
<br/>
Repeat from above: '''Not''' suitable for fixedwings.
<br/>
Suitable for rotorcraft. The magnetometer is used to determine the yaw and needs to be calibrated.
Recommended replacement: [[Subsystem/ahrs#Complementary_Quaternion_.28fixed_point.29|int_cmpl_quat]]
<br/>
The arithmetic is [http://en.wikipedia.org/wiki/Fixed-point_arithmetic fixed point] and is thus suitable if the processor (on your board) has no [http://en.wikipedia.org/wiki/Floating_point_unit FPU].
</p>




=== Kalman Filter Quaternion ===
Multiplicative Linearized [http://en.wikipedia.org/wiki/Kalman_filter Kalman Filter] in quaternion formulation.
* Available in v5.0 and later
* Estimates the gyro bias.
* Uses magnetometer to update all 3 axes.
{{Box Code|conf/airframes/myplane.xml|
{{Box Code|conf/airframes/myplane.xml|
<pre>
<source lang="xml">
   <firmware name="rotorcraft or fixedwing">
   <firmware name="rotorcraft">
     ...
     ...
     <subsystem name="ahrs" type="float_dcm"/>
     <subsystem name="ahrs" type="float_mlkf"/>
   </firmware>
   </firmware>
</pre>
 
  <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>
</source>
}}
}}
Also see the [[Subsystem/ahrs#Local_Magnetic_Field|Local Magnetic Field]] section.
<p>
No danger of [http://en.wikipedia.org/wiki/Gimbal_lock gimbal lock], since quaternions are used.
<br/>
'''Not''' suitable for fixedwings!
<br/>
Suitable for rotorcraft. The magnetometer is used and needs to be well calibrated.
Estimates attitude and heading. Does not use GPS.
<br/>
The arithmetic is [http://en.wikipedia.org/wiki/Floating_point floating point] and is thus '''not''' suitable if the processor (on your board) has '''no''' [http://en.wikipedia.org/wiki/Floating_point_unit FPU].
</p>


=== Infrared ===
=== Infrared ===
For use with infrared sensors that detect aircraft attitude and the [[Module/infrared| infrared module]].
=== Vectornav ===
Driver for the Vectornav VN-200 INS, running in AHRS mode (only orientation and rate estimates). See also [[Sensors/imu#Vectornav_VN-200]].


== Local Magnetic Field ==
== Local Magnetic Field ==
To calculate the normalised local magnetic field insert the relevant location coordinates [http://www.ngdc.noaa.gov/geomagmodels/IGRFWMM.jsp here]and calculate.
[[Image:Noaa_mag_data.png|thumb|right|200px|Screenshot of noaa page]]
[[Image:Normalised_mag_fields.png|thumb|right|200px|Screenshot of scilab page]]
'''This is needed if the magnetometer should be used !'''<br/>
Take also a look at [[ImuCalibration#Calibrating_for_the_Earth_magnetic_field| magnetometer calibration]] which should be done to increase accuracy !
 
First the values of the local magnetic field vector are needed. They can be found at the states geological institute.
 
USA [http://www.ngdc.noaa.gov/geomag-web/#igrfwmm ngdc.noaa.gov]
 
Needed values are:
* north component (x)
* east component (y)
* vertical component (z)
 
The vector components are in nanotesla (nT) but AHRS needs these values as a unit vector (the length of a unit vector is 1), so they need to be normalized.
 
'''Convert them:'''
 
Copy the north(x), east(y), and vertical(z) component values into scilab and execute "X/norm(X)" or run this in ipython:
 
<source lang=python>
import numpy as np
x = np.array([20875.1, 8480.2, -51279.8])
x/np.linalg.norm(x)
</source>
 
or enter this into [http://wolframalpha.com Wolfram Alpha]:
<source lang=python>
Normalize[{20875.1, 8480.2, -51279.8}]
</source>
 
Lastly, enter the results into your airframe file as H_X, H_Y, and H_Z:
{{Box Code|conf/airframes/myplane.xml|
<source lang="xml">
  <section name="AHRS" prefix="AHRS_">
    <define name="H_X" value="0.372692"/>
    <define name="H_Y" value="0.151401"/>
    <define name="H_Z" value="-0.915521"/>
  </section>
</source>
}}
 
=== Update geomagnetic field vector based on GPS ===
You can also add the [http://docs.paparazziuav.org/latest/module__geo_mag.html geo_mag.xml module], which will update the normalized magnetic field at startup using GNSS data of where you get the position fix:
{{Box Code|conf/airframes/myplane.xml|
<source lang="xml">
  <modules>
    <load name="geo_mag.xml"/>
  </modules>
</source>
}}


This way, wherever you fly on the planet, no need to update your airframe file every time.


Screenshot of noaa page.
== Enabling External Sensors ==
[[Image:Noaa_mag_data.png|240px]]


By default the AHRS algorithms will utilize onboard sensors. Paparazzi currently has support for external Magnetometers. In order to use an external sensor you must specify the ABI Message ID of the sensor.


Copy the north(x),east(y),vertical(z) component values into scilab and execute "X/norm(X)".
{{Box Code|conf/airframes/myplane.xml|
<source lang="xml">
    <subsystem name="ahrs" type="float_cmpl_quat">
      <define name="AHRS_FC_MAG_ID" value="MAG_HMC58XX_SENDER_ID"/>
    </subsystem>
</source>
}}




Screenshot of scilab page.
{| class="wikitable" style="text-align:center" border="1"
[[Image:Normalised_mag_fields.png|240px]]
! Type !! AHRS_XXX_MAG_ID
|-
|'''[[Subsystem/ahrs#Complementary_Quaternion_.28fixed_point.29|int_cmpl_quat]]''' || AHRS_ICQ_MAG_ID
|-
|'''[[Subsystem/ahrs#Complementary_Quaternion.2FRotation_Matrix_.28floating_point.29|float_cmpl]]'''  || AHRS_FC_MAG_ID 
|-
|'''[[Subsystem/ahrs#DCM_.28floating_point.29|float_dcm]]''' || AHRS_DCM_MAG_ID
|-
|'''[[Subsystem/ahrs#Complementary_Euler_.28fixed_point.29|int_cmpl_euler]]'''  || AHRS_ICE_MAG_ID
|-
|'''[[Subsystem/ahrs#Kalman_Filter_Quaternion|float_mlkf]]''' || AHRS_MLKF_MAG_ID
|}




[[Category:User_Documentation]] [[Category:Subsystems]]
[[Category:User_Documentation]] [[Category:Subsystems]]

Latest revision as of 00:55, 14 April 2020

Subsystems
Subsystem/actuators
Subsystem/ahrs
Subsystem/gps
Subsystem/imu
Subsystem/ins
Subsystem/stabilization
Subsystem/telemetry
Subsystems

AHRS subsystem

The Attitude and Heading Reference System subsystem specifies which attitude estimation filter you are using.

Currently possible AHRS subsystem types are:

Type Point type Firmwares Notes
int_cmpl_quat fixed all
float_cmpl_quat floating all
float_dcm floating fixedwing
int_cmpl_euler fixed rotorcraft
float_mlkf floating rotorcraft
infrared all
vectornav all external

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.

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


Complementary Quaternion (fixed point)

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 infer pitch/roll angles utilizing gravity vector measurement, but they suffer from noise due to vibrations. The measurement is then only accurate when averaged over a long term. Also, accelerometers alone are unable to give accurate angles when the vehicle is accelerating. 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.

  • No danger of gimbal lock, since quaternions are used.
  • The arithmetic is fixed point and is thus suitable if the processor (on your board) has no FPU.
  • Estimates the gyro bias.
  • For rotorcrafts: uses magnetometer for heading. Needs calibration!
    You can add <configure name="USE_MAGNETOMETER" value="TRUE"/> to be explicit about it.
  • Also suitable for fixedwings, but you need GPS.
    • 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.
    • For fixedwing firmware, GPS course is used for heading estimation by default (and magnetometer disabled), add <configure name="USE_MAGNETOMETER" value="FALSE"/> if you want to be explicit.
    • To use magnetometer for heading: <configure name="USE_MAGNETOMETER" value="TRUE"/> and <configure name="AHRS_USE_GPS_HEADING" value="FALSE"/>


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)
  • NEW since v5.1_devel-455-gb40da0a:
    • Proper scaling of corrections for 100Hz fixedwing or 500Hz for rotorcraft.
    • Allow tuning of the accel and mag correction natural frequency and damping.
    • Tunable gravity_heuristic_factor to reduce accelerometer influence only when the vehicle is accelerating (norm of ax,ay,az ~ 9,81 m/s2).
  • Flags of interest in master branch are:
    • AHRS_PROPAGATE_FREQUENCY: IMU gyrometer reading frequency ( Hz, depend on IMU subsystem used and its configuration)
    • AHRS_CORRECT_FREQUENCY: IMU accelerometer reading frequency (Hz)
    • AHRS_MAG_CORRECT_FREQUENCY: IMU magnetometer reading frequency (Hz)
    • AHRS_ACCEL_OMEGA: Complementary filter accelerometer cut-off frequency (rd/s). Default is 0.063 rd/s, the accelerometer reading are "averaged" over 100 seconds (= 2*pi/0.063) to correct gyro bias. Lower the cut-off frequency reduce the influence of accelerometers. WARNING: if ACCEL_OMEGA is set at a lower frequency, the gyro bias variations may not be corrected fast enough. As a result, the computed attitude may show significant static (or low frequency) errors. If accel_omega is set higher, the gyro bias will be well corrected and the static accuracy of the computed angle will be very good, but the dynamic error of the computed angle may be bad.
    • AHRS_ACCEL_ZETA: Complementary filter accelerometer damping. Default is 0.9
    • AHRS_MAG_OMEGA:Complementary filter magnetometer cut-off frequency (rd/s). Default is 0.04 rd/s. Acts the same as accelerometer but on the yaw axis.
    • AHRS_MAG_ZETA:Complementary filter magnetometer damping. Default is 0.9
    • AHRS_GRAVITY_HEURISTIC_FACTOR: Default is 30. Reduce accelerometer cut-off frequency when the vehicle is accelerating: norm(ax,ay,az) ~ 9,81 m/s2. WARNING: when the IMU is not well damped, the norm of accelerometers never equals to 9,81 m/s2. As a result, the GRAVITY_HEURISTIC_FACTOR will reduce the accelerometer bandwith even if the vehicle is not accelerating. Set AHRS_GRAVITY_HEURISTIC_FACTOR = 0 in case of vibrations.
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.

To use int_cmpl_quat for fixedwings without magnetometer (GPS based heading estimation instead):

File: conf/airframes/myplane.xml 
  <firmware name="fixedwing">
     ...
    <subsystem name="ahrs" type="int_cmpl_quat"/>
    <configure name="USE_MAGNETOMETER" value="FALSE"/>
  </firmware>

Complementary Quaternion/Rotation Matrix (floating point)

  • Estimates the gyro bias.
  • By default uses magnetometer for heading for rotorcrafts.
  • Choose either type="float_cmpl_rmat" or type="float_cmpl_quat", which define AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT respectively to select if the propagation is done in rotation matrix or quaternion representation.
  • For fixedwings:
    • 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_HEURISTIC_FACTOR: Default is 30. Reduce accelerometer cut-off frequency when the vehicle is accelerating: norm(ax,ay,az) ~ 9,81 m/s2. WARNING: when the IMU is not well damped, the norm of accelerometers never equals to 9,81 m/s2. As a result, the GRAVITY_HEURISTIC_FACTOR will reduce the accelerometer bandwith even if the vehicle is not accelerating. Set AHRS_GRAVITY_HEURISTIC_FACTOR = 0 in case of vibrations


File: conf/airframes/myplane.xml 
  <firmware name="rotorcraft or fixedwing">
     ...
    <subsystem name="ahrs" type="float_cmpl_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.

No danger of gimbal lock, since quaternions are used.
Suitable for fixedwings. Needs GPS!
Suitable for rotorcraft if the magnetometer is calibrated.
The arithmetic is floating point and is thus not suitable if the processor (on your board) has no FPU.

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="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="rotorcraft">
     ...
    <subsystem name="ahrs" type="int_cmpl_euler"/>
  </firmware>

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

Possible danger of gimbal lock, since quaternions are not used.
Repeat from above: Not suitable for fixedwings.
Suitable for rotorcraft. The magnetometer is used to determine the yaw and needs to be calibrated. Recommended replacement: int_cmpl_quat
The arithmetic is fixed point and is thus suitable if the processor (on your board) has no FPU.


Kalman Filter Quaternion

Multiplicative Linearized Kalman Filter in quaternion formulation.

  • Available in v5.0 and later
  • Estimates the gyro bias.
  • Uses magnetometer to update all 3 axes.
File: conf/airframes/myplane.xml 
  <firmware name="rotorcraft">
     ...
    <subsystem name="ahrs" type="float_mlkf"/>
  </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.

No danger of gimbal lock, since quaternions are used.
Not suitable for fixedwings!
Suitable for rotorcraft. The magnetometer is used and needs to be well calibrated. Estimates attitude and heading. Does not use GPS.
The arithmetic is floating point and is thus not suitable if the processor (on your board) has no FPU.

Infrared

For use with infrared sensors that detect aircraft attitude and the infrared module.


Vectornav

Driver for the Vectornav VN-200 INS, running in AHRS mode (only orientation and rate estimates). See also Sensors/imu#Vectornav_VN-200.

Local Magnetic Field

Screenshot of noaa page
Screenshot of scilab page

This is needed if the magnetometer should be used !
Take also a look at magnetometer calibration which should be done to increase accuracy !

First the values of the local magnetic field vector are needed. They can be found at the states geological institute.

USA ngdc.noaa.gov

Needed values are:

  • north component (x)
  • east component (y)
  • vertical component (z)

The vector components are in nanotesla (nT) but AHRS needs these values as a unit vector (the length of a unit vector is 1), so they need to be normalized.

Convert them:

Copy the north(x), east(y), and vertical(z) component values into scilab and execute "X/norm(X)" or run this in ipython: 

import numpy as np
x = np.array([20875.1, 8480.2, -51279.8])
x/np.linalg.norm(x)

or enter this into Wolfram Alpha: 

Normalize[{20875.1, 8480.2, -51279.8}]

Lastly, enter the results into your airframe file as H_X, H_Y, and H_Z:

File: conf/airframes/myplane.xml 
  <section name="AHRS" prefix="AHRS_">
    <define name="H_X" value="0.372692"/>
    <define name="H_Y" value="0.151401"/>
    <define name="H_Z" value="-0.915521"/>
  </section>

Update geomagnetic field vector based on GPS

You can also add the geo_mag.xml module, which will update the normalized magnetic field at startup using GNSS data of where you get the position fix:

File: conf/airframes/myplane.xml 
  <modules>
    <load name="geo_mag.xml"/>
  </modules>

This way, wherever you fly on the planet, no need to update your airframe file every time.

Enabling External Sensors

By default the AHRS algorithms will utilize onboard sensors. Paparazzi currently has support for external Magnetometers. In order to use an external sensor you must specify the ABI Message ID of the sensor.


File: conf/airframes/myplane.xml 
    <subsystem name="ahrs" type="float_cmpl_quat">
      <define name="AHRS_FC_MAG_ID" value="MAG_HMC58XX_SENDER_ID"/>
    </subsystem>


Type AHRS_XXX_MAG_ID
int_cmpl_quat AHRS_ICQ_MAG_ID
float_cmpl AHRS_FC_MAG_ID
float_dcm AHRS_DCM_MAG_ID
int_cmpl_euler AHRS_ICE_MAG_ID
float_mlkf AHRS_MLKF_MAG_ID
Retrieved from "http://wiki.paparazziuav.org/w/index.php?title=Subsystem/ahrs&oldid=24452"