Skip to content

Commit

Permalink
Re-structure filters' docs
Browse files Browse the repository at this point in the history 
- Move filters' rst files to folder 'filters'
- Update README with OLEQ and ROLEQ status.
- Fix duplicated object descriptions in filters.
- Set references as citations, instead of footnotes.
- Set 'needs_sphinx' value to 3.
  • Loading branch information
@Mayitzin
Mayitzin committed Aug 13, 2020
1 parent b00e000 commit 90d8365
Show file tree
Hide file tree
Showing 31 changed files with 225 additions and 286 deletions.
6 changes: 3 additions & 3 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,10 +145,10 @@ Implemented attitude estimators are checked:
- [x] Mahony (Nonlinear Explicit Complementary Filter)
- [x] Naïve Angular velocity integration
- [ ] OLAE (Optimal Linear Attitude Estimator)
- [ ] OLEQ (Optimal Linear Estimator of Quaternion)
- [x] OLEQ (Optimal Linear Estimator of Quaternion)
- [x] QUEST (QUaternion ESTimator)
- [ ] REQUEST (Recursive QUEST)
- [ ] ROLEQ (Recursive Optimal Linear Estimator of Quaternion)
- [x] ROLEQ (Recursive Optimal Linear Estimator of Quaternion)
- [x] SAAM (Super-fast Attitude of Accelerometer and Magnetometer)
- [ ] Sabatini (EKF-Based Quaternion Estimator)
- [ ] SOLEQ (Sub-Optimal Linear Estimator of Quaternion)
Expand All @@ -174,7 +174,7 @@ Implemented attitude estimators are checked:
| Tilt | NO | YES | YES |
| TRIAD | NO | YES | YES |

## Notes for future versions
## Note for future versions

`ahrs` is still moving away from plotting and data parsing submodules to better focus in the algorithmic parts. Submodules `io` and `plot` will not be further developed and, eventually, will be removed.

Expand Down
57 changes: 23 additions & 34 deletions ahrs/filters/complementary.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,14 @@
==============================================
Attitude quaternion obtained with gyroscope and acceleration measurements, via
complementary filter.
complementary filter as described by [Wu]_.
References
----------
.. [1] Jin Wu. Generalized Linear Quaternion Complementary Filter for Attitude
Estimation from Multi-Sensor Observations: An Optimization Approach.
IEEE Transactions on Automation Science and Engineering. 2019.
https://ram-lab.com/papers/2018/tase_2018.pdf
.. [Wu] Jin Wu. Generalized Linear Quaternion Complementary Filter for Attitude
Estimation from Multi-Sensor Observations: An Optimization Approach. IEEE
Transactions on Automation Science and Engineering. 2019.
(https://ram-lab.com/papers/2018/tase_2018.pdf)
"""

Expand All @@ -22,31 +22,6 @@ class ComplementaryQ:
"""
Class of complementary filter for quaternion estimation.
Attributes
----------
gyr : numpy.ndarray
N-by-3 array with N gyroscope samples.
acc : numpy.ndarray
N-by-3 array with N accelerometer samples.
mag : numpy.ndarray
N-by-3 array with N magnetometer samples.
frequency : float
Sampling frequency in Herz
Dt : float
Sampling step in seconds. Inverse of sampling frequency.
gain : float
Filter gain.
q0 : numpy.ndarray
Initial orientation, as a versor (normalized quaternion).
Methods
-------
updateIMU(q, gyr, acc)
Update orientation `q` using a gyroscope and an accelerometer sample.
updateMARG(q, gyr, acc, mag)
Update orientation `q` using a gyroscope, an accelerometer, and a
magnetometer gyroscope sample.
Parameters
----------
gyr : numpy.ndarray, default: None
Expand All @@ -55,19 +30,33 @@ class ComplementaryQ:
N-by-3 array with measurements of acceleration in in m/s^2
mag : numpy.ndarray, default: None
N-by-3 array with measurements of magnetic field in mT
Extra Parameters
----------------
frequency : float, default: 100.0
Sampling frequency in Herz.
Dt : float, default: 0.01
Sampling step in seconds. Inverse of sampling frequency. Not required
if `frequency` value is given.
if ``frequency`` value is given.
gain : float, default: 0.9
Filter gain.
q0 : numpy.ndarray, default: None
Initial orientation, as a versor (normalized quaternion).
Attributes
----------
gyr : numpy.ndarray
N-by-3 array with N gyroscope samples.
acc : numpy.ndarray
N-by-3 array with N accelerometer samples.
mag : numpy.ndarray
N-by-3 array with N magnetometer samples.
frequency : float
Sampling frequency in Herz
Dt : float
Sampling step in seconds. Inverse of sampling frequency.
gain : float
Filter gain.
q0 : numpy.ndarray
Initial orientation, as a versor (normalized quaternion).
Raises
------
ValueError
Expand Down
60 changes: 28 additions & 32 deletions ahrs/filters/davenport.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,13 @@
Davenport's q-Method
====================
Attitude estimation as proposed by Paul B Davenport [1]_.
Attitude estimation as proposed by Paul Davenport [Davenport]_.
References
----------
.. [1] Paul B. Davenport. A Vector Approach to the Algebra of Rotations with
Applications. NASA Technical Note D-4696. August 1968.
(https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19680021122.pdf)
.. [Davenport] Paul B. Davenport. A Vector Approach to the Algebra of Rotations
with Applications. NASA Technical Note D-4696. August 1968.
(https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19680021122.pdf)
"""

Expand All @@ -23,61 +23,56 @@
GRAVITY = WGS().normal_gravity(MUNICH_LATITUDE, MUNICH_HEIGHT)

class Davenport:
"""Davenport's q-Method for attitude estimation
Attributes
----------
acc : numpy.ndarray
N-by-3 array with N accelerometer samples.
mag : numpy.ndarray
N-by-3 array with N magnetometer samples.
w : numpy.ndarray
Weights for each observation.
Methods
-------
estimate(acc, mag)
Estimate orientation `q` using an accelerometer, and a magnetometer
sample.
"""
Davenport's q-Method for attitude estimation
Parameters
----------
acc : numpy.ndarray, default: None
N-by-3 array with measurements of acceleration in in m/s^2
mag : numpy.ndarray, default: None
N-by-3 array with measurements of magnetic field in mT
Extra Parameters
----------------
weights : array-like
Array with two weights used in each observation.
magnetic_dip : float
Magnetic Inclination angle, in degrees.
Magnetic Inclination angle, in degrees. Defaults to magnetic dip of
Munich, Germany.
gravity : float
Normal gravity, in m/s^2.
Normal gravity, in m/s^2. Defaults to normal gravity of Munich,
Germany.
Attributes
----------
acc : numpy.ndarray
N-by-3 array with N accelerometer samples.
mag : numpy.ndarray
N-by-3 array with N magnetometer samples.
w : numpy.ndarray
Weights of each observation.
Raises
------
ValueError
When dimension of input arrays `acc` and `mag` are not equal.
When dimension of input arrays ``acc`` and ``mag`` are not equal.
"""
def __init__(self, acc: np.ndarray = None, mag: np.ndarray = None, **kw):
self.acc = acc
self.mag = mag
self.w = kw.get('weights', np.ones(2))
# Reference measurements
mdip = kw.get('magnetic_dip') # Magnetic dip, in degrees
mdip = kw.get('magnetic_dip') # Magnetic dip, in degrees
self.m_q = np.array([MAG['X'], MAG['Y'], MAG['Z']]) if mdip is None else np.array([cosd(mdip), 0., sind(mdip)])
g = kw.get('gravity', GRAVITY) # Earth's normal gravity, in m/s^2
self.g_q = np.array([0.0, 0.0, g]) # Normal Gravity vector
g = kw.get('gravity', GRAVITY) # Earth's normal gravity, in m/s^2
self.g_q = np.array([0.0, 0.0, g]) # Normal Gravity vector
if self.acc is not None and self.mag is not None:
self.Q = self._compute_all()

def _compute_all(self) -> np.ndarray:
"""Estimate the quaternions given all data.
"""
Estimate all quaternions given all data.
Attributes `acc` and `mag` must contain data.
Attributes ``acc`` and ``mag`` must contain data.
Returns
-------
Expand All @@ -95,7 +90,8 @@ def _compute_all(self) -> np.ndarray:
return Q

def estimate(self, acc: np.ndarray = None, mag: np.ndarray = None) -> np.ndarray:
"""Attitude Estimation.
"""
Attitude Estimation
Parameters
----------
Expand Down
11 changes: 6 additions & 5 deletions ahrs/filters/ekf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,14 @@
References
----------
.. [Marins] João Luís Marins, Xiaoping Yun, Eric R. Bachmann, Robert B. McGhee,
and Michael J.Zyda. An Extended Kalman Filter for Quaternion-Based Orientation
Estimation Using MARG Sensors. Proceedings of the 2001 IEEE/RSJ International
Conference on Intelligent Robots and Systems, Maui, Hawaii, USA, Oct. 29 -
Nov. 03, 2001, pp. 2003-2011.
and Michael J.Zyda. An Extended Kalman Filter for Quaternion-Based
Orientation Estimation Using MARG Sensors. Proceedings of the 2001 IEEE/RSJ
International Conference on Intelligent Robots and Systems, Maui, Hawaii,
USA, Oct. 29 - Nov. 03, 2001, pp. 2003-2011.
(https://calhoun.nps.edu/handle/10945/41567)
.. [Sarkka] Simo Särkkä (2013). Bayesian Filtering and Smoothing. Cambridge
University Press. (https://users.aalto.fi/~ssarkka/pub/cup_book_online_20131111.pdf)
University Press.
(https://users.aalto.fi/~ssarkka/pub/cup_book_online_20131111.pdf)
.. [WikiEKF] Wikipedia: Extended Kalman Filter.
(https://en.wikipedia.org/wiki/Extended_Kalman_filter)
.. [Michel] Thibaud Michel (2016). On Attitude Estimation with Smartphones.
Expand Down
24 changes: 9 additions & 15 deletions ahrs/filters/famc.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
References
----------
.. [1] Zhuohua Liu, Wei Liu, Xiangyang Gong, and Jin Wu, "Simplified Attitude
.. [Liu] Zhuohua Liu, Wei Liu, Xiangyang Gong, and Jin Wu, "Simplified Attitude
Determination Algorithm Using Accelerometer and Magnetometer with Extremely
Low Execution Time," Journal of Sensors, vol. 2018, Article ID 8787236,
11 pages, 2018. https://doi.org/10.1155/2018/8787236.
Expand All @@ -17,6 +17,13 @@
class FAMC:
"""Fast Accelerometer-Magnetometer Combination
Parameters
----------
acc : numpy.ndarray, default: None
N-by-3 array with measurements of acceleration in in m/s^2
mag : numpy.ndarray, default: None
N-by-3 array with measurements of magnetic field in mT
Attributes
----------
acc : numpy.ndarray
Expand All @@ -27,23 +34,10 @@ class FAMC:
M-by-4 Array with all estimated quaternions, where M is the number of
samples. Equal to None when no estimation is performed.
Methods
-------
estimate(acc, mag)
Estimate orientation `q` using an accelerometer, and a magnetometer
sample.
Parameters
----------
acc : numpy.ndarray, default: None
N-by-3 array with measurements of acceleration in in m/s^2
mag : numpy.ndarray, default: None
N-by-3 array with measurements of magnetic field in mT
Raises
------
ValueError
When dimension of input arrays `acc` and `mag` are not equal.
When dimension of input arrays ``acc`` and ``mag`` are not equal.
Examples
--------
Expand Down
12 changes: 6 additions & 6 deletions ahrs/filters/flae.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,18 +4,18 @@
==============================
The Fast Linear Attitude Estimator (FLAE) obtains the attitude quaternion with
an eigenvalue-based solution.
an eigenvalue-based solution as proposed by [Wu]_.
A symbolic solutions to the corresponding characteristic polynomial is also
derived for a higher computation speed.
References
----------
.. [1] Jin Wu, Zebo Zhou, Bin Gao, Rui Li, Yuhua Cheng, et al. Fast Linear
Quaternion Attitude Estimator Using Vector Observations. IEEE
Transactions on Automation Science and Engineering, Institute of
Electrical and Electronics Engineers, 2018.
https://hal.inria.fr/hal-01513263 and https://github.com/zarathustr/FLAE
.. [Wu] Jin Wu, Zebo Zhou, Bin Gao, Rui Li, Yuhua Cheng, et al. Fast Linear
Quaternion Attitude Estimator Using Vector Observations. IEEE Transactions
on Automation Science and Engineering, Institute of Electrical and
Electronics Engineers, 2018.
(https://hal.inria.fr/hal-01513263)
"""

Expand Down

0 comments on commit 90d8365

Please sign in to comment.