Improved Pedestrian Positioning with Inertial Sensor Based on Adaptive Gradient Descent and Double-Constrained Extended Kalman Filter

The Foot-mounted Inertial Pedestrian-Positioning System (FIPPS) based on the Micro-Inertial Measurement Unit (MIMU) is a good choice for the forest fire fighters when the Global Navigation Satellite System is unavailable. Zero Velocity Update (ZUPT) provides a solution for reducing cumulative positioning errors caused by the integral calculation of the inertial navigation. However, the performance of ZUPT is highly affected by the low accuracy and high noise of the MIMU. The accuracy of conventional ZUPT for attitude alignment is reduced by the zero offset of acceleration and the drift of a gyroscope during the standing phase. An initial alignment algorithm based on Adaptive Gradient Descent Algorithm (AGDA) is proposed. In the stepping phase, the extended Kalman filter (EKF) is often used to correct attitude and position in track estimation. However, the measurement noise of the EKF is influenced by the high-frequency acceleration and angular velocity. Thus, the accuracy of the attitude and position will decrease. A double-constrained extended Kalman filtering (DEKF) is proposed. An adaptive parameter positively correlated with the acceleration and angular velocity is set, and the measurement noise in the DEKF is adaptively adjusted. The performance of the proposed method is verified by implementing the pedestrian test trajectory using MPU-9150 MIMU manufactured by InvenSense. The results show that the attitude error of the AGDA is 33.82% less than that of the conventional GDA. The attitude error of DEKF is 21.70% less than that of the conventional EKF. The experimental results verify the effectiveness and applicability of the proposed method.


Introduction
High-precision pedestrian navigation systems usually include GPS and autonomous navigation. ese systems can generate real-time data for a pedestrian's attitude and position that can be widely used in re protection, patrol, and military elds [1,2]. Positioning accuracy is a key in practical applications, especially in environments without GPS signals. However, GPS signals are unavailable in places with dense buildings and forested areas. An inertial navigation system uses the IMU to calculate attitude, velocity, and position and can realize pedestrian positioning without GPS [3,4]. e cumulative error characteristics of inertial navigation integration greatly impact the positioning accuracy of a pedestrian in environments lacking GPS. erefore, it is very important to improve the pedestrian attitude and positioning accuracy without GPS signals [5].
In recent years, the forest re ghter positioning system based on Micro-Inertial Measurement Unit (MIMU) has recently attracted widespread attention with the rapid development of Micro-Electro-Mechanical System (MEMS). e foot-mounted inertial/magnetic sensor unit consists of a gyroscope, an accelerometer, and a magnetometer, which can provide the angular velocity, acceleration, and magnetic eld intensity, respectively. en, according to the dead reckoning algorithm, the data measured can be transformed into attitude and position information [6,7]. However, the drift and integration errors of low-cost sensors greatly impact the nal positioning results [8,9]. e positioning accuracy in the pedestrian navigation and positioning system is greatly a ected by the attitude angle. As a traditional pedestrian location method, the angular velocity and acceleration integration method is used to obtain the attitude and position. However, this method encounters the problem of error accumulation, that is, the position and attitude have a large offset over a period of time. At present, the Zero Velocity Update (ZUPT) method is widely used in pedestrian positioning [10][11][12][13].
is method divides the human gait into two phases: standing and stepping [14]. e core of this method is to suppress the attitude and position errors in the standing phase. An extended Kalman filter (EKF) assists the pedestrian location of ZUPT to compensate for the errors caused by the integral characteristics of acceleration and angular velocity through observation [15][16][17]. However, the accuracy of this method is affected by the dimension of the state variables and cannot accurately estimate the attitude and position. Initial alignment is a common method to correct the attitude angle [18][19][20]. However, the zero bias of the accelerometer seriously affects the accuracy of the attitude angle. e gradient descent algorithm (GDA) based on initial alignment is a method of attitude calculation that uses gradients to find the optimal quaternion solution [21][22][23][24].
is method obtains the minimum sensor measurement errors by iterating in the gradient direction to improve the attitude accuracy. In [24][25][26], the quaternion derivative represented by the gradient descent and the quaternion calculated by the gyroscope are combined to reduce the quaternion error. However, the original GDA method uses a fixed step size to narrow the gap between the measured value and the real value, which causes two problems. First, when the step size is large, it is easy to miss the minimum value and the minimum error cannot be obtained. Second, when the step size is small, it is easy to fall into local optimal solutions, that is, to obtain a minimum point that is not minimum value. To overcome this problem, a gradient descent algorithm based on the adaptive step size is proposed that combines the acceleration and angular velocity measurements in the step size. e step size and attitude angle are stable when the error disappears.
Although the cumulative errors of the attitude and position are suppressed and eliminated in the stationary phase, attitude deviation is caused by the high noise of the sensor in the pedestrian movement phase [27]. e original attitude updating algorithm usually uses the quaternion updating method.
is method updates the attitude by using the real-time changes of the gyroscope. However, the noise of sensors, especially the accelerometer and gyroscope, causes the measured value to greatly deviate from the true value, which greatly influences on the attitude accuracy. e approaches to improve the attitude correction accuracy by multisource information fusion are proposed. Min et al. [28] proposed to combine the IMU signal with a human dynamics model to improve the positioning accuracy of the pedestrian's step. However, different people have different accuracies and unstable positioning performance under the same dynamic model. Linlin et al. and Zhang et al. [29,30] proposed the fusion of IMU information and GPS information to achieve mutual error correction, which can improve the attitude and positioning accuracy. However, GPS signal is weak in areas with dense occlusions. Skog et al. and Bose et al. [31,32] proposed to fuse multiple types of IMU information together to improve the positioning accuracy, but it is not convenient to wear multiple IMU devices.
ere are no complex constraints for a single IMU system, and the data fusion method is simple. In [33], the motion characteristics of the two feet are measured by the IMU at the same time.
e distance between the two feet is regarded as the constraint condition of the Kalman Filter, and the position accuracy is higher when this method is combined with the zero speed update. Skog [34,35] uses the relative positions of the two feet minus the height of the pedestrian as the constraint condition to establish a Kalman filter model, but the constraint condition is a fixed value, which is not accurate. e integration of accelerometers and gyroscopes will produce a large drift over a period of time, so the Kalman filter is applied to pedestrian positioning to reduce the positioning error. e Kalman filter can realize attitude updating by fusing two different quaternion updating methods. In [36], the prior error covariance is used as a variable parameter for Kalman filtering to ensure simplicity and accuracy. In [37], the errors in the data collected by polymer optical fiber sensors and IMU sensors are fused through the Kalman filter to improve the attitude angle accuracy. In [38], Zhang et al. use the estimated velocity of the carrier phase as the observed measurement for the Kalman filter, but the kinematic solution was not sufficiently smooth. e Kalman filter does not work well on nonlinear systems and may even cause filter divergence. erefore, the EKF method is proposed to be applied to systems with nonlinearity. In [39], the fuzzy adaptive EKF based on fuzzy logic inference was introduced based on the MIMU for pedestrians to correct the horizontal positioning error. In [40], a two-stage extended Kalman filter based on an accelerometer and a magnetometer is designed to solve magnetic field fluctuations. In [41], the extended Kalman filter is used to calculate the knee angle from an MAPR system. In [42], the heading is estimated by the EKF, Heuristic Heading Reduction, Flat-ground Hypothesis, and Cardinal Heading-Aided Inertial Navigation techniques. In [16], an optimal-enhanced Kalman filter is proposed, in which adaptive parameters are added to the covariance matrix to achieve accurate positioning of pedestrians within an enclosed environment. In [8,15,43,44], the EKF and zero velocity update are combined to eliminate heading angle drift, which is better than the zero velocity update only. In [45], Wang et al. reconstruct the prior error covariance by mining the posterior sequence online to overcome the inaccurate calculation of the Kalman covariance. In [46], Zampella et al. used the EKF algorithm to obtain more accurate heading information based on the PDR algorithm. However, the measurement error is increased by the noise disturbance of the gyroscope and accelerometer in the external environment. us, the noise estimation is the main factor affecting the filtering accuracy of the EKF. e traditional EKF estimation of noise is not accurate because this estimation is fixed. erefore, the noise of the accelerometer and gyroscope in the dualconstrained extended Kalman filter (DEKF) method proposed in this paper is considered in the measurement noise, 2 Complexity that is, adding adaptive parameters can be used to suppress the high-frequency noise in the measurement noise, which helps prevent filter divergence and ensures the accuracy of attitude and position. e rest of the paper is organized as follows: the basic principles of the pedestrian Inertial Navigation System are introduced in Section 2, including the ZUPTmethod and the pedestrian positioning algorithm. Section 3 introduces the AGDA at zero velocity, which is the first contribution of this article. In Section 4, a combination of updating the attitude angle by the AGDA and updating the attitude angle by the quaternion method is used as the state variables for the EKF in the stepping phase and the DEKF is used for the attitude and position corrections, which is the second contribution of this paper. In Section 5, the experimental algorithm proposed in this study is experimentally verified using the MPU9150 sensor.

Principle of ZUPT Algorithm.
e ZUPT algorithm performs well in suppressing the cumulative errors of sensors in the standing phase. According to the motion characteristics of pedestrian's foot, the detection thresholds of the zero velocity moment are set according to the acceleration modulus, the angular velocity modulus, and the acceleration variance. When the three parameter values are less than the corresponding thresholds at the same time, the pedestrian's foot is considered to be at zero velocity moment. After detecting the zero velocity moment, the initial alignment is performed to obtain the initial attitude angle, as in the following equation: where a x , a y , and a z are the axial accelerations measured in the x, y, and z directions, respectively, by the accelerometer; ‖a‖ � ���������� a 2 x + a 2 y + a 2 z , m x , m y , and m z are the axial magnetic field strengths measured in the x, y, and z directions, respectively, by the magnetometer, φ x is the roll angle, φ y is the pitch angle, and φ z is the heading angle. e initial attitude angle is updated by the quaternion method to obtain an updated attitude angle after initial alignment. e quaternions are represented in the following form: where q 0 , q 1 , q 2 , and q 3 are quaternions and i, j, and k are fundamental quaternion units. e quaternion differential equations related to the angular velocity of the carrier are established: e angular velocities of x, y, and z axes are measured by the gyroscope in the sensor frame (short for b frame) as ω x , ω y , and ω z , respectively. According to the attitude angle at time k − 1, the attitude angle at time k can be obtained by equation (3). In the stance phase, the acceleration modulus is theoretically the same as that of gravity and the angular velocity modulus is theoretically zero. However, the output of the sensor is not equal to the theoretical value. e measurements of the acceleration and angular velocity produce cumulative errors over a period of time. In the third part, the AGDA is proposed to correct the attitude angle affected by the cumulative error.

Pedestrian Positioning Algorithm.
e foot movements of a pedestrian have periodic characteristics. If the IMU is fixed on the pedestrian's foot, the pedestrian's speed and position can be calculated according to the dead reckoning algorithm. e basic formula of the dead reckoning algorithm is as follows: where C n b is the coordinate transformation matrix from the b frame to n frame, V n k and V n k− 1 are the velocities along the n frame at times k and k − 1, respectively, f b k is the projection of the acceleration on the b frame at time k, g n � 0 0 − g T is gravity, and T is sample time. e inertial pedestrian navigation system model is shown in Figure 1, which shows the whole process from the data acquisition to the output of attitude, velocity, and position. e velocity and position can be obtained by equations (5) and (6), respectively. In the stepping phase, the EKF can reduce the error in the velocity and position, but the influence of acceleration and angular velocity noise on the measurement noise cannot be ignored. To solve this problem, the errors of the quaternion obtained by the differential equation and quaternion obtained by AGDA are selected as the state variables of the improved EKF. In the fourth part, the measurement noise is estimated according to the acceleration modulus and angular velocity modulus. e attitude and position are corrected using the DEKF.

Adaptive Gradient Descent Algorithm
e initial attitude angle can be obtained by the initial alignment algorithm, but the noise of the gyroscope Complexity gradually makes the attitude deviate from the theoretical value, so the attitude error cannot be restrained in the standing phase when only the gyroscope is used. Gradient descent algorithm (GDA) is a new directional lter for obtaining high-precision attitude information. e purpose of this lter is to reduce the quaternion error through acceleration and magnetic eld intensity. When the error of the acceleration and magnetic eld intensity reaches the minimum, the quaternion is robust and the attitude error is eliminated. erefore, e GDA algorithm can be used as attitude calculation in the standing phase. However, the accelerometer bias and gyroscope drift seriously a ect the step size of gradient, so the attitude correction accuracy of GDA decreases.
To solve this problem, an adaptive gradient descent algorithm (AGDA) is proposed to correct the attitude of FIPPS in the standing phase. When the angular velocity varies greatly, the component of gradient descent compensation increases accordingly in the standing phase. us, the step size is positively correlated with the angular velocity. e rotation matrix error of acceleration characterization is the error between the measurement vector of the accelerometer and the projection of gravity acceleration vector in frame b, as shown in equation (7). e rotation matrix error of magnetic eld intensity characterization is the error between the measurement vector of the magnetometer and the projection of local magnetic eld intensity vector in frame b, as shown in equation (8): F m where F a is the rotation matrix error of acceleration characterization and F m is the rotation matrix error of the magnetic eld intensity characterization. e gradient value of the acceleration error function and the gradient value of the error function of the magnetic eld intensity can be written as follows: where J a is the Jacobian matrix of the acceleration and J m is the Jacobian matrix of the magnetic eld intensity. J a and J m are expressed by equations (10) and (11), respectively: e rotation matrix error can be eliminated by iteration from the initial attitude along the opposite direction of the gradient to the minimum point of the error function. e gradient direction can be attained by normalizing the gradient.
e gradient direction of acceleration error function and the gradient direction of the error function of magnetic eld intensity can be obtained by the following equations:

Complexity
where ∇F • and ∇h • are the gradient directions of acceleration error function and the gradient direction of the error function of magnetic field intensity, respectively, and ‖∇F‖ and ‖∇h‖ are the moduli of ∇F and ∇h, respectively.
From the above equations, the quaternion at time k can be computed by the quaternion at time k − 1: where Q k and Q k− 1 are quaternions at times k and k − 1, respectively, μ is the step size of the AGDA, and α 1 is the adaptive factor. When the rotation matrix errors of the acceleration and magnetic field intensity characterization are larger, the distrust degree of the algorithm on the measured values of the accelerometer and magnetometer is deeper and the part of compensating gyroscope deviation by the accelerometer and magnetometer should be reduced accordingly. α 1 is set in the algorithm. α 1 is positively correlated with the angular velocity modulus of the gyroscope and negatively correlated with the rotation matrix errors of acceleration and the magnetic field intensity. e adaptive factor can be expressed as where ‖ω‖ is the angular velocity module, ‖F a ‖ and ‖F m ‖ are moduli of F a and F m , respectively, c 1 is a positive correlation parameter, and ε 1 and β 1 are negatively correlated parameters. is algorithm realizes the adaptive adjustment of the step size and reduces the influence of accelerometer bias, magnetometer disturbance, and gyroscope drift on the attitude angle correction. erefore, the AGDA improves the accuracy of attitude angle in the standing phase.

Pedestrian Attitude and Position Updating Based on Double-Constrained Kalman Filter
e EKF can reduce the errors of attitude and position, but changes in acceleration and angular velocity in the step phase seriously affect the changes in the noise interference. In this section, we first introduce the AGDA affected by high-frequency acceleration and angular velocity in the step phase. en, the filtering accuracy of the traditional EKF is affected by acceleration and angular velocity noise, so the above two kinds of noise are considered in DEKF.

Adaptive Gradient Descent Algorithm Based on the Stepping Phase.
e acceleration and angular velocity of pedestrians change constantly during the stepping phase. According to the AGDA in the standing phase, the quaternion at k time can be obtained as follows: where α 2 is the adaptive factor. When the angular velocity increases, the gradient descent compensation component increases, so the step size is positively correlated with the angular velocity. When the rotation matrix error of magnetic field intensity characterization is larger, the distrust degree of the algorithm on the measured values of the magnetometer is deeper, and the part compensating gyroscope deviation by the magnetometer should be reduced. us, the step size is negatively correlated with the rotation matrix errors of magnetic field intensity. When the measurement value of the accelerometer is larger, the high-frequency interference of the accelerometer is stronger and the deeper is the distrust of the algorithm on the measurement value of the accelerometer. e step size is negatively correlated with the measurement value of the accelerometer. α 2 is set to adjust the step size adaptively as follows: where c 2 is a positive correlation parameter and ε 2 and β 2 are the negative correlation parameters. is algorithm uses the adaptive adjustment of the step size to reduce the influence of magnetometer disturbance, gyroscope drift, and highfrequency acceleration disturbance. erefore, the AGDA improves the accuracy of attitude and position correction in the stepping phase.

Double-Constrained Kalman Filtering Algorithm.
In pedestrian navigation and positioning system, the EKF can effectively reduce the errors of attitude and position. However, the noise in acceleration and angular velocity increases sharply during the moving state, Figures 2 and 3 are, respectively, the waveforms of acceleration and angular velocity of the foot from the standing phase to the stepping phase in a walking cycle. e noise interference in the acceleration and angular velocity reduces the precision of basic EKF results. To improve the positioning accuracy, the attitude angle corrected by the AGDA and the attitude angle corrected by the quaternion method are fused by the improved EKF and the noise interference of acceleration and angular velocity is constrained adaptively by DEKF. e state equation and observation equation of the navigation system model are shown in the following equations: where X − k and X − k− 1 are state vectors at time k and k − 1, respectively, the state vector can be expressed by equation (20), the observation vector Z consists of quaternions updated by AGDA, W k and V k are the process noise matrix and observation noise matrix, respectively, the measurement matrix H is shown in equation (21), and the transfer matrix of coordinate system F can be computed by equation (22): where ξ x , ξ y , and ξ z are gyroscope drift of x, y, and z axes, respectively, and F ω and F b can be written by equations (23) and (24), respectively: where λ x , λ y , and λ z are the gyroscope drift coe cients of the x, y, and z axes, respectively. e measurement noise matrix in the basic EKF is a constant value, but in practice, the acceleration and angular velocity in the step phase have high-frequency interference, and the modulus of acceleration and angular velocity are positively correlated with the noise disturbance. erefore, the DEKF is proposed in this study at the moment of motion. e core formula is as follows: where K k is the Kalman gain, τ is an adaptive parameter, c 3 is the noise perturbation parameter on the stepping phase, the moduli of acceleration and angular velocity are positively correlated with the observed noise R, P k and P − k are error covariance matrices and prediction error covariance matrices, respectively, and P k , P − k , and X k are shown in the following formulas: In the DEKF, τ is used to estimate the high-frequency interference caused by the change of acceleration and angular velocity in the stepping phase. e measurement noise of the DEKF is constrained by adaptive adjustment of parameter τ. erefore, the attitude and position errors of the pedestrian positioning system are reduced.

Experimental Study on Pedestrian
Positioning Algorithms

Structure of Improved Pedestrian Attitude and Position
Algorithms. Figure 4 introduces the ow chart from the original input data to the output data. e attitude correction is carried out in two phases by AGDA and DEKF methods. e thin line frame is the basic step of pedestrian positioning, the dotted line frame is the standing phase which incorporates the AGDA, and the thick line frame is the stepping phase which incorporates the DEKF.

Experimental Results.
According to the experiment, the validity of AGDA and DEKF is veri ed. MPU-9150 is used as the acquisition system to measure the static and motion parameters of pedestrian feet. e parameters of MPU-9150 sensor are shown in Table 1.
e experimental devices are utilized to collect the output and calculate the experimental results of the pedestrian, which are shown in Figure 5. e red line is the reference trajectory (16 m × 16 m) in test 2. The sample frequency of sensors is 30 Hz. e parameter selection of the algorithm proposed in the experiment is shown in Table 2.
To verify that the attitude accuracy is improved by using the AGDA in the standing phase, an experiment is carried out in a step cycle. In the pedestrian position experiment, the attitude angle is the main parameter used to evaluate the  Full-scale range (μT) ±1200 Interface CPU MIMU Figure 5: Hardware system and experimental area. respectively. In addition, the legends for the curves in gures are summarized as follows: GDA: the traditional gradient descent algorithm discussed in Section 3 AGDA: the adaptive gradient descent algorithm proposed in Section 3 In the attitude angle comparison results, the blue line is the result of attitude correction using basic GDA in the standing phase and the red line is the result of attitude correction using AGDA in the standing phase. As seen from the above gure, the attitude angle error can be reduced and stabilized by using the AGDA. To obtain better experimental veri cation, four di erent groups of people were tested and the experimental results of four groups were calculated. e average errors of the pitch angle, roll angle, and heading angle are shown in Table 3. Compared with the basic method, the average error of the pitch angle obtained by AGDA algorithm is reduced by 69.06%, 12.06%, 18.26%, and 24.80%, the average error of the roll angle obtained by AGDA algorithm is reduced by 20.76%, 66.03%, 24.92%, and 24.83%, and the average error of the heading angle obtained by AGDA algorithm is reduced by 19.70%, 35.25%, 46.04%, and 44.05%.
To verify the improvement in attitude and positioning accuracy by using DEKF in the stepping phase, a 16 m × 16 m path planning experiment is carried out. e AGDA proposed in Section 3 is used to correct the attitude during the standing phase of the pedestrian walking cycle. e comparison experiments are carried out using di erent methods in the stepping phase. Figure 9 shows the results of the heading angle error obtained by di erent methods. Figure 10 shows the results of the position error obtained by di erent methods. In Figure 11, the trajectories are obtained by using di erent methods in the stepping phase. In addition, the legends of the curves in the gures are summarized as follows: Quaternion method: basic quaternion algorithm GDA + EKF: the attitude angles obtained by the quaternion algorithm and GDA are fused by thEKF AGDA + EKF: the attitude angles obtained by the quaternion algorithm and AGDA are fused by the EKF AGDA + DEKF: the attitude angles obtained by the quaternion algorithm and AGDA are fused by the DEKF Comparing the heading angle errors obtained by different methods, the results obtained by AGDA are better than those obtained by GDA. e heading angle error calculated by DEKF is smaller than that calculated by the EKF. e average errors of the heading angles are shown in Table 4. e average error of using AGDA in motion time is reduced by 20.30% compared with using GDA. e average error of using DEKF in motion time is reduced by 21.70% compared with using EKF.
Compared with the position errors obtained by di erent methods, the position error is e ectively reduced by using the DEKF fusion quaternion algorithm and AGDA. e     Figure 11 re ects the reduction of position errors by AGDA and DEKF. e results show that compared with the reference trajectory, the AGDA performs better than GDA for trajectory correction and DEKF is more e ective than EKF in improving the positioning accuracy. In the comparison of the above methods, the DEKF is mentioned by fusing the attitude angles, which are obtained by AGDA and     Complexity quaternion methods, respectively, and this method calculated the trajectory closest to the real trajectory by correcting the attitude and position.

Conclusion
In this work, a pedestrian attitude correction method based on foot-mounted inertial navigation is studied. An improved adaptive step-size adjustment algorithm is provided by the combining rotation matrix error of acceleration and magnetic field intensity. According to the noise interference characteristics of acceleration and angular velocity in pedestrian motion, an enhanced attitude fusion algorithm is proposed. Experiments and comparison analysis demonstrate that the AGDA and DEKF can effectively reduce attitude errors and improve the positioning accuracy. e proposed method is suitable for the navigation and positioning of firefighters in forest areas, and it can be extended to pedestrian positioning in other nonsatellite navigation aided areas, such as the positioning of elderly people in buildings, positioning of workers in mines, and positioning of pedestrians in underground supermarkets. However, the performance of the improved method has not been considered with changes in attitude. Future work will focus on the performance of the proposed method in 3D positioning.

Data Availability
e data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest
e authors declare that they have no conflicts of interest.