Dual MIMU Pedestrian Navigation by Inequality Constraint Kalman Filtering.

The foot-mounted inertial navigation system is an important method of pedestrian navigation as it, in principle, does not rely any external assistance. A real-time range decomposition constraint method is proposed in this paper to combine the information of dual foot-mounted inertial navigation systems. It is well known that low-cost inertial pedestrian navigation aided with both ZUPT (zero velocity update) and the range decomposition constraint performs better than those in their own respective methods. This paper recommends that the separation distance between the position estimates of the two foot-mounted inertial navigation systems be restricted by an ellipsoidal constraint that relates to the maximum step length and the leg height. The performance of the proposed method is studied by utilizing experimental data, and the results indicate that the method can effectively correct the dual navigation systems' position over the traditional spherical constraint.

1. Introduction

Positioning and tracking systems have developed over several decades in various applications, ranging from tracking of pedestrians to autonomous vehicles [1]. A high-precision navigation system is often needed for urban and indoor lives where GPS is unavailable, for example, in wearable body area networks [2]. Increasing attention for the pedestrian navigation problem can be partially attributed to the significant progress in affordable wearable computing platforms and enhancement in sensor quality, especially with respect to micro electro mechanical systems (MEMS) [3]. The micro inertial measurement units (MIMU) usually consist of gyroscopes, accelerometers, magnetometers, and pressure sensors [4].

In real-life application, however, the low-cost inertial navigation systems suffer from the accumulation of errors while calculating the traveled distance of the objects. These errors cause the trajectories to drift away from the actual path as time grows. An effective technique is to bind the error growth using ZUPT [5]. In [6], the authors presented an open-source, real-time, embedded implementation of a foot-mounted, zero-velocity-update-aided inertial navigation system (INS).

When we use the ZUPT-aided INS to track pedestrian location, the heading drift of navigation information is unobservable [7], so it is difficult to get accurate location information only through ZUPT in long-term navigation. Some previous works used the information fusion of multiple inertial sensors to ulteriorly correct the position [8,9]. As shown in [8], the use of the two-feet range constraints can significantly improve the navigation performance and a 110 m straight-line experiment showed that the spherical constraint algorithm can reduce the mean error and covariance of the final position estimates.

In [9], the authors proposed a method for a dual-mounted INS to reduce the systematic heading drift. They set up a dual MIMU system with two feet, in which the maximum distance between the two systems is derived from the calibration procedure. The proposed algorithm can obtain the walk trajectory when the initial heading estimates are known, but the method did not significantly improve the positioning accuracy of the system.

The experiments in [8,9] are two-dimensional (2-D) plane experiments, and the feasibility of the algorithm in three-dimensional space was not verified. When we use the spherical constraint [8] based on maximum step size to correct the position of the two feet, if the step size is greater than the height of heels, the spherical constraint algorithm would have little significant effect on the height constraint of the two feet. Therefore, in this paper we take into account different separation constraints in level and height directions, which is shown to be more effective to correct the pedestrian location.

2. Principle and Theory

2.1. Discrete Kalman Filter

The Kalman filter addresses the general problem of estimating the state of a discrete-time process that is governed by the linear stochastic difference equation [10,11]: with a measurement that is where denotes transition matrix relating the state at the previous time step to the state at the current step , denotes the system noise drive matrix, represents a measurement matrix, is series of measurement noise, and represents noise excitation sequence for the system. The and simultaneously meet:

In practice, the process noise covariance matrices and measurement noise covariance matrices might change with each time step or measurement, and we assume they are a positive definite matrix:

Equations (4)–(7) are the basic equations of Kalman filtering. If the initial values about and are given, we can perform the state estimation at time according to measurements at the same moment.

2.2. Inequality Kalman Filter

The inequality Kalman filter appears in solving practical problems between state variables where there are inequality relationships [12]. The inequality relationship can be expressed as a constraint equation and combined with the Kalman filter, as a result of which the optimal solution strictly conforms to the inequality constraints between state variables, and a better result could be obtained.

The inequality-constrained Kalman optimal solution [13] is expressed as where is the unconstrained (standard) Kalman filter estimate and is a symmetric positive definite weighting matrix [14,15]. As such, the weighted error of the constrained filter is minimized [16]. Expanding the first type of Equation (8):

Thus, the inequality constrained problem can be further simplified as

3. Methods

3.1. Generalized Likelihood Ratio Test (GLRT)

The output of MIMU can be expressed as where the specific force measurement vector and the angular rate measurements vector . Assuming a series of measured value . We employ a double hypothesis testing as such, MIMU stationary, MIMU moving. The false alarm probability is expressed as

The detection probability is . Two hypotheses' observation data probability density functions are, respectively, defined as and .

The mathematical sensor model can be expressed as , where and , the force of MIMU is , and MIMU angular rate is expressed as . The symbol denotes the vector of unknown elements accelerometers noise, gyroscopes noise. Assume the noises follows zero mean Gaussian distribution, with noise covariance matrix , where and , respectively, represent accelerometers and gyroscopes noise variance.

Since the sensor measurement can be obtained from the joint probability density as where:

GLRT is determined by the hypothesis if where denotes the threshold. In Equation (12), and represent the maximum likelihood estimate of the unknown element under the assumptions and , respectively. Equation (12) can be simplified as means that the pedestrian is in a stationary state.

In practice, ZUPT can effectively aid inertial navigation system to remove long-time accumulated errors [5,17]. The velocity error of carrier is used as a concept [18,19]. When pedestrians stay static, the MIMU measured velocity is regarded as an error to correct the system using Kalman filtering.

The state error vector is defined as which, respectively, represents the three-dimensional attitude error, gyro drift, position error, velocity error and accelerometer bias.

The zero-velocity correction Kalman filter model is

When the MIMU is stationary, the speed is zero, in theory; thus, the ZUPT speed measurement equation is where the state transition matrix is given as where is the specific force anti-symmetric matrix, and .

3.2. The Ellipsoidal Constraint Method

Each foot are fixedly mounted by a MIMU. For regular human kinematics, the separation distance between the right and left feet cannot be larger than a quantity known as foot-to-foot maximum separation [8,9]. The maximum step size is a typical feature of pedestrian to walk and can be used to constrain the navigation error [20,21], namely, in addition to using ZUPT to improve the accuracy of pedestrian navigation. In specific, we decompose the constraint into three degrees of freedom and then use the obtained sub-constraints to correct the navigation system. Based on this intuition, we constrain the position estimate of right and left foot-mounted ZUPT-aided INSs.

According to the coordinate system identified of the MTI-G-700 units (3D motion tracking system, from Xsens Technologies B.V., Enschede, The Netherlands), the carrier coordinate system, as shown in Figure 1, shows the X axis is parallel to the surface of the MIMU, in the forward direction, and the Z axis is perpendicular to the MIMU surface, in the upward direction. In this dual-MIMU integrated navigation system, the navigation coordinate system’s X axis is forward, the Y axis points to the right, and the Z axis perpendicular to the X plane, upwards. The coordinates of the navigation subsystem bound to the feet are defined in the same way.

Figure 1

(Left) The two MIMU are mounted to the feet separately, the OX coordinate system are the carrier coordinates; (right) side view, the OX coordinate system are the navigation coordinates.

For two MIMU navigation systems, the , system real state is described as (including position, velocity, and attitude), the estimated state as at the time , where .

The joint state vector is defined as where .

Letting the maximum step size of the pedestrian be given by , the real displacement difference between the two navigation systems should be less than or equal to . As the leg height is subject to certain constraints, during the pedestrian normal walking state, the positions of the right and left foot can be approximately constrained in a ellipsoid (Figure 2). The position of one foot is constrained within the circle of radius in the XOY plane, and is confined within the circle with leg-related radius in XOZ and YOZ planes, both centered at the other foot (Figure 3).

Figure 2

The ellipsoid constraint calibration diagram ().

Figure 3

Level constraint for one foot: a circle of radius , centered at the other foot.

Assuming that left foot is on the ground and the right foot is in movement at moment k (Figure 3), then we can calculate , defined as the angle between the position of the two feet in the XOY plane (in navigation coordinate system):

As we can see from Figure 2, there is a space azimuth between the right and the static left foot, we can calculate this angle by the positional relationship between the feet:

Therefore, the ellipsoidal constraint correction algorithm between the feet can be defined as

Defining the matricies, where represents the real-time constraint value of the ellipsoid constraint on the X axis, so ; represents the real-time constraint value of the ellipsoid constraint on the Y axis, so ; represents the real-time constraint value of the ellipsoid constraint on the Z axis, so .

We assume that two navigation systems attitude is accurate in the current moment when the decompose step size constraint. When can constraint the state to be satisfied with the state modification is recommended as where denotes the Kalman filter estimated covariance matrix state.

Defining , Equation (22) can be written as

The covariance matrix of the process measurement noise of the dual-MIMU integrated navigation system is where , , , and .

The covariance matrix of the measurement noise of the dual-MIMU integrated navigation system is where .

The sampling rate of the filter is 400 Hz.

4. Experiment

To compare the performance of the proposed algorithm with the existing in [8] (the spherical constraint method). Experiments are carried out using two MTI-G-700 units and the performance parameters of them are shown in Table 1. The procedure is summarized as follows:

Table 1

The performance parameters of MTI-G-700.

Sensors	Accelerometer		Gyroscope
	Typ	Max	Typ	Max
Standard full range	50 m/s^2	-	450°/s	-
Bias repeatability (1 year)	0.03 m/s^2	0.05 m/s^2	0.2°/s	0.5°/s
In-run bias stability	40 μg	-	10°/h	-
Noise density	80 μg/hz	150 μg/hz	0.01°/s/hz	0.015°/s/hz
Non-linearity	0.03% FS	5% FS	0.01% FS	-

In a complex 2D environment: some closed trajectory containing a straight line path and turning eight times (turning angle: 90°).

In a complex 3D environment: a six-story staircase, and parts of corridors in the Sheng-Hua building at the Central South University. The walk strats at the first floor and ends at the sixth floor.

According to the experimenter gait characteristics, we set m and m, respectively.

In the 2-D closed experiment, the ZUPT-aided INS can track the pedestrian feet positions (Figure 4), but the distance of two feet reaches about 6 m, which is unreasonable in pedestrian navigation. Comparing the three different sets of trajectories, the position estimation information under the ellipsoidal restriction can obtain the position estimates more accurately. Figure 5 shows the relative positions of the two feet in the axis direction. Since constrains the altitude difference, the feet height difference can be reduced near to the true value and the pedestrian location can be more accurate.

Figure 4

Pedestrian walking a closed path in a corridor outside the laboratory. (a) feet trajectories without constraint; (b) feet trajectories with spherical constraint; (c) feet trajectories with ellipsoidal constraint.

Figure 5

Left and right feet altitude difference in 2D the closed path experiment. (a) the height difference of the two feet without contraint; (b) the height difference of the two feet with spherical constraint; (c) the height difference of the two feet with ellipsoidal constraint.

For estimation evaluation, we have chosen the root mean square error (RMSE) as an accuracy measure in this work. This is used to measure the difference between the actual values and the output of an estimator. For quantitative comparison, we only checked the starting and final positions, both in 2D and 3D, in all walking tests. We assume the starting position of the left foot and right foot as (0, 0.1, 0) and (0, –0.1, 0), hence, only the final estimates are inserted into the RMSE formula. Both of these quality indicators are given in comparison Table 2. Note that the unconstrained method represented the results of only ZUPT corrections. Both spherical and ellipsoidal constraint methods can reduce the 2D and 3D trajectories error, but the latter obviously reduces the proportion more, and the correction effect is more obvious.

Table 2

RMSE deduction by using the ellipsoidal constraint method for a closed path test.

Method	(L/R) 2D RMSE (m)	(L/R) 3D RMSE (m)	Remarks
Unconstraint	1.2640/0.9493	1.5473/1.2293	Time: 73 s Distance: 61.6 m Error rate(%): 0.93
Spherical constraint	0.6533/0.6194	0.8732/0.8482
Ellipsoidal constraint	0.5709/0.5953	0.6977/0.7174

In the 3D upstairs experiment, the result shows the maximum step ellipsoidal constraint method reduces the error accumulation in the -axis direction effectively. From the results presented in Figure 6 and Figure 7, we can observe that the proposed method in this paper can reduce the altitude difference of the feet position from 2.6 m to 0.56 m. Comparing to the spherical constraint method, the relative positions concentrate between –0.3 to 0.3 m which are obviously smaller than the spherical confinement results and are more suitable to the characteristics of the feet height on the stairs. By analyzing the results obtained in Figure 6 and Figure 7, we observe that the proposed algorithm can track the feet trajectory more accurately than the spherical constraint method.

Figure 6

The trajectories of the feet in upstairs experiment. (a) feet trajectories without constraint; (b) feet trajectories with spherical constraint; (c) feet trajectories with ellipsoidal constraint.

Figure 7

The left and right feet position altitude difference in the upstairs experiment. (a) the height difference of the two feet without constraint; (b) the height difference of the two feet with spherical constraint; (c) the height difference of the two feet with ellipsoidal constraint.

In the indoor upstairs test, because of the objective factors of the irregular staircase, we are unable to accurately know the horizontal coordinate of the end point. However, the position in the Z axis can be accurately measured, so, in this experiment, we only analyze the root mean square error in the Z axis direction. The numerical results are given in Table 3.

Table 3

RMSE deduction by using ellipsoidal constraint method for indoor upstairs test.

Method	Left-3D RMSE (m)	Right-3D RMSE (m)	Remarks
Unconstraint	1.6712	0.8249	Time: 157 s Height: 31.75 m Error rate(%): 1.71
Spherical constraint	0.7676	0.7874
Ellipsoidal constraint	0.6537	0.5414

5. Results

Low-cost inertial pedestrian navigation aided with both ZUPT and the range decomposition constraint performs better than those in their own respective method. In this paper, we decompose the maximum step length along the navigation coordinate axes in real time, and establish an ellipsoidal constraint more suitable for actual walking situations. Each sub-constraint changes along with different times and makes the aiding scheme of the step size more specific and accurate. Experimental tests on different paths show that the proposed ellipsoidal constraint method can effectively improve the position accuracy of pedestrian navigation.