- Notifications
You must be signed in to change notification settings - Fork94
Attitude and Heading Reference Systems in Python
License
Mayitzin/ahrs
Folders and files
| Name | Name | Last commit message | Last commit date | |
|---|---|---|---|---|
Repository files navigation
AHRS is a collection of functions and algorithms in pure Python used to estimate the orientation of mobile systems.
Orginally, anAHRS is a set of orthogonal sensors providing attitude information about an aircraft. This field has now expanded to smaller devices, like wearables, automated transportation and all kinds of systems in motion.
This package's focus isfast prototyping,education,testing andmodularity. Performance isNOT the main goal. For optimized implementations there are endless resources in C/C++ or Fortran.
AHRS is compatible withPython 3.6 and newer.
The most recommended method is to install AHRS directly from this repository to get the latest version:
git clone https://github.com/Mayitzin/ahrs.gitcd ahrspython -m pip install.
Or usingpip for the stable releases:
pip install ahrs
AHRS depends merely onNumPy. More packages are avoided, to reduce its third-party dependency.
In order to update the version, use hatch and adjust it automatically
hatch version<major, minor, patch>
(Click on each topic to see more details.)
TheWorld Magnetic Model (WMM) is fully implemented.
It is a re-implementation of the Spherical Harmonics approximation used by the United States'National Geopatial-Intelligence Agency. It can be used to estimate all magnetic field elements on any given place of Earth for dates between 2015 and 2025.
>>>fromahrs.utilsimportWMM>>>wmm=WMM(latitude=10.0,longitude=-20.0,height=10.5)>>>wmm.magnetic_elements{'X':30499.640469609083,'Y':-5230.267158472566,'Z':-1716.633311360368,'H':30944.850352270452,'F':30992.427998627096,'I':-3.1751692563622993,'D':-9.73078560629778,'GV':-9.73078560629778}
TheEllipsoid model of theWorld Geodetic System (WGS84) is also included.
The estimation of the main and derived parameters of the WGS84 using the ellipsoid model are implemented:
>>>fromahrs.utilsimportWGS>>>wgs=WGS()# Creates an ellipsoid model, using Earth's characteristics by default>>>wgs_properties= [xforxindir(wgs)ifnot (hasattr(wgs.__getattribute__(x),'__call__')orx.startswith('__'))]>>>forpinwgs_properties:...print('{:<{w}} {}'.format(p,wgs.__getattribute__(p),w=len(max(wgs_properties,key=len))))...a6378137.0arithmetic_mean_radius6371008.771415059aspect_ratio0.9966471893352525atmosphere_gravitational_constant343591934.4authalic_sphere_radius6371007.1809182055b6356752.314245179curvature_polar_radius6399593.625758493dynamic_inertial_moment_about_X8.007921777277886e+37dynamic_inertial_moment_about_Y8.008074799852911e+37dynamic_inertial_moment_about_Z8.03430094201443e+37dynamical_form_factor0.0010826298213129219equatorial_normal_gravity9.78032533590406equivolumetric_sphere_radius6371000.790009159f0.0033528106647474805first_eccentricity_squared0.0066943799901413165geometric_dynamic_ellipticity0.003258100628533992geometric_inertial_moment8.046726628049449e+37geometric_inertial_moment_about_Z8.073029370114392e+37gm398600441800000.0gravitational_constant_without_atmosphere398600098208065.6is_geodeticTruelinear_eccentricity521854.00842338527mass5.972186390142457e+24mean_normal_gravity9.797643222256516normal_gravity_constant0.0034497865068408447normal_gravity_potential62636851.71456948polar_normal_gravity9.832184937863065second_degree_zonal_harmonic-0.00048416677498482876second_eccentricity_squared0.006739496742276434w7.292115e-05
It can be used, for example, to estimate the normal gravity acceleration (in m/s^2) at any location on Earth.
>>>wgs.normal_gravity(50.0,1000.0)# Normal gravity at latitude = 50.0 °, 1000 m above surface9.807617683884756
Setting the fundamental parameters (a,f,GM,w) yields a different ellipsoid. For the moon, for instance, we build a new model:
>>>moon_a=ahrs.MOON_EQUATOR_RADIUS>>>moon_f= (ahrs.MOON_EQUATOR_RADIUS-ahrs.MOON_POLAR_RADIUS)/ahrs.MOON_EQUATOR_RADIUS>>>moon_gm=ahrs.MOON_GM>>>moon_w=ahrs.MOON_ROTATION>>>moon=WGS(a=moon_a,f=moon_f,GM=moon_gm,w=moon_w)>>>moon.normal_gravity(10.0,h=500.0)# Gravity on moon at 10° N and 500 m above surface1.6239259827292798>>>moon.is_geodetic# Only the Earth is geodeticFalse
A full implementation of theEarth Gravitational Model (EGM2008) using Spherical Harmonics isNOT available here.
TheInternational Gravity Formula and the EU'sWELMEC normal gravity reference system are also implemented.
>>>ahrs.utils.international_gravity(50.0)# Latitude = 50° N9.810786421572386>>>ahrs.utils.welmec_gravity(50.0,500.0)# Latitude = 50° N, height above sea = 500 m9.809152687885897
New classDCM (derived fromnumpy.ndarray).
This new class represents 3x3 Direction Cosine Matrices used to describe orientations / rotations operations.
>>>fromahrsimportDCM>>>R=DCM(x=10.0,y=20.0,z=30.0)>>> type(R)<class'ahrs.common.dcm.DCM'>>>>R.view()DCM([[0.81379768-0.469846310.34202014], [0.543838140.82317294-0.16317591], [-0.204874130.318795780.92541658]])>>>R.inv# or R.Iarray([[0.813797680.54383814-0.20487413] [-0.469846310.823172940.31879578] [0.34202014-0.163175910.92541658]])>>>R.logarray([0.26026043,0.29531805,0.5473806 ])>>>R.to_axisangle()# Axis in 3D NumPy array, and angle as radians(array([0.38601658,0.43801381,0.81187135]),0.6742208510527136)>>>R.to_quaternion()array([0.94371436,0.12767944,0.14487813,0.26853582])>>>R.to_quaternion(method='itzhack',version=2)array([0.94371436,-0.12767944,-0.14487813,-0.26853582])
New classQuaternionArray (derived fromnumpy.ndarray).
This class can be used to simultaneously handle an array with several quaternions at once.
>>>Q=QuaternionArray(np.random.random((3,4))-0.5)>>>Q.view()QuaternionArray([[0.31638467,0.59313477,-0.62538687,-0.39621099], [0.24973118,-0.37958194,-0.67851278,-0.57721079], [-0.44643469,0.17200957,-0.72678553,0.49284031]])>>>Q.warray([0.31638467,0.24973118,-0.44643469])>>>Q.to_DCM()array([[[-0.09618377,-0.49116723,-0.86573866], [-0.99258756,-0.017584 ,0.1202528 ], [-0.07428738,0.8708878 ,-0.48583519]], [[-0.58710377,0.80339746,0.09930598], [0.22680733,0.04549051,0.97287669], [0.77708918,0.5937029 ,-0.20892408]], [[-0.54221755,0.19001389,0.81847104], [-0.69007015,0.45504228,-0.56279633], [-0.47937805,-0.86996048,-0.115609 ]]])>>>Q.conjugate()array([[0.31638467,-0.59313477,0.62538687,0.39621099], [0.24973118,0.37958194,0.67851278,0.57721079], [-0.44643469,-0.17200957,0.72678553,-0.49284031]])>>>Q.average()array([0.19537239,0.17826049,-0.87872408,-0.39736232])
- Type hints are added.
- NumPy is now the only third-party dependency.
- New submodule
framesto represent the position of an object in different reference frames. - Metrics for rotations in 3D spaces using quaternions and direction cosine matrices.
- New operations, properties and methods for class
Quaternion(now also derived fromnumpy.ndarray) - A whole bunch ofnew constant values (mainly for Geodesy) accessed from the top level of the package.
- Docstrings are improved with further explanations, references and equations whenever possible.
One of the biggest improvements in this version is the addition of many new attitude estimation algorithms.
All estimators are refactored to be consistent with the corresponding articles describing them. They have in-code references to the equations, so that you can follow the original articles along with the code.
These estimators are based on two main solutions:
- Wahba's Problem (WP), which finds a rotation matrix between two coordinate systems. This means we compare measurement vectors against reference vectors. Their difference is the rotation. The solution to Wahba's problem mainly compares accelerometers and magnetometers against the gravitational and geomagnetic vectors, correspondingly.
- Dead Reckoning (DR) integrating the measured local angular velocity to increasingly estimate the angular position of the sensor.
Implemented attitude estimators are:
| Algorithm | Gyroscope | Accelerometer | Magnetometer |
|---|---|---|---|
| AQUA | YES | YES | Optional |
| Complementary | YES | YES | Optional |
| Davenport's | NO | YES | YES |
| EKF | YES | YES | YES |
| FAMC | NO | YES | YES |
| FLAE | NO | YES | YES |
| Fourati | YES | YES | YES |
| FQA | NO | YES | Optional |
| Integration | YES | NO | NO |
| Madgwick | YES | YES | Optional |
| Mahony | YES | YES | Optional |
| OLEQ | NO | YES | YES |
| QUEST | NO | YES | YES |
| ROLEQ | YES | YES | YES |
| SAAM | NO | YES | YES |
| Tilt | NO | YES | Optional |
| TRIAD | NO | YES | YES |
To use the sensor data to estimate the attitude simply pass the data to a desired estimator, and it will automatically estimate the quaternions with the given parameters.
>>>attitude=ahrs.filters.Madgwick(acc=acc_data,gyr=gyro_data)>>>attitude.Q.shape(6959,4)
Some algorithms allow a finer tuning of its estimation with different parameters. Check their documentation to see what can be tuned.
>>>attitude=ahrs.filters.Madgwick(acc=acc_data,gyr=gyro_data,mag=mag_data,gain=0.1,frequency=100.0)
Speaking of documentation...
A comprehensive documentation, with examples, is now available inRead the Docs.
ahrs moves away from plotting and data handling submodules to better focus in the algorithmic parts. Submodulesio andplot are not built in the package anymore, and will be entirely removed from the base code in the next release.
This way you can also choose your favorite libraries for data loading and visualization. This also means, getting rid of its dependency onmatplotlib too.
About
Attitude and Heading Reference Systems in Python
