Adaptive Kalman filter for MEMS IMU data fusion using enhanced covariance scaling

Fuseini Mumuni·Alhassan Mumuni

Abstract MEMS (micro-electro-mechanical-system) IMU (inertial measurement unit) sensors are characteristically noisy and this presents a serious problem to their effective use.The Kalman filter assumes zero-mean Gaussian process and measurement noise variables,and then recursively computes optimal state estimates.However,establishing the exact noise statistics is a non-trivial task.Additionally,this noise often varies widely in operation.Addressing this challenge is the focus of adaptive Kalman filtering techniques.In the covariance scaling method,the process and measurement noise covariance matrices Q and R are uniformly scaled by a scalar-quantity attenuating window.This study proposes a new approach where individual elements of Q and R are scaled element-wise to ensure more granular adaptation of noise components and hence improve accuracy.In addition,the scaling is performed over a smoothly decreasing window to balance aggressiveness of response and stability in steady state.Experimental results show that the root mean square errors for both pith and roll axes are significantly reduced compared to the conventional noise adaptation method,albeit at a slightly higher computational cost.Specifically,the root mean square pitch errors are 1.1? under acceleration and 2.1? under rotation,which are significantly less than the corresponding errors of the adaptive complementary filter and conventional covariance scaling-based adaptive Kalman filter tested under the same conditions.

Keywords IMU state-space model·Role and pitch estimation·More·Attitude estimation·Adaptive Kalman filter·Covariance scaling

1 Introduction

An inertial measurement unit (IMU) measures a body’s angular rate and specific force–parameters that provide information for the determination of spatial orientation.It typically comprises tri-axial gyroscope,accelerometer and magnetometer.IMUs shave applications in a wide range of fields,including robotics,medical rehabilitation,aviation,navigation and consumer electronics.In robotics,they are used to determine pose of self-balancing robots for autonomous stabilization.Unmanned arial vehicles (UAVs) also use IMUs for attitude estimation.In navigation,IMUs play an important role in improving results,and are also useful for aiding navigation in GNSS-denied areas,as well as in simultaneous localization and mapping (SLAM) [1,2],where their data can augment visual feeds from cameras.In medical rehabilitation,they are used for pose estimation in exoskeletons for improving motor function in recovering stroke patients [3] and patients suffering from other mobility impaired conditions.

Most of these applications require the use of low-cost micro-electro-mechanical-system (MEMS)-based IMUs,which are both lighter and less power-consuming compared to high-end tactical-grade fiber optic gyro (FOG) and ring laser gyro (RLG) IMUs.However,these sensors are characterized by such inaccuracies as scale factor errors,sensor axes misalignment,bias instability,non-zero drift,cross-axis sensitivities and quantization errors.Moreover,IMU applications in these domains often involve high dynamic operations such as fast turns and external accelerations–situations that further degrade the sensors’ performance.Thus,the gyroscopes and accelerometers of these low-grade strapdown MEMS IMUs have intolerable errors and drift in inertial navigation systems (INS).While standard INS algorithms adequately address the noise and drift issues by incorporating auxiliary sensors such as magnetometers,GNSS receivers,Doppler velocity sensors and odometers,this work considers simple,low-cost applications in which the inclusion of supporting sensors would be undesirable.Thus,no heading information or information from any other sensor besides accelerometer and gyroscope is used.Therefore,techniques such as the Kalman filter (KF) are needed to deal with these noisy IMU signals.To implement the KF,one needs to determine the process noise covariance matrixQand measurement noise covariance matrixRin advance.However,these noise statistics are hard to measure accurately.For a Kalman filter whose model disturbances and measurement errors are governed by first-order Markov processes,[4] showed that guaranteed state estimation can be achieved by adjusting the given filter to a new design model that incorporates additional noise.However,we consider the IMU situation whereQandRare also time-varying and are affected by changes in environmental conditions such as temperature,magnetic fields,and device degradation due to aging.Furthermore,the noise statistics are variable under high dynamic operating conditions and are especially susceptible to external accelerations,vibrations and fast rotational motions.These changes in noise parameters degrade filtering performance and can lead to filter divergence.

To address these problems,various noise identification algorithms [5,6] have been proposed for the Kalman filter and its derivatives.Broadly,they are classified as covariance matching,correlation based,Bayesian and maximum likelihood approaches [5].Of these,the Bayesian methods are the most accurate,yet they are not as widely used as the covariance matching technique sowing to their enormous computational overhead.Noise adaptation based on optimization techniques [7,8] and other intelligent approaches [9,10] have also gained prominence in the last couple of years.Nevertheless,the covariance matching strategies continue to be popular owing to their relatively low computational requirements.Covariance matching relies on finding theQandRvalues that result in minimum deviation of the innovation or residual sequences from their theoretical values.

Earlier works such as [11–13] employed fixed window size scaling for the covariances,but by so doing a trade-off is made between robustness and aggressiveness of action.Furthermore,a suitable window needs to weigh previous measurements heavier at the beginning of transients and then systematically increase the weight on incoming measurements as filter adapts to the changing conditions and measurements become more reliable.The window size used for scaling is also often dependent on operating conditions and needs to be determined experimentally.To accommodate a wide range of operating conditions,the concurrent use of multiple filters has been proposed [14],where the right filter is selected based on the operating point.However,in fast and abruptly changing conditions,it is difficult keeping track of,and selecting appropriate filters in real time.Besides,the transition from one filter to another may result in intolerable transients.

In addressing some of these challenges associated with adaptive Kalman filtering,Huang et al.[15] and Hu et al.[16] adopted a variational Bayesian noise estimation approach and used inverse Wishart priors to determine noise covariances.However,as mentioned,the Bayesian methods have relatively high computational requirement that makes them unsuitable for computationally sensitive microcontroller-based applications.

On the other hand,the Sage–Husa adaptive Kalman filter(SHAKF) [17,18] yields adequate noise estimation results if appropriate forgetting factor is selected,but his filter is unsuitable for high dynamic operations where forgetting factor is required to change with significantly changing operating points.Filter divergence is a major concern in this situation.Xia et al.[19] proposed the adaptive estimation of forgetting factors based on operating conditions;however,as the authors themselves have warned,such an approach is computationally prohibitive and unsuitable for real-time applications.The strong tracking Kalman filter (STF),which incorporates automatically fading weights to lessen the influence of past measurements relative to current ones,overcomes this problem but the determination of optimal fading factors for a given application is a difficult task [20,21].Liu and Fan [22] introduced a computationally less demanding approach to determining attenuating factors.Nevertheless,our observation from empirical studies is that given the degree of IMU sensitivity to accelerations and rotations,the attenuation factor needs to change more smoothly over a wider range.This is not provided by the exponential weighting employed by Liu and Fan.Thus,the rate of attenuation is important,and,although the choice of window size has long been recognized as a major contributing factor to the performance of noise scaling-based adaptive Kalman filtering algorithms [23,24],few works have thus far incorporated smooth adaptive window sizing for noise scaling,while none has implemented element wise scaling for covariances.

This work improves on the covariance scaling technique by adopting a suitably smooth attenuating window that gradually attaches more importance to current measurements as the filter transitions to steady state after a disturbance.This adaptive window has been determined empirically.Additionally,we apply a unique scale factor to each element of the process and measurement noise covariances to ensure optimal scaling.The performance of the proposed optimal scaling-based adaptive Kalman filter (osAKF) is validated in attitude estimation studies of a MEMS IMU system.We present performance comparison of the proposed algorithm and an adaptive complementary filter (CF) [19],as well as a regular covariance matching adaptive Kalman filter (AKF) developed by [23]–a hugely successful method which has remained one of the most effective techniques in INS data fusion tasks.The work in [23]was based on the noise scaling approach originally introduced by [25].

The three filters contrast in the following essential ways:Both the AKF and the osAKF estimate the process and measurement noise variables,but unlike the AKF which scales the covariance matrix by a constant scale factor,the osAKF uses element-wise scaling to achieve optimal update of each element of the noise covariances.On the other hand,the CF does not estimate noise introduced by changes in system dynamics,but merely adjusts the filter gain to offset such disturbances.

2 MEMS IMU state-space model

This design uses a 9DOF MPU-9250 device [26].It comprises a tri-axis gyroscope,a tri-axis accelerometer and a triaxis magnetometer.However,the yaw orientation is not part of the current study and the magnetometer is therefore not used.Furthermore,to ensure tractability and practicability of realization on a microcontroller platform,a simple linear IMU model is used.

2.1 Gyroscope model

A gyroscope measures a body’s angular rate relative to the inertial reference frame.The gyroscope is firmly attached to the platform with itsx-axis oriented along the longitudinal axis of the platform,y-axis in the lateral direction,andz-axis complementsx,yto form the right-handed coordinate system.The measured gyroscope rateωgis given by

whereωis the true angular rate,bgis the gyroscope bias,Sgis the gyroscope’s scale factor error andηgis gyroscope white noise.The scale factor error is significant in extremely high rate dynamics and can be ignored in the present case.Thus,Eq.(1) becomes

Given an attitude angle at any timetk,its value at time (t+Δt) can be approximated as

whereεis the error term.

2.2 Accelerometer model

The acceleration output of the accelerometeraais expressed in terms of the true instantaneous linear accelerationa1,the gravitational acceleration g,accelerometer bias (ba) and noise(ηa).That is,

whereSais the accelerometer’s scale factor error.Like the gyroscope’s,the accelerometer’s reference frame is affixed to the platform withx-axis oriented along the longitudinal axis of the object,y-axis in the lateral direction,andz-axis complementsx,yto form the right-handed coordinate system,i.e.at zero pitch and roll it is directed downwards.The roll and pitch angles are computed from the accelerometer according to Eqs.(5) and (6),respectively.

Equation (6) holds true only forθroll=0.However,it is a valid approximation for small values ofθroll.

3 IMU data fusion

Gyroscopes provide relatively accurate measurements which gradually drift over time,making these devices unreliable in long term use.On the other hand,accelerometer measurements are characteristically noisy but are not so susceptible to temporal drifts.Therefore,the measurements from the two sensors are fused to compensate for the stated shortcomings associated with each device.Thus,the tilt angles computed from the accelerometer according to Eqs.(5) and (6) are combined with the discrete form of the gyroscope angleθg(t) and angular rateωg(t) given in Eq.(7) to form the final angles using the Kalman filter-based sensor fusion algorithm.

whereθis the tilt angle,and it represents roll or pitch orientation depending on the corresponding gyroscope axis used.The indiceskandk+1 represent the current and next time steps,respectively,ΔTsis the sampling time.The states of the modelled system at timek+1 are the tilt angleθk+1and the bias rate.?is the lumped gyroscope noise andμis a slowly-varying bias component.In state-space form Eqs.(7) and (8) can be combined and rewritten as

θk|k?1andθb[k|k?1]denote the angle and bias rate,respectively,at time stepk,computed using information at timek?1 .Equation (9) corresponds to the general state equationxk+1=Fkxk+Gkuk+wk.Similarly,the measurement equationzk=Hkxk+1+vkcan be written as

is the state transition matrix,andis control input matrix,ukis the input at time stepk,Hk=[1 0] is the observation matrix,zkandθkare designations for measured output at time stepk.

The Kalman filter algorithmThe Kalman filter estimates the states of a system defined by the linear difference Eqs.(9) and (10).In these equations,wkandvkare assumed to be uncorrelated,zero-mean white process noise with covariance matrixQkand measurement noise with varianceRk,respectively.Implementation of the Kalman filter involves two stages:first,in the time-update stage,the state at discrete timek?1 is predicted based on the previous state estimate at time stepkand the current inputuk.The state error covariance matrix is also estimated at this stage based on the previous error covariance.This stage is described by Eqs.(11) and (12).Thus,the priori state prediction

and the error covariance projection

are performed at this stage.Second,in the measurement update stage,the state and variance are updated according to Eqs.(14) and (15) using the predicted state,the measurement atk+1 and the Kalman gain given by (13).

4 Noise estimation

The scale factors used for updating the process and measurement noise components are referred to here as windows.These windows control the quality of adaptation of the filter.As opposed to the other AKF methods which scale all entries of noise covariance matrices with the same scale factor,our proposed optimal scaling version (the osAKF) uses a twopart window.The first part is a matrix of scale factors that is multiplied element-wise by the noise covariance matrices.This part of the window determines the relative importance of each entry in the noise covariance matrix.The second part of the window is a scalar quantity that determines the overall magnitudes of the scale factors.The rest of this section develops the concept of the adaptive osAKF window.

4.1 Adaptation of process noise

The state residual Δxk,which is the difference between the present state prediction and the projection of the state after the measurement update,is a measure of the accuracy of the state prediction and hence,of the associated noise.The state residual at timek+1 is determined as

We also consider the difference between the state estimation error covariance matrix before (Pk+1|k) and after (Pk+1|k+1)the measurement update.This parameter also reflects the quality of state prediction and can be computed as

For insight into the relevance of Eq.(17),consider first that the previous prediction error covariance was zero,then according to Eq.(13) the current gainKk+1will also be zero,and state estimate (Eq.(14)) will be unchanged after the measurement update.Therefore,the state residual in this case will be zero.Also,according to Eq.(15),whenKk+1=0,Pk+1|k+1=Pk+1|kand the change in error covariance will as well be equal to zero.Conversely,a large gain,sayKk+1=Iwill makePk+1|k+1=0 and the current observation will be weighted more heavily in the state update.This situation will result in large difference in both Δxkand ΔPk.It is clear from the preceding discussion that both Δxk+1and ΔPk+1provide an indication of how correctly the state is being predicted.The misprediction could either be due to inaccurate process noiseQkor measurement noiseRk.For now,we assumeRkto be known,so that we adaptQkto mitigate the prediction error Thus,is a representation of the effect of noise on the state residual ΔPk+1while is a representation of the effect of noise on the error covariance matrix.However,their respective weights depend on the magnitude of the noise components.Therefore,it is often necessary to attenuate one of these parts relative to the other.In the current work,our attenuation is in the ratio of 3:1.Thus,the process noise scaling matrixis given by

where 0 ≤ρ≤ 1.In the current work,ρ=0.75 was used.

We estimateQk+1by performing Hadamard product ofandQk,resulting in element-wise scaling ofQkin proportion to the corresponding element ofIn the osAKF algorithm,a second adaptive factorαk+1is determined according to [23] as

The symbol“ ? ”denotes the Hadamard product,c1is a design constant that ensures that the adaptation is carried out only if at least one of the components of the noise deviation factor is significant enough to warrant an update.The normalization ofin the equation implies that the elements of the process noise covariance matrix are only scaled in relative terms bywhereas the scalar quantityαk+1determines the overall magnitudes of the elements.

In this IMU noise estimation,the prospect of the covariance matrixlosing its positive (semi-) definiteness is not a concern if we start off with a positive definiteQk,and initialize our state estimate vectorand error covariance matrixPocorrectly (see Sect.4.3).However,in a particular situation where this is anticipated,rather than the standard state error covariance update law given by Eq.(15),the Joseph stabilized form (26) [27] presented in Eq.(23) is used:

This ensures that when starting from a symmetric positive definite state,the update is guaranteed to retain positive definiteness and symmetry.Additionally,can be checked for positive semi-definiteness,and if it fails the test a simple adaptation [28] can be used to find the closest covariance matrix tothat satisfies the constraint of positive semi-definiteness.

4.2 Adaptation of measurement noise

The measurement residual is the difference between the actual and estimated measurements and is defined as

The measurement noise covariance factorreflects the measurement projection accuracy and,in addition to the measurement residual,must take into account the influence of state estimation error on the measurement.Thus,

where the second term in Eq.(25) is the contribution of the state uncertainty to measurement misprediction.Note also that Eq.(25) is a general formulation of the algorithm for matrix quantities,whereas in the present case,our measured output is a scalar quantity.Thus,the first term at the right-hand side of the equation is simply Δz.In addition,a square root attenuating factorγis incorporated,the selection of which has been entirely empirical,

The adaptation process starts when there is a change in measurement dynamics determined by

Initially,the previous estimate of the measurement noise is weighted heavily relative to the computed adaptive part.This serves to minimize the effect of outliers on filter performance.However,as time progresses,more importance is given to new updates.Adaptation eventually stops when

4.3 Notes on application of the algorithm

i.The state vector consists of the attitude angleθand the bias rateIn the application of the algorithm,we start at a known attitude angle and initialize the state estimation vectorto this starting angle and the bias rate which is obtained through sensor calibration.Since we know the exact state vectorx0,our initialized state estimatehas zero error;hence,the error covariance matrixP0would be initialized to zero.

ii.The routines for updating process noise and measurement noise are run in alternate epochs.In the iteration that one is updated,the other is assumed to be perfectly know.

iii.Finally,it is necessary to ensure that arithmetic precision is high enough to prevent problems with numerical stability.

5 Experiments and results

The setup (shown as Fig.1) consists of an InvenSense MPU9250 IMU connected to a Texas Instruments TM4C123G ARM Cortex M4 microcontroller through I2C interface.The microcontroller computes tilt angles from the raw accelerometer and gyroscope data sampled at 200 Hz using the proposed technique.The microcontroller is connected to a computer that runs PUTTY serial software to enabled the logging of computed tilt angles which were then analyzed offline using MATLAB.The IMU is fastened to a Stabila STB196E electronic spirit level with accuracy of 0.029?.The entire hardware assembly is mounted on a stepper motor-driven turntable which itself is firmly attached to a wheeled cabin trolley translational stand.The turntable and translational stand allow the system to be subjected to rotational motion and external acceleration respectively,and were used to study the IMU performance under these dynamic conditions.

Fig.1 Experimental setup:1–power adapter,2–microcontroller,3–IMU,4–turntable,5–electronic spirit level,6–adjustable fittings,7–wheel

To compare the performance of the three algorithms under various conditions,tilt estimations are presented from experiments in three different scenarios–static,external acceleration,and continuous rotation.In addition,noise estimation performance of the AKF and osAKF are also shown (for static condition only).Our observations of tilt angle estimations show similar performance regardless of the axis being measured,so results for tilt estimation have been presented for either roll or pitch,and never the two axes at the same time.

5.1 Static conditions—no movement

In this test,the setup was kept still while the readings were being taken.While the device’s tilt angles (roll and pitch)were fixed at zero,their corresponding values computed by the CF,AKF and osAKF were logged for 100 s and are presented in Fig.2.As shown by the plots,both the AKF and osAKF were able to achieve low mean error levels of less than 0.5?.Similarly,the CF after optimal tuning attained a high accuracy level with mean error of about 0.75?.Thus,under static conditions,there is no significant difference in the performance of the three algorithms.

Fig.2 Pitch error under static conditions

5.2 External acceleration

In this case,the setup was subjected to a sudden and pronounced jerk in the forward direction between 1 and 7.2 s after system initialization.The results under this condition is shown in Fig.3.The performance of the CF significantly deteriorates in the presence of external acceleration.This is to be expected since the accelerometer in this case measures the external acceleration in addition to the gravity vector,leading to a distortion of the true measurements.Though this problem can be mitigated by de-emphasizing the gain of the accelerometer component of the CF,the filter eventually suffers from accumulated errors due to gyroscope drift with time.

Fig.3 Pitch error when sensor reference frame is accelerating

On the other hand,both the AKF and osAKF are able to identify the process noise introduced by external acceleration,thereby restoring performance after brief transients.However,the osAKF’s ability to optimally allocate weights to individual components of the noise covariance matrices leads to a faster and more accurate noise identification.As a result,its average root mean square error,even within the span of the disturbance is less than 1?.

5.3 Rotational motion

This set of experiments was conducted by means of the turntable to evaluate performances of the three algorithms with the IMU in a 32 r/min turn.The rotation was started 200 ms after system initialization and continued until 6.3 s.The results are presented in Fig.4.

The large errors of the CF shown in Fig.4 are largely due to centrifugal and centripetal forces degrading the accelerometer measurements.By identifying the noise,both the AKF and the osAKF were able to maintain higher accuracy levels.However,the osAKF is shown to have overall accuracy of more than twice the AKF.Moreover,the osAKF errors damped almost immediately after the rotation ceased,while the AKF’s errors returned to steady state level after additional 500 ms later.

Fig.4 Pitch error when sensor reference frame is rotating

5.4 Noise adaptation

This experiment compares the noise estimation performance of the AKF and the proposed osAKF.Since the true system and measurement noise variables cannot be accurately determined experimentally,there is no ground-truth reference for these parameters.Hence,the convergence speed of the estimation to steady state is what is compared here.Furthermore,this particular experiment was carried out under static conditions to demonstrate that although osAKF’s main strength is in its dynamic performance,it is still noticeably superior to AKF even under static conditions.Figure 5 shows the adaptation of the main diagonal termsQθ=Q11andQθb=Q22,with initial values of 0.0042 (?)2and 0.0020 ((?)∕s)2which gradually converged to their steady state values.As seen in Fig.5,the optimized scaling window version of each of the noise components (Qos11andQos22)converged to their steady states more than two times faster than the corresponding non-optimized window counterpart.

Meanwhile,the off-diagonal components are essentially zero.The adaptive capability ofRis shown in Fig.6.

Fig.6 Estimation of measurement noise R

5.5 Error evaluation

To quantify the performances of the CF,the AKF and the proposed osAKF under the three conditions studied,the rootmean square errors (RMSEs) of the of Euler angles been computed and plotted in Fig.7.The plots show that the superiority of the proposed algorithm is more pronounced under external acceleration,where the three algorithms recorded RMSEs as CF=3.24?,AKF=1.441?and osAKF=0.5566?,and under rotation,where CF=10.916?,AKF=2.7?and osAKF=0.988?.

Fig.7 Estimation of main diagonal components of process noise

6 Discussion of results

The results presented in Sect.5 compare the performances of the CF,AKF and the proposed osAKF,and show how the osAKF achieves better performance with respect to convergence time,noise adaptation speed,and overall accuracy(evaluated using the root mean square error or RMSE metric shown in Fig.7).With regards to convergence times shown in Figs.3 and 4,the osAKF estimations converged fastest to the ground truth angular values after acceleration and rotation disturbances.This convergence speed can be explained by the faster noise adaptation of the proposed osAKF,illustrated by Figs.5 and 6 and explained in Sect.5.4.A faster noise adaptation implies that when the system noise variables change,the filter takes less time in correctly estimating the new state,thus,ensuring that good set point tracking is restored immediately.On the other hand,the slower response of the AKF to noise dynamics results in a slightly more prolonged degradation in its set point tracking ability in the event of changes in system dynamics caused acceleration or rotation,leading to higher initial errors,which would only damp after filter noise adaptation.Consequently,these errors accumulate,eventually leading to relatively higher overall RMSEs.Note that the complementary filter is omitted from the plots of Figs.5 and 6 since it only updates its gain in response to changes in system dynamics,and does not perform any noise estimation.Meanwhile,the superior noise adaptation of the osAKF can be explained by examining how the algorithm works:When there is a sudden change in system dynamics,the system behavior begins to deviate from what is described by the process model.This deviation causes the error covariance,Eq.(17),and the state residual,Eq.(16) to increase.Using the scale factors given in Eqs.(18) and (22),the noise covarianceQkis then proportionally scaled upwards,causing the filter gain given by Eq.(13) to increase,thus driving the estimates closer to their true values.More importantly,Eq.(18) computes the process noise covariance scalaras a matrix of scale factors.Such a design choice allows each element ofQkto be updated independently by a designated scale factor,thus improving noise adaptation.A similar intuition explains how the update ofRk–using information about the innovation sequence,i.e.,the term in bracket on the right-hand side of Eq.(13),and the measurement residual in Eq.(24)–ensures good estimation performance even in the presence of changes in measurement noise dynamics.

7 Conclusions

In this study,an osAKF is developed based on the covariance matching principle.In the osAKF algorithm,a new adaptive scale factor is introduced into the process and measurement noise adaptation routines that adapts individual components of the covariances and has been shown to improve both the rate of convergence to steady state and the accuracy of noise estimation upon disturbance.The experimental results show that the strength of the proposed technique over the CF and AKF are most notable under rotational motion and under external acceleration,where root mean square pitch errors are shown to significantly lower.