Intelligent Real-Time MEMS Sensor Fusion and Calibration," in IEEE Sensors Journal, Vol

This is the accepted version of the manuscript: D. Nemec, A. Janota, M. Hruboš and V. Šimák, "Intelligent Real-Time MEMS Sensor Fusion and Calibration," in IEEE Sensors Journal, vol. 16, no. 19, pp. 7150-7160, Oct.1, 2016, doi: 10.1109/JSEN.2016.2597292, URL: http://ieeexplore.ieee.org/document/7529156

Intelligent real-time MEMS sensor fusion and calibration

Dušan Nemec, Aleš Janota, Marián Hruboš, and Vojtech Šimák

 example of velocity measurement when the sensor is Abstract—This paper discusses an innovative adaptive measuring time derivative of desired variable. Main heterogeneous fusion algorithm based on estimation of the mean disadvantage of this method is great sensitivity of the output square error of all variables used in real time processing. The quality to the precision of the sensor measurements (especially algorithm is designed for a fusion between derivative and sensor bias will cause increasing drift of the integrated result). absolute sensors and is explained by the fusion of the 3-axial gyroscope, 3-axial accelerometer and 3-axial magnetometer into First step of elimination of the integrated error is calibration of attitude and heading estimation. Our algorithm has similar error the sensor. A standard way of calibration measures raw output performance in the steady state but much faster dynamic of the sensor as a response to stimulus with known amplitude. response compared to the fixed-gain fusion algorithm. In Relation between raw and real sensor outputs is formed into comparison with the extended Kalman filter the proposed the transfer function (calibration curve) and its parameters are algorithm converges faster and takes less computational time. On obtained from the measurements during calibration in offline the other hand, Kalman filter has smaller mean square output error in a steady state but becomes unstable if the estimated state mode. It is possible to calibrate by: changes too rapidly. Additionally, the noisy fusion deviation can --One point (zero order transfer function - bias only). be used in the process of calibration. The paper proposes and --Two points (first order transfer function - bias and explains a real-time calibration method based on machine gain), learning working in the online mode during run-time. This allows --Multiple points (calibration curve is a polyline or higher compensation of sensor thermal drift right in the sensor’s order curve). working environment without need of re-calibration in the laboratory. In order to eliminate influence of the sensor random noise each calibration point has to be computed as an average of Index Terms—calibration, inertial navigation, mean square multiple measurements in the same conditions [1]. This error methods, sensor fusion. requires special laboratory equipment which provides accurate and steady simulation of different sensor stimuli. Zhang et al. proposed a method of estimation of the calibration constants I. INTRODUCTION for the 3-axial inertial sensor (gyroscope, accelerometer) [2]. NERTIAL SENSORS manufactured by the MEMS (Micro Gyroscope bias is determined directly in a steady state and I Electro-Mechanical Systems) technology are the core of accelerometer bias is computed after multiple steps when the modern low-cost AHRSs (Attitude and Heading Reference acceleration sensor is oriented vertically along each of its axes Systems). The purpose of these systems is to determine one by one or the sensor has to be exposed to precisely known rotation of the measured object with respect to the horizontal stimuli [3][4]. All these methods are working in the offline plane and northern direction which is a crucial task in mobile mode. Wang and Hao proposed a method utilizing an artificial robotics, aviation, automated car navigation and many others. neural network combined with the Kalman filter for estimation These sensor systems use nonlinear discrete numerical of nonlinear calibration parameters [5]. For online calibration integration of the measured angular velocity which is a typical it is necessary to detect steady state of the object, e.g. by lower vibrations [6]. © 2016 IEEE. Personal use of this material is permitted. Permission from When MEMS sensors are used, their calibration parameters IEEE must be obtained for all other uses, in any current or future media, tend to drift with temperature [5] [7]. Transfer function is including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers therefore two-dimensional – one input corresponds to raw or lists, or reuse of any copyrighted component of this work in other works. sensor data and the second input is sensor temperature. Most of commercially available integrated MEMS sensors The paper was submitted for review in February 22, 2016. This work was supported by the European Regional Development Fund and the Ministry of incorporate a temperature sensor which allows usage of Education of the Slovak Republic, within the project ITMS 26220220089 advanced temperature compensation techniques. “New methods of measurement of physical dynamic parameters and In order to compensate integrated error during run-time it is interactions of motor vehicles, traffic flow and road”. Authors are with Dept. of Control and Information Systems, Faculty of necessary to use a secondary absolute sensor and provide a Electrical Engineering, University of Žilina, Univerzitná 8215/1, Žilina data fusion. The secondary sensor may be much noisier and 01026, Slovakia. e-mail: have slower response but its error has to be kept inside fixed {dusan.nemec; ales.janota; marian.hrubos; vojtech.simak}@fel.uniza.sk bounds. A modification of the Kalman filter can be used as a This is the accepted version of the manuscript: D. Nemec, A. Janota, M. Hruboš and V. Šimák, "Intelligent Real-Time MEMS Sensor Fusion and Calibration," in IEEE Sensors Journal, vol. 16, no. 19, pp. 7150-7160, Oct.1, 2016, doi: 10.1109/JSEN.2016.2597292, URL: http://ieeexplore.ieee.org/document/7529156 core of the sensor fusion algorithm [3][8][9][23], however it each axis and constant: might be difficult to estimate parameters of the filter MSE(i t)  MSE(i )  Egyro  const (3) (covariance matrix, state model) for a standalone sensor MSEs of the updated uncompensated rotational matrix Ru system because the Kalman filter parameters depend on the are: measured system. Another sensor fusion utilizes Bayesian MSE(R )  MSE(R )  R 2 E  R 2 E  networks and the stochastic approach [10][11][12]. We have u1,k 1,k 2,k gyro 3,k gyro (4.1) 2 2 proposed a heterogeneous sensor fusion method for one   y MSE(R3,k )   z MSE(R2,k ), differential sensor and one absolute sensor which requires MSE(R )  MSE(R )  R 2 E  R 2 E  only minimum count of parameters independently from the u2,k 2,k 3,k gyro 1,k gyro (4.2) 2 2 measured system. The performance of our algorithm will be   z MSE(R1,k )   x MSE(R3,k ), compared with the performance of the extended Kalman filter 2 2 MSE(Ru3,k )  MSE(R3,k )  R1,k Egyro  R2,k Egyro  (EKF) used in direct form described in [23]. (4.3)   2 MSE(R )   2MSE(R ), Our real-time calibration method utilizes error estimate x 2,k y 1,k obtained as a side output from the sensor fusion algorithm. where index k = 13. As can be seen, errors of all matrix This approach eliminates the need of steady state detection elements are increasing with new samples. Uncompensated and offline calibration. Since it can be running all the time Euler angles are then [15]: when the sensor is in use our method should compensate long- gyro  atan2Ru2,3,Ru3,3 , (5.1) term drifts continuously. gyro  arcsin Ru1,3 , (5.2)   atan2 R , R . (5.3) II. HETEROGENEOUS FUSION ALGORITHM CONSIDERING gyro  u1,2 u1,1  QUALITY Corresponding MSEs of the Euler angles are: R 2MSE(R )  R 2MSE(R ) The method will be explained on the example of the fusion MSE( )  u3,3 u2,3 u2,3 u3,3 , (6.1) of the 3-axial gyroscope (velocity sensor), 3-axial gyro 2 2 2 Ru2,3  Ru3,3  accelerometer (absolute attitude sensor) and 3-axial MSE(R ) magnetometer (absolute heading sensor). Sensor axes are MSE( )  u1,3 , (6.2) gyro 1 R 2 orientated according to the NED convention (x-North or u1,3 2 2 forward, y-East or right, z-Down), Euler angles are computed Ru1,1 MSE(Ru1,2 )  Ru1,2 MSE(Ru1,1 ) MSE( gyro)  . (6.3) in the ZYX convention (α – Roll, β- Pitch, γ- Yaw). Attitude 2 2 2 Ru1,1  Ru1,2  of object is then expressed by roll and pitch angles; heading is expressed by yaw angle. These formulas are undefined at the gimbal lock (cos  = 0 In order to express the quality of estimation we will use the and Ru1,3= 1). If such condition occurs it is impossible to mean square error (MSE). In general the error model of the determine both roll and yaw (one has to be chosen) and MSE attitude estimation is nonlinear [13]. MSE of the directly estimation is very imprecise. measured data is considered constant and depends on the used B. Estimating roll and pitch from accelerometer readings sensor; MSE of a computed variable y = f(x , x , …,x ) is 1 2 N Secondary, the attitude of the object can be obtained from approximated by: 2 acceleration readings by formulas [14]: N  f    (1) acc  atan2(ay , az ) , (7) MSE(y)    MSE(xk ). k1 x  k  2 2 acc  atan2(ax , ay  az ) , (8) A. Estimating Euler angles from gyroscope readings where ax, ay, az are the components of the acceleration In the inertial navigation the object’s Euler angles are measured by the accelerometer bound with a moving object. primary computed from the angular velocity ω measured by MSE of attitude estimation depends on the dynamics of the the gyroscope. There are several methods of angular velocity system (the vector a measured by the accelerometer is a sum integration into Euler angles; we usually use the matrix-based of the gravitational acceleration g and the object’s own algorithm. Rotation is expressed as the 3D transformation acceleration including vibrations which might be useful in matrix R updated by the infinitesimal update matrix computed different applications [16]). If the object is steady, variance of from each sample of the angular velocity [14]: the vector a is smaller and attitude estimation is more precise: 2 2  1 z t   y t a MSE(a )  a MSE(a )   MSE( )  y z z y R  R  R, where R    t 1  t (2) acc 2 , (9) u update update  z x  a 2  a 2    y z    y t  x t 1  MSE(acc )  and ωx, ωy, ωz are the Cartesian components of the angular 2 2 2 2 2 2 velocity vector and Δt is a sampling period of the gyroscope. a y  az  MSE(ax )  ax a y MSE(a y )  az MSE(az ) (10)  2 2 2 2 2 2 a y  az ax  a y  az 

The error of the gyroscope can be considered the same for In order to decrease error of estimation the acceleration can be averaged from multiple samples (oversampled This is the accepted version of the manuscript: D. Nemec, A. Janota, M. Hruboš and V. Šimák, "Intelligent Real-Time MEMS Sensor Fusion and Calibration," in IEEE Sensors Journal, vol. 16, no. 19, pp. 7150-7160, Oct.1, 2016, doi: 10.1109/JSEN.2016.2597292, URL: http://ieeexplore.ieee.org/document/7529156 measurement). Then MSEs of average acceleration and can be considered constant: components are: MSE(Bi )  Emag . (19) N 1 2 MSE(ai ) MSE(ai )  ai [k]  ai   C. Heterogeneous fusion algorithm N k 1 N 2 (11) N N The scheme shown in Fig. 1describes the algorithm that can 1 2  1  MSE(ai )   ai [k]    ai [k]  . be used for fusion of the Euler angles computed from N k 1  N k1  N gyroscope, accelerometer and magnetometer readings. The second expression allows processing of accelerometer The fusion algorithm is based on incremental compensation samples by batches with size N with lower memory of difference between incrementally integrated Euler angles requirements. Accelerometer’s own errors MSE(a ) are i  ,  ,  and absolute but noisy Euler angles  ,  , negligible at higher N with respect to the errors caused by gyro gyro gyro acc acc  . The adaptive gain block (see Fig. 1) optimizes the impact vibrations. However, the simulations have shown that using mag of each data source in order to increase resultant precision. A batches is causing relatively large step changes in the resultant value with lower MSE has a higher effect to the result [18]. If estimated variable. Therefore it is better to use online the gain block has unit gain, the values obtained by gyroscope approximation of average and MSE; then the fusion can be integration are not taken into account and result is equal to performed in each step [17]: Euler angles obtained from the accelerometer and gyroscope. N 1s [n 1]  a [n]2 s [n]  i i , (12.1) i N N 1a [n 1]  a [n] a [n]  i i , (12.2) i N where si[n]is a mean square of the i-th acceleration component in the n-th step. Formulas (12) are first order low-pass IIR filters with cut-off frequency: f  1  sample   fcut  arccos 1  . (13) 2  2NN 1

MSE of the estimated average ai[n] is then: MSE(a [n]) MSE(a [n])  s [n]  a [n]2  i . (14) i i i N In order to compute yaw from the magnetic induction vector B, it has to be rotated from objects’ local coordinates to the global horizontal plane by following:

Bx   cos sin sin cos sin  Bx         By    0 cos  sin   By  . (15)       Bz   sin sin cos cos cos  Bz  Yaw is then computed by the formula:

 mag  atan2(By , Bx) . (16) The vertical component of the magnetic induction B is not z Fig. 1. The fusion algorithm scheme. used; therefore the third row of the matrix in (15) can be Resultant compensated Euler angles are equal to: omitted in the algorithm. Since the transformation (15) is   gyro d  Kacc  1 K gyro using estimated roll and pitch, quality of the yaw estimation             d  K   1 K  , (20) depends on the quality of the attitude estimation.    gyro      acc    gyro         2 2    gyro d K  mag  1 K  gyro MSE(Bx )  cos  MSE(Bx )  sin sin  MSE(By )         cos sin 2 MSE(B )  where K, K, K are variable fusion gain coefficients adjusted z (17.1) 2 according to the estimation errors and their values vary from 0  By cos sin  Bz sin sin  MSE()  to 1. The vector [d, d, d] represents fusion deviations: 2  Bx sin  By sin cos  Bz cos cos  MSE( ) d  K  acc  gyro 2 2     MSE(By )  cos  MSE(By )  sin  MSE(Bz )  d  K    (21) (17.2)       acc gyro 2 d  K      Bz cos  By sin  MSE().       mag gyro Then MSE of the yaw estimated by the magnetometer is: In case when fusion deviation di is not available due to 2 2 different sampling frequencies, it is simply considered zero. By  MSE(Bx )  Bx  MSE(By ) MSE( mag )  2 . (18) Usually the magnetometer has much lower sampling B 2  B 2  y x  frequency than gyroscope or accelerometer. When the new Error of the magnetometer is also the same for each axis sample from magnetometer is not available, the fusion This is the accepted version of the manuscript: D. Nemec, A. Janota, M. Hruboš and V. Šimák, "Intelligent Real-Time MEMS Sensor Fusion and Calibration," in IEEE Sensors Journal, vol. 16, no. 19, pp. 7150-7160, Oct.1, 2016, doi: 10.1109/JSEN.2016.2597292, URL: http://ieeexplore.ieee.org/document/7529156 deviation dγ is null. 2 MSE(R2,1 )  sin sin  cos sin cos  MSE()  Fusion deviations are important inputs for the automatic  sin cos cos 2 MSE()  (29.4) calibration algorithm described in the next chapter. Their 2 errors are:  cos cos  sin sin sin  MSE( ), 2 2 MSE(d )  K MSE(acc )  MSE(gyro), (22.1) MSE(R2,2 )  sin cos  cos sin sin  MSE()  2  sin cos sin 2 MSE()  (29.5) MSE(d )  K MSE(acc )  MSE(gyro), (22.2) 2 2  cos sin  sin sin cos  MSE( ), MSE(d )  K MSE( mag )  MSE( gyro). (22.3) MSE(R )  cos cos 2 MSE()  sin sin 2 MSE() , (29.6) The fusion gain coefficients have to be adjusted in order to 2,3 2 minimize the output error which is equal to: MSE(R3,1 )  cos sin  sin sin cos  MSE()  2 2 2 MSE()  K MSE(acc )  (1 K ) MSE(gyro) . (23)  cos cos cos  MSE()  (29.7) Note that this formula is also valid for pitch and yaw angles  sin cos  cos sin sin 2 MSE( ), with corresponding coefficients. Output error is minimal 2 MSE(R )  cos cos  sin sin sin  MSE()  when: 3,2 2 MSE() 2MSE()  cos cos sin  MSE()  (29.8)  0   0 . (24) 2  sin sin  cos sin cos 2 MSE( ), K K 2 2 Solving these conditions we obtain the optimal gain: MSE(R3,3 )  sin cos  MSE()  cos sin  MSE(). (29.9) MSE( ) gyro By comparing expressions in brackets with elements of the K  . (25) MSE(gyro)  MSE(acc ) matrix R in (28) it is possible to simplify the formulas (29): MSE(R )  sin cos 2 MSE()  R 2MSE( ), D. Proof of Precision Improvement 1,1   1,2 (30.1) 2 2 We can substitute the optimal gain in the formula (23): MSE(R1,2 )  sin sin  MSE()  R1,1 MSE( ), (30.2)

MSE(gyro)  MSE(acc ) 2 MSE()  . (26) MSE(R1,3 )  cos  MSE(), (30.3) MSE(gyro)  MSE(acc ) 2 2 MSE(R2,1 )  R3,1 MSE()  sin cos cos  MSE()  By dividing by MSE(gyro) we can get: (30.4) 2 MSE() MSE( )  R2,2 MSE( ),  acc  1 . (27) 2 2 MSE(gyro) MSE(gyro)  MSE(acc ) MSE(R )  R MSE()  sin cos sin  MSE()  2,2 3,2 (30.5) Since MSE is always higher than zero MSE() is always 2  R2,1 MSE( ), smaller than MSE(gyro). Since the formula (26) is 2 2 MSE(R2,3 )  R3,3 MSE()  sin sin  MSE(), (30.6) symmetrical, the same is valid for MSE(acc). Therefore the theoretical fusion output error is always smaller than errors of MSE(R )  R 2MSE()  cos cos cos 2 MSE()  3,1 2,1 (30.7) any single estimation. Note that the output error directly  R 2MSE( ), depends on estimation of the MSEs during execution of the 3,2 MSE(R )  R 2 MSE()  cos cos sin 2 MSE()  algorithm. 3,2 2,2 (30.8)  R 2MSE( ) E. Error of the Resultant Rotational Matrix 3,1 2 2 Fig. 1 contains the conversion block from compensated MSE(R3,3 )  R2,3 MSE()  cos sin  MSE() . (30.9) Euler angles to the rotational matrix [15]: Initial MSE of Euler angles should be initialized to a large

 c c c s  s  value representing low quality, e.g.:   2 2 R  s s c  c s s s s  c c s c  , (28) MSE(0 )  MSE( 0 )   MSE(0 )   2 . (31)   c s c  s s c s s  s c c c  In order to avoid extreme values of MSE (e.g. around where s = sin , c = cos , etc. Note that the matrix R = Ru gimbal lock singularity, see (6)), it is convenient to limit MSE when no magnetometer and accelerometer samples are of the rotational matrix to the interval (-1, 1). Maximal values available due to the different sampling frequency. for Euler angles’ MSEs can be set according to (31). When a new sample is processed and errors of the compensated Euler angles are computed according to (26), it III. THE REAL-TIME CALIBRATION ALGORITHM is possible to compute MSE of each element in the rotational In this chapter we will discuss how fusion deviation (a side matrix which is used in next sample processing (used in (4)): product of the fusion of two or more sensors) can be used for 2 2 sensor calibration. It is convenient when error estimate of MSE(R1,1 )  sin cos  MSE()  cos sin  MSE( ) , (29.1) 2 2 deviation is also available. The algorithm will be explained on MSE(R1,2 )  sin sin  MSE()  cos cos  MSE( ) , (29.2) the example of the gyroscope, accelerometer and 2 magnetometer fusion proposed in the previous chapter. MSE(R1,3)  cos  MSE() , (29.3)

3-axial MEMS vibrational gyroscope and accelerometer are usually used for this purpose [4]). Each axis has its own gain and the offset calibration functions (independently from each other) according to (33). However, if the vector variable is measured, it is necessary to take sensor orientation into account. The most general case occurs when each axis is measured by a single physical sensor. The relation between the vector of measured raw data w and the calibrated vector ω is following (in the orthonormal coordinate system): ω  AG(T)w  B(T), (37) where G(T) is a diagonal matrix of the shifted gain functions and B(T) is a vector of the bias functions:

Gx (T) 1 0 0    G(T)   0 Gy (T) 1 0  , (38.1    0 0 Gz (T) 1 Fig. 2. The real-time calibration scheme in the context of the sensor data processing and fusion. Bx (T)   B(T)  By (T) . (38.2) B (T) A. Calibration model  z  Each of G (T) and B (T) functions are polynomials A universal relation between the raw value w measured by i i according to (34) and (35) respectively. For example, the MEMS sensor and the actual value ω of the measured coefficients of the polynomial G (T) are marked g g …, variable is following: x x,0 x,1 coefficients of the polynomial Bx(T) are marked bx,0 bx,1 …etc.   C(w, T) , (32) The matrix A is a constant alignment matrix which where T is sensor’s temperature and C(w,T) is a calibration considers misalignment between sensor’s axes and the object’s transfer function [19]. Note that errors caused by the sensor’s axes [22]. Since the gain matrix G(T) normalizes each axis hysteresis are neglected [20]. separately, the alignment matrix has to be column-normalized: Our algorithm assumes first order calibration transfer  1  a 2  a 2 a a  function at any given temperature, therefore there are two one-  2,1 3,1 1,2 1,3  dimensional functions: gain G(T) and bias B(T) for which it is A   a 1  a 2  a 2 a  . (39)  2,1 1,2 3,2 2,3  valid:  a a 1  a 2  a 2    G(T)  w  B(T) (33)  3,1 3,2 1,3 2,3  Since the C(w,T) function is continuous, partial functions B. Backpropagation of the fusion error G(T) and B(T) are also continuous and they can be expressed by polynomials: Parameters ai,k gi,k bi,k have to be obtained by learning. Since N the true value of the measured variable is unavailable during k G(T)   gkT , (34) runtime and the data processing algorithm is nonlinear and k0 N recursive, we cannot minimize the error between raw readings k B(T)  bkT . (35) of the sensor and the actual value as the static (offline) k0 calibration does. However, it is possible to minimize the where gk and bk are constants which change only by the long- fusion deviation (see (21)) in long term. Fusion deviation is term drift (e.g. by aging). In order to suppress sensitivity of therefore used as an output error for learning. Implementation the higher order coefficients, full scale of the sensor raw data of the deterministic least-squares method for non-linear and temperature should be normalized to the interval (0,1) or recursive system would require remembering all previous (-1,1). The proposed learning algorithm requires floating-point samples and would be very complicated. Such approach would number implementation [21]. In order to maintain precision in be difficult to implement into low-cost hardware. Considering full range and avoid ignoring small incremental steps mentioned drawbacks, we have chosen to use a stochastic (especially by 32-bit floating point IEEE754 format), the gain learning algorithm. function should be shifted by one. Biases of the MEMS In order to decrease memory requirements and decrease the sensors drift faster than gain, therefore it is convenient to computational time for one iteration of the learning algorithm, make the bias function independent from the gain function. we have decided to avoid recurrent learning. To be able to do The modified relation is: that it is necessary to back-propagate the fusion deviation   G(T) 1w  B(T). (36) T vector [d, d, d] through the data processing algorithm to T If the calibration formula (36) is used, the polynomial the angular velocity error vector e = [ex, ey, ez] . The data coefficients gk and bk can be initialized to zero. processing algorithm disregarding sensor fusion can also be In inertial navigation it is required to measure vectors of modelled by the following continuous non-linear differential linear acceleration and angular velocity in all three axes (the equation [15]: This is the accepted version of the manuscript: D. Nemec, A. Janota, M. Hruboš and V. Šimák, "Intelligent Real-Time MEMS Sensor Fusion and Calibration," in IEEE Sensors Journal, vol. 16, no. 19, pp. 7150-7160, Oct.1, 2016, doi: 10.1109/JSEN.2016.2597292, URL: http://ieeexplore.ieee.org/document/7529156

 d  By differentiation of (37) we get:    sin sin cos sin  T dt  a    1  ω 1,2 cos cos  x   G (T) 1w  B (T) 1 0 , (45.1)  d      a y y y a    0 cos  sin   , (40) 1,2  2,2  dt    y     sin cos    T 0  z   a   d   cos cos  ω 1,3    Gz (T) 1wz  Bz (T) 1 0  , (45.2)   a a  dt  1,3  3,3  The 3x3 matrix in (40) represents sensitivity of the output T ω a2,1  (Euler angles) to the calibrated gyroscope readings. The  Gx (T) 1wx  Bx (T)  1 0 , (45.3) a2,1  a1,1  inverted matrix can be used to transform errors of the Euler   T angles (fusion deviations) to the errors of the calibrated ω  a   G (T) 1 w  B (T)  0 1 2,3 , (45.4) angular velocity:  z  z z    a2,3  a3,3 

T   e 1 0  sin d  ω a3,1  x      G (T) 1w  B (T)  0 1 , (45.5)   1     x x x e  0 cos sin cos  d , (41) a3,1  a1,1   y  t      cal   T ez  0  sin cos cos  d       ω  a3,2   Gy (T) 1wy  By (T) 0 1 , (45.6) where Δtcal is a period of the calibration procedure (can be a3,2  a2,2  larger than the sampling period of the gyroscope). The relation ω (41) is valid only if the Euler angles α, β, γ are estimated  w  B (T)T k  a a a T , (46) g i i 1,i 2,i 3,i precisely, which is achieved by compensation of the errors by i,k sensor fusion. In order to avoid invalid adjustments of the ω k T  Gi (T) 1T  a1,i a2,i a3,i  . (47) calibration parameters caused by initial low quality of the bi,k estimated Euler angles, the calibration procedure is applied where Gi(T) is the value of the polynomial Gi at the current only if the fusion deviation is below some predefined normalized temperature T etc. Speed of learning depends on threshold. This approach also eliminates the need for recurrent the setting of learning rate. Because the bias of MEMS sensors learning and the learning algorithm optimizes the error vector drifts rapidly, learning rate of the coefficients bi,k should be of the gyroscope (instead of direct optimization of the fusion highest. Since the alignment matrix is constant, learning rate deviation). of ai,k coefficients should be the smallest. This distribution avoids false changes in the alignment matrix according to the C. Adaptive Scaling of the Fusion Deviation short-term drift of the sensor bias. Since the smaller calibration error will cause the smaller parameter change we can scale the fusion deviations according IV. EXPERIMENTAL RESULTS to their MSEs (22). The scaling function applied before For evaluation of the proposed fusion and calibration backpropagation and learning can be e.g.: algorithms we have used both the simulation model of the  MSE(d)  d  d  max1 , 0 , (42) sensor and the real MEMS sensor. Simulation allows us to  2   emax  compare real bias and gain parameters with those obtained by learning. We have compared performance of the extended where emax is the maximal RMS of the fusion deviation acceptable for usage in calibration. This avoids degradation of Kalman filter implemented according to [23], fusion with calibration by initial low-quality data. fixed gain (also known as the complementary filter) and our proposed fusion algorithm with adaptive gain based on D. The Learning Algorithm estimation of MSE. It is possible to choose from many incremental learning A. Sensor Fusion Simulation algorithms. We have chosen the Adam algorithm [24], which is an extension of the well-known gradient descent algorithm. The first simulation analyzed a steady-state at nonzero Its advantage over the standard gradient descent algorithm is attitude  = 30°,  = -45°,  = 60°. The simulated sensors’ raw its build-in 1st order infinite response filter for gradient and readings included noise according to Table 1; all sensors have adaptive learning rate. As a result, the calibration parameters used the output sample rate 512Hz. MSE of the sensor ci,k develop smoother than by the standard gradient descent readings were considered to be equal to the square of RMS method. and time-invariant. Goal of the learning is to minimize the loss function E: TABLE I SIMULATED NOISE PARAMETERS 1 1 E  ω  ω2  e 2 . (43) 2 real 2 Sensor RMS Bias The Adam algorithm requires gradients of the loss function by Gyroscope 0.5 °/s 20 °/s each parameter which is given by: Accelerometer 1.0 m/s2 none Magnetometer 10 % of full range none E ω  e  (44) ci,k ci,k This is the accepted version of the manuscript: D. Nemec, A. Janota, M. Hruboš and V. Šimák, "Intelligent Real-Time MEMS Sensor Fusion and Calibration," in IEEE Sensors Journal, vol. 16, no. 19, pp. 7150-7160, Oct.1, 2016, doi: 10.1109/JSEN.2016.2597292, URL: http://ieeexplore.ieee.org/document/7529156

The value of the fixed fusion gain has been chosen TABLE II according to the steady value of the adaptive gain (approx. COMPARISON OF FUSION ALGORITHMS IN A STEADY ATTITUDE Roll Pitch Yaw Fusion Execution time 0.05 according to the Fig. 4). As can be seen in Fig. 3 the RMS RMS RMS algorithm [ms/sample] fusion algorithm with adaptive gain has shorter convergence [deg] [deg] [deg] time than the other methods. Faster convergence is caused by Fixed gain 1.17 1.06 2.31 0.21 the peak of fusion gain (see Fig. 4) which is a result of initial Adaptive gain 1.09 0.93 1.56 0.23 Kalman filter 0.58 0.53 1.06 0.29 low quality of the output (see (31)) and system increased fusion gain in order to compensate rapidly the initial errors by In order to verify dynamic parameters of the sensor fusion accelerometer. Results of fusion using fixed gain converge to algorithm we have simulated harmonic rotation around axis x the results of the fusion with adaptive gain after approx. 150 (see Fig. 5). Due to the definition of Euler angles in Z-Y-X milliseconds. Output of the Kalman filter has much smoother convention, the rotation around x-axis will affect only roll response and lower error in a steady state. angle. Noise parameters of the sensors are the same as those used in the previous simulation. In order to visualize the difference between compared algorithms, Fig. 5 displays only the beginning of the experiment. Fig. 6 illustrates one important advantage of our proposed fusion algorithm over extended Kalman filter – stability in highly dynamic conditions. As can be seen the Kalman filter becomes instable after 6 periods of harmonic banking at frequency 1 Hz, but our proposed algorithm maintains its stability and precision. If we decrease the frequency of the banking (rotation) below 0.5 Hz, the instability of the extended Kalman filter disappears.

Fig. 3. The estimated roll angle in a steady state from noisy data with the fusion after reset.

Fig. 5. The roll angle estimation during simulated periodic banking from -60° to +60°.

Fig. 4. The adaptive gain of the roll estimation fusion in a steady state after reset.

RMS errors of the estimated Euler angles in a steady attitude (100 seconds of experiment) are compared in Table 2. Execution time of the algorithm depends on implementation and hardware, therefore the values shown in Table 2 can be used only for relative comparison between discussed methods. All algorithms were implemented in the MATLAB Fig. 6. Instability of the extended Kalman filter during periodic banking environment; it is possible to decrease the execution time by after longer period. using a compiled programming language like C and the code optimization. This is the accepted version of the manuscript: D. Nemec, A. Janota, M. Hruboš and V. Šimák, "Intelligent Real-Time MEMS Sensor Fusion and Calibration," in IEEE Sensors Journal, vol. 16, no. 19, pp. 7150-7160, Oct.1, 2016, doi: 10.1109/JSEN.2016.2597292, URL: http://ieeexplore.ieee.org/document/7529156

B. Sensor Fusion Experiment Due to the vibrations caused by the servomotor during Experiments with the real MEMS sensor utilized a movement the adaptive fusion gain is lower while the combined sensor board containing the 3-axial gyroscope and servomotor is running (see Fig. 9). Note that the gain is accelerometer IMU-3000 and the 3-axial magnetometer stabilized in a steady state in the value K = 0.1 which was also LSM303DLH). The noise parameters of the used sensor used in the constant gain fusion in above comparison. module are shown in Table 3. TABLE III REAL NOISE PARAMETERS

Sensor Full scale RMS [x,y,z]

Gyroscope 500 °/s [0.43, 0.41, 0.48] °/s Accelerometer 8 g ≈ 78.5 m/s2 [0.65, 0.53, 0.51] m/s2 Magnetometer 4 Gauss [0.12, 0.19, 0.09] Gauss

All noise parameters are estimated in a steady state. The bias of the accelerometer and magnetometer is considered to be zero (due to the previous static calibration). The sensor board was banked by a servomotor from zero roll to approx. - 54 degrees (see Fig. 7). Filter parameter N used in (12) and (14) was set to N = 5 which corresponds to the cut-off frequency fcut ≈ 18 Hz at the sampling rate 512 Hz. The absolute errors of the constant gain and the adaptive gain fusion are compared in Fig. 8. MSE of the Kalman filter is Fig. 9. The adaptive fusion gain during the experiment. slightly higher (0,5%) than MSE of our proposed algorithm. C. Experimental Validation of the Calibration Algorithm The proposed real-time calibration algorithm was used to estimate the bias and gain matrices of our gyroscope sensor module. Initial calibration parameters gi,k and bi,k were null. Since all sensors are placed on one board the sensor misalignment was neglected; therefore the A matrix according to (39) was not needed to be adjusted. The thermal drifts of calibration parameters were not considered during the experiment because temperature of the sensor module was stable. If the general temperature-dependent version of the calibration algorithm is used the higher-order thermal coefficients gi,k>0 and bi,k>0 has to be adjusted slowly because their real values do not change rapidly during lifetime of the sensor. Mentioned higher-order calibration parameters allow faster adaptation to different temperature which can be useful in some applications (e.g. indoor-outdoor transition of a Fig. 7. Roll movement during the experiment compared with the values mobile robot). estimated from the accelerometer only and from the adaptive fusion.

Fig. 10. Raw measured angular velocity around the x-axis during the Fig. 8. Error of the estimated roll during the experiment. experiment.

As an experimental input we have used a previously The used gyroscope module has internally compensated mentioned combined MEMS sensor module. The module was gain therefore the gain deviance is very small (note scale of moved randomly with no pre-defined pattern (see Fig. 10 for the y-axis in Fig. 12). an illustration) therefore the precise attitude and yaw are TABLEV unknown. The raw (non-calibrated) accelerometer and COMPARISON BETWEEN LEARNED BIAS AND VALUES ESTIMATED BY STATIC CALIBRATION magnetometer readings as a secondary input have been used in Learned Bias obtained by Error of the learned Gyroscope order to demonstrate robustness of the learning algorithm. The bias static calibration bias axis learning parameters were following: [rad/s] [rad/s] [% of the full scale] -2 -2 x -2.0·10 -2.52·10 0.05 % y -0.9·10-2 -1.19·10-2 0.03 % TABLE IV z 1.5·10-2 1.26·10-2 0.03 % LEARNING PARAMETERS Learning Parameter Symbol Value V. DISCUSSION -6 Bias learning rate λbias 10 -8 As can be seen in Fig. 3 gyroscope bias is effectively Gain learning rate λgain 10 st 1 momentum filter [24] β1 0.999 suppressed by the sensor fusion. The fusion with the constant nd 2 momentum filter [24] β2 0.9999 fusion gain causes smoother output but higher estimation Maximal fusion MSE emax 5 °/s error. The adaptive gain allows very fast reactions and start- The given learning rate is close to the upper limit for usage up. According to Fig. 4 the adaptive algorithm reflects initial in real systems; since the whole fusion algorithm is iterative low quality of roll estimation; therefore the gain (weight of the (closed loop) higher learning rates could cause instability. The attitude estimated by the accelerometer) is initially high and lower learning rate will result in smoother but also slower bias then rapidly decreases with the roll estimation error. If we development. Resultant bias development during automated compare the proposed algorithm with the widely-used learning is shown in Fig. 11. extended Kalman filter, our algorithm converges faster and it is stable even during very dynamic changes. On the other hand, the extended Kalman filter has smoother response and lower RMS. Another advantage of our algorithm comes from its lower execution time. Experiments with the real MEMS sensors approved results obtained by simulations. The MSE estimation algorithm is able to detect rapid changes in movement including vibrations (see Fig. 9) which adaptively decreases the influence of the accelerometer (absolute but noisy sensor) to the result. Estimation of MSE during algorithm execution also provides additional valuable information about the quality of the result. If additional information about the object’s state is available, it is possible to change input MSE of the accelerometer to reflect known systematic errors. Fig. 11. Gyroscope bias real-time learning. Second experiment series evaluated the automated calibration algorithm. According to Fig. 11 the bias learning works also during movement and slowly converges to the precise bias values. Disadvantage of the intelligent calibration in comparison with laboratory calibration is its lower precision which can be improved by decreasing of the learning rate. The lower learning rate will however require longer learning time. The learning rate of the bias Bi has to be much higher (100- times) than the learning rate of the gain Gi, otherwise the calibration stability might be corrupted and the overall calibration parameters would diverge.

VI. CONCLUSION In this paper we have proposed the improved sensor fusion algorithm. As an explanatory example we have used the fusion between the 3-axial gyroscope (measuring angular velocity), Fig. 12. Real-time learning of the gyroscope’s gain. 3-axial accelerometer (measuring acceleration of the local Comparison with the actual bias values measured after the system including gravity) and 3-axial magnetometer experiment by static calibration is in Table 5. As can be seen, (measuring Earth’s magnetic field induction) into estimation the real-time calibration method converges to the real values. of attitude and yaw (AHRS system). The algorithm is based on This is the accepted version of the manuscript: D. Nemec, A. Janota, M. Hruboš and V. Šimák, "Intelligent Real-Time MEMS Sensor Fusion and Calibration," in IEEE Sensors Journal, vol. 16, no. 19, pp. 7150-7160, Oct.1, 2016, doi: 10.1109/JSEN.2016.2597292, URL: http://ieeexplore.ieee.org/document/7529156 the estimation of the mean square error during run-time. Then REFERENCES the theoretical optimal fusion gain is computed. According to [1] W. Yuanxin, and S. Wei, “On Calibration of Three-Axis Fig. 4 and Fig. 5 the adaptive gain has comparable results with Magnetometer,” Sensors Journal, IEEE, vol. 15, no. 11, pp. 6424- the carefully chosen fixed gain fusion, but has much better 6431. dynamic characteristics (at least 5-times shorter rise time). [2] Z. Zhang, and G. Yang, "Calibration of Miniature Inertial and Additionally, the parameters of adaptive systems are easier to Magnetic Sensor Units for Robust Attitude Estimation," IEEE measure directly (e.g. noise parameters of the used sensors are Transactions on Instrumentation and Measurement, vol.63, no.3, usually available from their manufacturer) comparing with the pp.711-718. [3] Q. Gan, and C.J. Harris, "Comparison of two measurement fusion difficulty of proper fixed fusion gain selection. However, methods for Kalman-filter-based multisensor data fusion," IEEE output MSE should be considered only as a qualitative factor Transactions on Aerospace and Electronic Systems, vol.37, no.1, since the formula used for estimation of the mean square error pp.273-279. is only the first order approximate. Main disadvantage of the [4] H. Qasem, O. Gorgis, and L. Reindl, "Design and calibration of an adaptive fusion algorithm is the higher CPU load (approx. 2- inertial sensor system for precise vehicle navigation," in Proc. times more CPU time needed for MSE estimation compared WPNC, 2008, pp.229-231. with the fixed-gain based fusion algorithm). [5] L. Wang, Y. Hao, and F. Wang, "Calibration of low cost MEMS inertial Measurement Unit for an FPGA-based navigation system," in Second part of the paper proposes the innovative run-time ICIA, 2011, pp.181-186. calibration method based on processing of fusion deviation [6] L. You, J. Georgy, N. Xiaoji, L.Qingli, and N. El-Sheimy, data. The system was designed to utilize any incremental “Autonomous Calibration of MEMS Gyros in Consumer Portable stochastic optimization (learning) method; in this article we Devices,” Sensors Journal, IEEE, vol. 15, no. 7, pp. 4062-4072. have deployed learning method called Adam, which is an [7] S.A. Zotov, B.R. Simon, A.A. Trusov, and A.M. Shkel, “High extension of the gradient descent method [24]. Because the Quality Factor Resonant MEMS Accelerometer with Continuous learning algorithm is using small learning rate, it is resistant to Thermal Compensation,” Sensors Journal, IEEE, vol. 15, no. 9, pp. 5045-5052. the occasional high-power noise contained in fusion data. This [8] Z. Yingwei, “Cubature + Extended Hybrid Kalman Filtering Method feature is supported by evaluation of the mean square error of and its Application in PPP/IMU Tightly Coupled Navigation the fusion. In the discussed case of inertial attitude Systems,” Sensors Journal, IEEE, vol. 15, no. 12, pp. 6973-6985. measurement our calibration algorithm is most suitable if the [9] A. Assa, and F. Janabi-Sharifi, “A Kalman Filter-Based Framework measured movement is not continuous because in a steady for Enhanced Sensor Fusion,” Sensors Journal, IEEE, vol. 15, no. 6, state the quality of estimated Euler angles rises with time. The pp. 3281-3292. algorithm automatically recognizes such a state and measured [10] G. Mirzaei, M.M. Jamali, J. Ross, P.V. Gorsevski, and V.P. Bingman, “Data Fusion of Acoustics, Infrared, and Marine Radar for data have greater impact on the calibration parameters. Since Avian Study,” Sensors Journal, IEEE, vol. 15, no. 11, pp. 6625-6632. the fusion is heterogeneous (absolute sensor data are merged [11] P. Chongyan, and S. Shuli, “Fusion Predictors for Multisensor with derivative sensor data) the bias of the absolute sensor Stochastic Uncertain Systems with Missing Measurements and does not affect the calibration procedure. Unknown Measurement Disturbances,” Sensors Journal, IEEE, vol. Although the fusion method proposed by this manuscript is 15, no. 8, pp. 4346-4354. derived for this special case, authors believe it can be used in [12] L. Chang, K. Li, and B. Hu, “Huber’s M-Estimation-Based Process many other applications as an alternative to the extended Uncertainty Robust Filter for Integrated INS/GPS,” Sensors Journal, IEEE, vol. 15, no. 6, pp. 3367-3374. Kalman filter. The proposed fusion method is especially [13] M. Zhong, D. Guo, and J. Guo, “PMI-Based Nonlinear H∞ suitable, if the sensor readings are processed by non-linear Estimation of Unknown Sensor Error for INS/GPS Integrated functions in a recursive way. The proposed fusion and System,” Sensors Journal, IEEE, vol. 15, no. 5, pp. 2785-2794. calibration methods can be adjusted to other sensor fusion [14] A. Janota, V. Šimák, D. Nemec, andJ. Hrbček, „Improving precision scenarios (e.g. combination of the GNSS system – absolute and speed of Euler angles computing from low cost sensor data,“ velocity and a position sensor and accelerometer – a Sensors, vol. 15, no. 3, pp. 7016-7039. differential velocity sensor; an impulse volume sensor and a [15] J. Diebel. (2006). Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors. Stanford University. [Online]. flow sensor and many others). Available: http://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf ACKNOWLEDGMENT [16] R. Hostettler, W. Birk, M.L. Nordenvaad, “Joint Vehicle Trajectory This work has been supported by the European Regional and Model Parameter Estimation Using Road Side Sensors,” Sensors Development Fund and the Ministry of Education of the Journal, IEEE, vol. 15, no. 9, pp. 5075-5086. [17] V. Kreinovich, et al., “Towards Combining Probabilistic and Interval Slovak Republic, within the project ITMS 26220220089 “New Uncertainty in Engineering Calculations: Algorithms for Computing methods of measurement of physical dynamic parameters and Statistics under Interval Uncertainty, and Their Computational interactions of motor vehicles, traffic flow and road.” Complexity,” Reliable Computing, vol. 12, no. 6, pp. 471-501. [18] J. Wei, L. Yong, and C. Rizos, “Optimal Data Fusion Algorithm for Navigation Using Triple Integration of PPP-GNSS, INS, and Terrestrial Ranging System,” Sensors Journal, IEEE, vol. 15, no. 10, pp. 5634-5644. [19] L. Meyer, A. Buhmann, D.C. Meisel, and J.G. Korvink, “Relationship Between Zero-Rate Output and the MEMS Element in This is the accepted version of the manuscript: D. Nemec, A. Janota, M. Hruboš and V. Šimák, "Intelligent Real-Time MEMS Sensor Fusion and Calibration," in IEEE Sensors Journal, vol. 16, no. 19, pp. 7150-7160, Oct.1, 2016, doi: 10.1109/JSEN.2016.2597292, URL: http://ieeexplore.ieee.org/document/7529156

a Closed-Loop System,” Sensors Journal, IEEE, vol. 15, no. 12, pp. Marián Hruboš was born in 1987 in Martin, 7200-7207. Slovakia. Currently works as a postdoc researcher at [20] S. Luczak, “Experimental Studies of Hysteresis in MEMS the Dept. of Control and Information Systems of the Faculty of Electrical Engineering at the University of Accelerometers: A Commentary,” Sensors Journal, IEEE, vol. 15, Žilina (UNIZA). His research is focused on no. 6, pp. 3492-3499. industrial automation, electrical engineering and [21] M. Hoffman, P. Bauer, B. Hemrnelman, and A. Hasan, "Hardware programming, particularly on data fusion from synthesis of artificial neural networks using field programmable gate multiple sensors for creationof 3Dmacro-textured arrays and fixed-point numbers," in Region 5 IEEE Conference, models of the real spacesand their use in both virtual 2006, pp.324-328. and real applications. During his Bc. and MSc. [22] F. Jiancheng, and L. Zhanchao, “In-Flight Alignment of POS Based studies he was with the Institute of Competitiveness and Innovations, UNIZA. He was awarded a Certificate of Merit from the Association of on State-Transition Matrix,” Sensors Journal, IEEE, vol. 15, no. 6, Electrotechnical Industry of the Slovak Republic as a designer of the year pp. 3258-3264. 2014. [23] R. Munguia and A. Grau, “A Practical Method for Implementing an Attitude and Heading Reference System,” International Journal of Advanced Robotic Systems, vol. 11, no. 62, pp. 1–12, 2014. Vojtech Šimák was born in Žilina, Czechoslovakia, [24] D. P. Kingma, J. L. Ba, “Adam: A Method for Stochastic in 1980. He received the M.S. degree in information Optimization,” International Conference on Learning and safety-related systems (2004) and the Ph.D. degree in Control Engineering (2008) from the Representations, 2015, arXiv:1412.6980 Faculty of Electrical Engineering, University of Žilina (UNIZA), Slovakia. During his Ph.D. study he stayed for 5 months at the Helsinki University of Dušan Nemec was born in Žilina, Slovakia in 1991. Technology in the Control Engineering Laboratory He received his MSc. degree in Automation from (2007). From 2007 to 2009, he was a researcher the University ofŽilina in 2015. He is currently with the Dept. of Industrial Engineering, Faculty of pursuing the Ph.D. degree in automation at the Dep. Mechanical Engineering, UNIZA. Since 2009 he has worked with the Dept. of of Control and Information Systems of the Faculty Control and Information Systems at the Faculty of Electrical Engineering, of Electrical Engineering at the University of Žilina. UNIZA, Slovakia. His research interests include industrial and mobile His research area is focused on mobile robotics and robotics and positioning systems, particularly navigation, inertial systems and sensor systems, especially inertial navigation and computer vision. flying mobile robots. In 2015 he became a laureate of the award: “Student personality of the Slovakia of the academic year 2014/2015 in the area of Electrical Engineering and Industrial Technologies”.

Aleš Janota was born in 1963 and received his MSc. degree from Technical University of Transport and Communications, Žilina, Czechoslovakia in 1981. He post-graduated from University of Zilina (UNIZA), Slovakia, in 1998 in the field of Telecommunications. From 2003 to 2009 he acted as an Assistant Professor for Information and Safety- related systems. Since 2010 he has worked as a professor in Control Engineering at the Dept. of Control and Information Systems, UNIZA. His research interests include sensors, artificial intelligence and intelligent transportation systems. From 2010 to 2014 he was a national delegate in the Domain Committee for Transport and Urban Development of the COST program.