MEMS IMU / ZUPT Based Cubature Kalman Filter applied to Pedestrian Navigation System

In most related work with Pedestrian navigation, indirect filtering approach is used based on linear error based Kalman Filter (KF). In this research, it is proposed to outperform this approach by the use of direct filtering approach. Based on only MEMS IMU, we propose the use of modern algorithms developed in the last decade; Sigma Point Kalman Filters (SPKF), and recently developed Cubature Kalman Filter (CKF) as a superior alternative to all previous filters. The CKF improves the mean and covariance propagation as compared with EKF and previous SPKF (UKF, CDKF). Although the CKF provides a better estimate of the orientation, velocity and position with Zero velocity UPdaTes (ZUPT) and Zero Angular Rate UpdaTes (ZARUT) measurements. Cubature Information Filter has been also implemented for sake of completeness.


Introduction
The problem of Pedestrian Navigation based on direct Kalman filtering recently deserved the attention of the navigation community, although more studies are necessary to master this application.Inertial sensors Microelectromechanical systems (MEMS) combined with aiding information is an accurate and efficient integrated system for navigation purposes under Gaussian conditions [2] [3][4].In our case, we consider that we use only IMU mounted on foot in order to determine the position, velocity and attitude of the body.A solution based on Zero Velocity UPdaTes ZUPT and Zero UPdaTes ZARUT has been developed in the last five years, which provides an aiding information to correct inertial drifts and bias of gyroscopes and accelerometers and estimate person during walk.As a potential application, many attentions have been devoted to blind peoples navigation in urban environment and such development could be a potential solution for this complex problem.In this paper, the proposed solution implements an Information filter variant of CKF algorithm [1].

-ZUPT / ZARU Algorithms
In its most basic implementation, a shoe-mounted inertial navigation system estimates and corrects drift errors via an assisted zero-velocity updating (ZUPT) indirect Kalman filter (IKF).In this work, the zero velocity update (ZUPT) is directly correcting and observing velocity calculated and delivered by INS through non linear filters.The algorithm relies on detecting the foot stance phase in order to build a zero-velocity-based pseudo-measurement which is delivered to the EKF, CKF and Cubature Information Filter CIF.

A. Zero Velocity Updating (ZUPT)
The velocity measurement  is the measurement noise, modelled as a Gaussian random variable with zero-mean vector and covariance matrix V R .When zero velocity "V=0" is determined, this information is used to correct the non linear filter which uses the new innovation given by: Correcting velocity, it automatically leads to corrected coordinates after integration.

1.1.B. Zero Angular Updating (AUPT)
By analogy to ZUPT algorithm, other scientists proposed to use the information of zero angular velocity update in order to get accurate estimation of attitude angles yielding also to more accurate (1) velocity and position integration and estimation by the Kalman filter variants.The intuitive idea behind ZARUT is that during stance phase, foot attitude is considered constant.It is possible to write attitude measurement as given below: is the vector containing the true IMU's roll, pitch and yaw angles, with  n white Gaussian noise with Covariance matrix . In this paper, the attitude is represented by Euler angles because of the bounded nature of motion, it is not needed to use to the quaternion model [2][5].

INS/ZUPT/ZARU Integrated Navigation System
Several architectures exist and were investigated in the literature [4][5]9].Different filtering approaches with multiple state vector configurations have been widely simulated and experimented.We comment the most common. i.
The magnitude of the acceleration ii.
The magnitude of the gyroscope, iii.The local acceleration variance must above a given threshold: Where b k a is local mean acceleration value and s defines the averaging window (s=20 samples)., computed by the following expressions: If these three "i,ii,iii" conditions are respected, it is then decided to select sensors outputs at time k as a ZUPT and ZARUT measurements.

Proposal of Direct Filtering approach:
In direct Kalman filter formulation, direct position, velocity and attitude angles are estimated.As it is known to be more computational demanding that indirect filtering, this approach has not been extensively explored in the past due to the CPU limitations previously explained.It is now possible to propose such direct design as an alternative of high dimensional error state vector usually proposed in the indirect filtering approach.In this method, different non linear filters are applied, most of them have been described in [10][12][18], except for CKF which is described in the next section.In this case, the state vector is computed in discrete time as given below, estimating directly the navigation states [3][5][10]: H is defined as the dual observation ZUPT and ZARUT.In this case, we assume to have an additional direct observation of yaw angle from electronic compass, with: This information is directly used to correct the yaw output of IMU.Real Output Data from IMU ADIS 16438: For direct filtering design, H is the identity matrix which means linear measurement.For tightly coupled design, H is the distance (or pseudo-distance) from beacons aiding navigation to the receiver (RFID or WiFi) mounted on foot with IMU, and in this case, measurements are strongly non linear [4].

Experimental Section
An application of the shoe-mounted inertial measurement unit IMU was tested [9].The system hardware is composed by an: -IMU Analog Device 16367, equipped with accelerometers, gyroscopes and magnetometers that can be sampled at -250Hz frequency -Stance-Still step detector and an assisted AUPT-ZUPT "Direct Sigma Point Kalman filter" which we propose here.
Experimental results are depicted in Fig. 3 and Fig. 4.These figures depict a superposition of the IMU's outputs norm against the step detector output, as we can see the stance phases, characterized by almost no variation of the norm, are clearly identified.
Fig4.MEMS IMU accelerometers output (view of accelerations and zoomed view of angular rates x,y,z) This detection is based on multiple samples outputs.In our case we used 20 samples to determine if the system is in stationary phase or not such as described in [9].In Fig. 5 it is possible to observe during walking pitch and yaw angles in the zoomed figures.The error of standalone inertial system is clear and visible comparing with the EKF, CKF/CIF estimation that are following the true trajectory.In Fig. 6 it is possible to observe that comparing with measurements, the most efficient and centred filter is CIF, somewhat analogue to CKF but after duration of time, after 56 second, it is possible to distinguish that CKF and EKF suffer slightly from bias and sensors errors at the opposite of CIF which provides the best estimate.

Conclusions
After different simulations and analysis of results based on real data, the general conclusion is that MEMS ADIS IMU 16367 based on CKF is a good candidate for pedestrian navigation applications.
CKF does not need the computation of any Jacobian or Hessian matrix.During different analysis, it has been observed that it is necessary to estimate accurately steps "stance" and "still" phases during walk in order to correct MEMS IMU from drifts and bias of gyroscopes and accelerometers.In parallel, in order to prevent initialization impact on the accuracy of coordinate estimation, CIF has been an excellent choice to estimate navigation states and presented more robustness against errors and bias of inertial sensors during long period of navigation [6][10].

Fig. 1 .
Fig.1.Strapdown Inertial Measurement Unit IMU / Mounted on foots (pictures from www.openshoe.org)1.1 -INERTIAL SYSTEM MEASUREMENT Due to drifts, bias, scale factors and noises of inertial sensors, ZUPT and ZARUT are used to aid navigation with bounded error of position, velocity and attitude estimation.In this work, we suppose both techniques of velocity update and angular rate update are applied with the use of three stationary conditions at ones.
at time k With the assumption that noises are non Gaussian in the process with: 1 k Q : Covariance matrix of k l ( sensors noises), 2 k Q : Covariance matrix of k w (system noise)

Fig3.
Fig3.MEMS IMU gyroscopes output ( original view and zoomed view of 03 angular rates x,y,z)