KalmanAbsoluteAttitudeProcessor

class KalmanAbsoluteAttitudeProcessor(val computeEulerAngles: Boolean = true, val computeCovariances: Boolean = true, val computeQuaternionCovariance: Boolean = true, val computeEulerAnglesCovariance: Boolean = true, val computeAccelerometerCovariance: Boolean = true, val computeRotationAxisCovariance: Boolean = true, val gyroscopeNoisePsd: Double = DEFAULT_GYROSCOPE_NOISE_PSD, val accelerometerNoiseStandardDeviation: Double = DEFAULT_ACCELEROMETER_NOISE_STANDARD_DEVIATION, val magnetometerNoiseStandardDeviation: Double = DEFAULT_MAGNETOMETER_NOISE_STANDARD_DEVIATION, val quaternionStepIntegratorType: QuaternionStepIntegratorType = QuaternionStepIntegrator.DEFAULT_TYPE, val gravityNorm: Double = EARTH_GRAVITY, val freeFallThreshold: Double = DEFAULT_FREE_FALL_THRESHOLD, val symmetricThreshold: Double = SYMMETRIC_THRESHOLD, var processorListener: KalmanAbsoluteAttitudeProcessor.OnProcessedListener? = null, var numericalInstabilitiesListener: KalmanAbsoluteAttitudeProcessor.OnNumericalInstabilitiesDetectedListener? = null)

Estimates absolute attitude using a Kalman filter. This is based on Android native implementation: https://android.googlesource.com/platform/frameworks/native/+/refs/heads/master/services/sensorservice/Fusion.cpp And also based on: https://www.mdpi.com/1424-8220/20/23/6731

This implementation uses a Kalman filter where attitude is estimated based on direction vector obtained from accelerometer and magnetometer measurements for filter corrections, and current system state based on angular speed for attitude predictions.

Constructors

Link copied to clipboard
constructor(computeEulerAngles: Boolean = true, computeCovariances: Boolean = true, computeQuaternionCovariance: Boolean = true, computeEulerAnglesCovariance: Boolean = true, computeAccelerometerCovariance: Boolean = true, computeRotationAxisCovariance: Boolean = true, gyroscopeNoisePsd: Double = DEFAULT_GYROSCOPE_NOISE_PSD, accelerometerNoiseStandardDeviation: Double = DEFAULT_ACCELEROMETER_NOISE_STANDARD_DEVIATION, magnetometerNoiseStandardDeviation: Double = DEFAULT_MAGNETOMETER_NOISE_STANDARD_DEVIATION, quaternionStepIntegratorType: QuaternionStepIntegratorType = QuaternionStepIntegrator.DEFAULT_TYPE, gravityNorm: Double = EARTH_GRAVITY, freeFallThreshold: Double = DEFAULT_FREE_FALL_THRESHOLD, symmetricThreshold: Double = SYMMETRIC_THRESHOLD, processorListener: KalmanAbsoluteAttitudeProcessor.OnProcessedListener? = null, numericalInstabilitiesListener: KalmanAbsoluteAttitudeProcessor.OnNumericalInstabilitiesDetectedListener? = null)

Types

Link copied to clipboard
object Companion
Link copied to clipboard

Interface to notify when error covariances are reset because numerical instabilities have been detected in Kalman filter.

Link copied to clipboard
fun interface OnProcessedListener

Interface to notify when a new attitude has been processed.

Properties

Link copied to clipboard
val accelerationState: AccelerationTriad

Contains estimated accelerometer state as measurements expressed in NED coordinates.

Link copied to clipboard

Error covariance of estimated accelerometer state. This is only available if computeCovariances and computeAccelerometerCovariance are true.

Link copied to clipboard

Accelerometer noise standard deviation expressed in m/s^2. If not provided DEFAULT_ACCELEROMETER_NOISE_STANDARD_DEVIATION will be used.

Link copied to clipboard

Indicates whether accelerometer covariance is estimated or not. This is only taken into account if computeCovariances is true.

Link copied to clipboard

Indicates whether attitude covariances must be processed or not.

Link copied to clipboard

Indicates whether Euler angles must be estimated from NED attitude or not.

Link copied to clipboard

Indicates whether propagated covariance for Euler angles expressed in NED coordinates is estimated or not. This is only taken into account if computeCovariances is true.

Link copied to clipboard

Indicates whether attitude quaternion covariance must be processed or not. This is only taken into account if computeCovariances is true.

Link copied to clipboard

Indicates whether rotation axis covariance is estimated or not. This is only taken into account if computeCovariances is true.

Link copied to clipboard

Euler angles associated to estimated NED attitude. Array contains roll, pitch and yaw angles expressed in radians (rad) and following order indicated here. This is only available if computeCovariances and computeEulerAngles are true.

Link copied to clipboard

Error covariance of estimated Euler angles for NED attitude. This is only available if computeCovariances and computeEulerAnglesCovariance are true.

Link copied to clipboard

Threshold to consider that device is in free fall. When device is in free fall, accelerometer measurements are considered unreliable and are ignored (only gyroscope predictions are made).

Link copied to clipboard

Gravity norm at current device location expressed in meters per squared second (m/s^2).

Link copied to clipboard

Gyroscope interval between measurements expressed in seconds (s).

Link copied to clipboard

Variance of the gyroscope output per Hz (or variance at 1Hz). This is equivalent to the gyroscope PSD (Power Spectral Density) that can be obtained during calibration or with noise estimators. If not provided DEFAULT_GYROSCOPE_NOISE_PSD will be used.

Link copied to clipboard

Magnetometer noise standard deviation expressed in rad/s. If not provided DEFAULT_MAGNETOMETER_NOISE_STANDARD_DEVIATION will be used.

Link copied to clipboard
val nedAttitude: Quaternion

Estimated attitude expressed in NED coordinates.

Link copied to clipboard

Error covariance of estimated quaternion attitude expressed in NED coordinates. This is only available if computeCovariances and computeQuaternionCovariance are true.

Link copied to clipboard

listener to notify when error covariances become numerically unstable and have been reset.

Link copied to clipboard

listener to notify new attitudes and their uncertainties.

Link copied to clipboard
val quaternionStepIntegratorType: QuaternionStepIntegratorType

type of quaternion step integrator to be used for gyroscope integration.

Link copied to clipboard

Error covariance of estimated rotation axis state. This is only available if computeCovariances and computeRotationAxisCovariance are true.

Link copied to clipboard

Contains estimated rotation axis state.

Link copied to clipboard

Functions

Link copied to clipboard

Processes provided synced accelerometer, gyroscope and magnetometer measurement to obtain a relative leveled attitude.

fun process(accelerometerMeasurement: AccelerometerSensorMeasurement, gyroscopeMeasurement: GyroscopeSensorMeasurement, magnetometerMeasurement: MagnetometerSensorMeasurement, timestamp: Long = gyroscopeMeasurement.timestamp): Boolean

Processes provided accelerometer, gyroscope and magnetometer measurements at provided timestamp to obtain a relative leveled attitude.

Link copied to clipboard
fun reset()

Resets internal parameters.