Repository files navigation

qml-imu is a sensor fusion module that operates on gyroscope, accelerometer andmagnetometer sensor data to estimate the device orientation and linearacceleration (without the gravity component) in the fixed global ground frame.The ground frame is defined to have its z axis point away from the floor andits y axis point towards magnetic north.

The following is required for qml-imu to work:

• Ubuntu14.04
• Qt5.3.2
• OpenCV3.0.0-alpha

Properties related to sensors themselves:

• gyroId :QString - Gyroscope sensor ID, set to the first found gyroscope's ID at startup and can be changed later
• accId :QString - Accelerometer sensor ID, set to the first found accelerometer's ID at startup and can be changed later
• magID :QString - Magnetometer sensor ID, set to the first found magnetometer's ID at startup and can be changed later
• accBias :QVector3D, default(0,0,0) - Accelerometer bias to be subtracted from every raw measurement

Startup related properties:

• startupTime :qreal, default1 - Length of the startup period in seconds where measurements affect the state more in order to settle quickly to the true orientation
• startupComplete :bool - Whether the startup period ended

Measurement noise covariance related properties:

• R_g_startup :qreal, default10^-1 - Diagonal entries of the measurement covariance matrix related to the gravity observation during startup
• R_y_startup :qreal, default10^-3 - Diagonal entries of the measurement covariance matrix related to the magnetometer observation during startup
• R_g_k_0 :qreal, default1.0 - Constant coefficient in gravity measurement covariance diagonal entries
• R_g_k_w :qreal, default7.5 - Angular velocity magnitude coefficient in gravity measurement covariance diagonal entries
• R_g_k_g :qreal, default10.0 - Acceleration magnitude deviation coefficient in gravity measurement covariance diagonal entries
• R_y_k_0 :qreal, default10.0 - Constant coefficient in magnetometer measurement covariance diagonal entries
• R_y_k_w :qreal, default7.5 - Angular velocity magnitude coefficient in magnetometer measurement covariance diagonal entries
• R_y_k_g :qreal, default10.0 - Acceleration magnitude deviation coefficient in magnetometer measurement covariance diagonal entries
• R_y_k_n :qreal, default20.0 - Magnetic vector magnitude deviation coefficient in magnetometer measurement covariance diagonal entries
• R_y_k_d :qreal, default15.0 - Magnetic vector dip angle deviation coefficient in magnetometer measurement covariance diagonal entries
• m_mean_alpha :qreal, default0.99 - Smoothing factor when estimating magnetic vector mean magnitude and mean dip angle, between0 and1

Linear velocity estimation related properties:

• velocityWDecay :qreal, default15.0 - Angular velocity magnitude decay coefficient in velocity estimate, larger values make decay threshold smaller and decay sharper
• velocityADecay :qreal, default8.0 - Acceleration magnitude decay coefficient in velocity estimate, larger values make decay threshold smaller and decay sharper

Sensor fusion outputs, all in the fixed ground frame:

• rotAxis :QVector3D - Latest estimated rotations's unit axis in angle-axis representation
• rotAngle :qreal - Latest estimated rotation's angle in degrees in angle-axis representation
• rotQuat :QQuaternion - Latest estimated rotation in unit quaternion representation
• linearAcceleration :QVector3D - Latest estimated linear acceleration in m/s^2

Displacement related properties and invokables:

• targetTranslation :QVector3D - Displacement calculation target's translation in local rigid body frame, with respect to the IMU
• targetRotation :QQuaternion - Displacement calculation target's rotation in local rigid body frame, with respect to the IMU
• resetDisplacement() :void - Sets the last pose as the current pose for the displacement calculation
• getLinearDisplacement() :QVector3D - Gets the translation of the target point with respect to its pose at the last call toresetDisplacement()
• getAngularDisplacement() :QQuaternion - Gets the rotation of the target point with respect to its pose at the last call toresetDisplacement()

At the core of the sensor fusion is a an extended Kalman filter estimating thedevice orientation and linear acceleration. [1] describes the fusion schemeused in this library.

Inputs to the filter are gyroscope data (mandatory), accelerometer data(if present) and magnetometer data (if present). The main premise is thatgyroscope data can provide reliable short term orientation data ifintegrated, but will drift with time. Accelerometer data provides the sum oflinear acceleration and the reaction due to gravitational acceleration.Assuming that linear acceleration is small compared to gravity, drift inorientation due to the integration of gyroscope data is corrected in thelong term since the acceleration vector will be pointing away from the flooron average. While this corrects the drift in rotations around the x and yaxes, magnetometer data is used to correct the drift in rotation around thez axis due to its pointing towards a linear combination of floor andmagnetic north, depending on the device location on the Earth.

In addition to this, since the device orientation with respect to the groundis known, the gravity component from the accelerometer data can be removed(assuming it is always pointing away from the floor and has magnitude 9.81m/s^2), leaving us with linear acceleration only.

Without magnetometer data, the rotation estimate will drift around the zaxis. Without accelerometer data, the rotation estimate will drift aroundthe y axis and linear acceleration cannot be estimated. Without both, therotation estimate will drift around all axes and linear acceleration cannotbe estimated. Without gyroscope data, the filter cannot operate.

Finally, this object also provides on-demand angular and linear displacementvalues via respective API calls, calculated from the a posteriori stateestimates. These values represent the displacement of the device at demandtimet in the device frame at initial timet0. This initial time can beindicated on demand via another API call.

The state is described as the following 7x1 vector:

X(t) = (q_w(t), q_x(t), q_y(t), q_z(t), a_x(t), a_y(t), a_z(t))^T     = (q(t), a(t))^T

Here,q(t) is the orientation of the local body frame with respect to theglobal inertial ground frame.a(t) is the linear acceleration of the localbody frame in the global inertial ground frame; this linear accelerationdoes not include and is separated from the reaction force against gravity.

The transition process from statet-1 to statet is made according to thegyroscope and accelerometer input and is nonlinear in terms of the state.The following describes the process for the orientation (and is linear):

q(t|t-1) = q(t-1|t-1) + deltaT*(dq(t-1|t-1)/dt)         = q(t-1|t-1) + deltaT*(1/2)*q(t-1|t-1)*(0, w(t-1))         = q(t-1|t-1) + deltaT*(1/2)*omega(t-1|t-1)*w(t-1)

Herew(t-1) is the measured angular velocity anddeltaT is the time passedsince the last gyroscope measurement. In the second line,q(t-1)*(0, w(t-1))represents Hamilton product of the orientation and the angular velocity,considered as a quaternion with zero scalar part.omega(t) is a 4x3 matrixthat is defined as follows and implements the Hamilton product:

           / -q_x(t)    -q_y(t)    -q_z(t) \omega(t) = |  q_w(t)    -q_z(t)     q_y(t) |           |  q_z(t)     q_w(t)    -q_x(t) |           \ -q_y(t)     q_x(t)     q_w(t) /

The following describes the process for the linear acceleration (and isnonlinear):

a(t|t-1) = rot(q(t-1|t-1)^-1)*a_m(t-1) - (0, 0, g)^T

Here,rot() represents the orthogonal rotation matrix given the inputquaternion.a_m(t-1) represents the raw accelerometer measurement andg isa constant equal to 9.81 m/s^2. This process, assuming thatq(t|t) describesthe orientation of the local body with respect to the ground inertial frameand is stable, rotates the total acceleration into the ground inertialframe and removes the gravity component in order to give the linearacceleration.

Note that the process on linear acceleration does not depend on the previouslinear acceleration state.

Given these, the process can be expressed as the following:

X(t|t-1) = f(X(t-1|t-1), u(t-1))u(t-1) = (w(t-1), a_m(t-1))^T

wheref implements the above processes. As per the extended Kalman filter,the state transition matrix is 7x7 and is described as follows:

F(t-1) = (df/dX)(X(t-1|t-1), u(t-1))

The process noise is, as usual, described by a 7x7 covariance matrix (Q)that should be tuned by the user. As a design choice, the components of thismatrix are multiplied bydeltaT at each step. This is based on the premisethat the error involved in integrating the angular velocity increases withan increased time gap between each angular velocity measurement.

The observation vector is a 6x1 vector and is described as:

z(t) = (a_m_x(t), a_m_y(t), a_m_z(t), m_x(t), m_y(t), m_z(t))^T     = (a_m(t), m(t))^T

wherea_m(t) is the measured acceleration vector with accelerometer biassubtracted. This bias is to be measured/tuned by the user and is constantthroughout runtime.m(t) is the measured magnetic vector with z componentrejected and normalized. This is achieved by assuming that the local bodyorientation estimate is accurate and stable. If this assumption is made,thenm(t) can be calculated as the following:

m(t) = m~(t)/|m~(t)|m~(t) = m_m(t) - (m_m(t).v_floor(t))*v_floor(t)v_floor(t) = rot(q(t|t-1)^-1)*(0, 0, 1)^T

whererot() represents the orthogonal rotation matrix given the inputquaternion,. represents dot product,v_floor(t) is the unit floor vectorin the local body frame andm_m(t) is the measured magnetic vector.

The predicted observation vector is also a 6x1 vector, and using the apriori state estimate, is described (non-linearly) as follows:

h(X(t|t-1)) = (rot(q(t|t-1)^-1)*(0, 0, g)^T, rot(q(t|t-1)^-1)*(0, 1, 0)^T)

where g is 9.81 m/s^2 androt() represents the orthogonal rotation matrixgiven the input quaternion. This design assumes that the measurementa_m(t),on average, points away from the floor with magnitude 9.81 m/s^2 and themeasurementm(t), on average, points towards the magnetic north withmagnitude 1, and that they are stable in the long term. With theseassumptions, the local body frame's z axis aligns with the floor vector andy axis aligns with the magnetic north, correcting the drift in orientationdue to the integration of angular velocity measurements. The x axis istherefore also implicitly well defined as the cross product of y and z axes.

As per the extended Kalman filter, the observation matrix is 6x7 and isdescribed as follows:

H(t) = (dh/dX)(X(t|t-1))

An important assumption made by the model is that magnetometer measurementsare less frequent than or at approximately equal frequency as accelerometermeasurements. Under this assumption, Kalman filter correction step is doneat every accelerometer reading. When there is no magnetometer readingpresent during a correction step, the magnetic vector measurement andpredicted measurement are set to zero, causing the magnetometer part of theobservation to have no effect on the device y axis. This update model designis purely due to computational performance reasons.

The observation noise is described by a 6x6 covariance matrix (R(t)) that isdependent on the acceleration and magnetic vector measurements. It isdefined as follows:

       /            |            \       |  I*R_g(t)  |     0      |       |       (3x3)|            |R(t) = |-------------------------|       |            |            |       |      0     |  I*R_y(t)  |       \            |       (3x3)/

whereI is a 3x3 identity matrix. The noise components in this matrix aredesigned according to the methodology described in [2].

R_g(t) is the gravity measurement noise coefficient and is, by design,defined as follows:

R_g(t) = R_g_k_0 +         R_g_k_w*|w(t)| +         R_g_k_g*|g - |a_m(t)||

whereR_g_k are coefficients to be tuned by the user,w(t) is the angularvelocity measurement,a_m(t) is the accelerometer measurement andg is 9.81m/s^2. This design aims that more noise is attributed to accelerometermeasurements when the device is not stable, i.e angular velocity is presentand it is accelerated externally. This causes the measurements toappropriately affect the state less.

R_y(t) is the magnetic vector measurement noise coefficient and is, bydesign, defined as follows:

R_y(t) = R_y_k_0 +         R_y_k_w*|w(t)| +         R_y_k_g*|g - |a_m(t)|| +         R_y_k_n*||m_m(t)| - m_norm_mean(t)| +         R_y_k_d*|m_dip_angle(t) - m_dip_angle_mean(t)|

whereR_y_k are coefficients to be tuned by the user andw(t),a_m(t) andg are the same as above.m_m(t) is the magnetic vector measurement andm_norm_mean(t) is the magnitude mean ofm_m(t) estimated by a low passfilter as follows:

m_norm_mean(t+1) = m_mean_alpha*m_norm_mean(t) + (1 - m_mean_alpha)*|m(t)|

m_dip_angle(t) is defined as the angle between the estimated z axis and themeasured magnetic vector in radians.m_dip_angle_mean(t) is the mean ofm_dip_angle(t) estimated similarly by a low pass filter as follows:

m_dip_angle_mean(t+1) = m_mean_alpha*m_dip_angle_mean(t) + (1 - m_mean_alpha)*m_dip_angle(t)

In the above equations, the smoothing factorm_mean_alpha should be tunedby the user and should be between 0 (meaning no smoothing) and 1 (meaningno update).

By design, similar to the accelerometer measurement noise, more noise isattributed to the magnetic vector measurement when the device is not stable,i.e angular velocity and linear acceleration are above base levels. Inaddition to the previous case, more noise is attributed also when themagnetic vector magnitude and the dip angle fluctuate. Since the magnitudeand the dip angle are ideally constant on any point on Earth (locally and inshort time scales), this fluctuation indicates the presence of non-whitemagnetic noise due to e.g ferromagnetic materials nearby. This is reflectedonto the model by increasing the measurement noise estimate, causing themeasurements to affect the state less.

Finally, during the initial time period (length of which is to be tuned bythe user) of the filter's runtime,R_g andR_y are not calculated as abovebut are set to significantly lower values (to be tuned by the user). Theassumption during this period is that the device is stable and accelerometerand magnetometer readings accurately describe the floor vector withmagnitude 9.81 m/s^2 and the magnetic north vector respectively. Thisensures that at startup, the orientation of the device settles quickly tocorrect values instead of settling in a "slow drift correction" fashion thatis by design the regular operation of the observation measurements.

The user can request the linear and angular displacement of a target localreference frame on the rigid device body on-demand via respective API calls.These calls return the translation and rotation of this frame in the sameframe in a previous point in time. This time is indicated by a displacementresetting API call. In other words, the following can be requested on demandby the user:

T_target(t in t0)^TR_target(t in t0)^T

whereT andR represent translation and rotation, andt0 indicates thelast displacement reset time.

Calculation of the linear displacement requires the estimation of local bodyvelocity. With inertial sensors, this is only possible with the integrationof the linear acceleration and is impossible otherwise without any referenceexternal to the device. This causes the velocity estimate to drift in a randomwalk fashion (due to accelerometer noise) and causes it to be unbounded inmagnitude.

Because of this, an additional assumption is required. In this case, theassumption is that the IMU resides on a handheld device and is stationary(has velocity zero) when it is inertially stable, i.e the angular velocityand the linear acceleration magnitudes are close to zero. Please note thatthis is not necessarily true for an IMU on an arbitrary device; a goodexample would be an aerial vehicle or a sattelite.

This assumption is modeled by two sigmoid decay factors multiplied by thevelocity estimate at each time step. This is described as follows:

v(t+1) = d_w(t)*d_a(t)*(v(t) + deltaT*a(t+1))

wherea(t+1) denotes the linear acceleration,deltaT denotes the timebetweent andt+1 andd denote the decay factors. They are designed asfollows:

d_w(t) = (1 - exp(-w_decay*|w(t)|))/(1 + exp(-w_decay*|w(t)|))d_a(t) = (1 - exp(-a_decay*|a(t)|))/(1 + exp(-a_decay*|a(t)|))

Here,w(t) is the angular velocity measurement,a(t) is the linearacceleration estimated by the filter andw_decay anda_decay arecoefficients that are to be tuned by the user. The operation of these decayfactors is such that they are essentially 1 when the magnitudes are above some"threshold" value and smoothly drop to 0 when the magnitudes drop below thisthreshold. This drop could be made sharp or extended in time by adjusting thisthreshold viaw_decay anda_decay.

[1] S. Sabatelli, M. Galgani, L. Fanucci, A. Rocchi,"A Double-Stage KalmanFilter for Orientation Tracking With an Integrated Processor in 9-D IMU",Instrumentation and Measurement, IEEE Transactions on, vol.62, no.3,pp.590-598, March 2013

[2] D. Jurman, M. Jankovec, R. Kamnik, M. Topič,"Calibration and Data FusionSolution for the Miniature Attitude and Heading Reference System", Sensors andActuators A: Physical, vol.138, no.2, pp.411-420, August 2007