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

Authors: Miaoxin Ji; Jinhao Liu; Xiangbo Xu; Yuyang Guo; Zhenchun Lu
Journal: Complexity
 (2020)

---

## Abstract

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.


---

## Body

## 1. Introduction

High-precision pedestrian navigation systems usually include GPS and autonomous navigation. These systems can generate real-time data for a pedestrian’s attitude and position that can be widely used in fire protection, patrol, and military fields [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]. The cumulative error characteristics of inertial navigation integration greatly impact the positioning accuracy of a pedestrian in environments lacking GPS. Therefore, it is very important to improve the pedestrian attitude and positioning accuracy without GPS signals [5].In recent years, the forest firefighter positioning system based on Micro-Inertial Measurement Unit (MIMU) has recently attracted widespread attention with the rapid development of Micro-Electro-Mechanical System (MEMS). The foot-mounted inertial/magnetic sensor unit consists of a gyroscope, an accelerometer, and a magnetometer, which can provide the angular velocity, acceleration, and magnetic field intensity, respectively. Then, 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 final positioning results [8, 9].The positioning accuracy in the pedestrian navigation and positioning system is greatly affected 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–13]. This method divides the human gait into two phases: standing and stepping [14]. The 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–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–20]. However, the zero bias of the accelerometer seriously affects the accuracy of the attitude angle. The gradient descent algorithm (GDA) based on initial alignment is a method of attitude calculation that uses gradients to find the optimal quaternion solution [21–24]. This method obtains the minimum sensor measurement errors by iterating in the gradient direction to improve the attitude accuracy. In [24–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. The 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]. The original attitude updating algorithm usually uses the quaternion updating method. This 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. The 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. There 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. The 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. The 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. The 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. The Kalman filter does not work well on nonlinear systems and may even cause filter divergence. Therefore, 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. Thus, the noise estimation is the main factor affecting the filtering accuracy of the EKF. The traditional EKF estimation of noise is not accurate because this estimation is fixed. Therefore, the noise of the accelerometer and gyroscope in the dual-constrained extended Kalman filter (DEKF) method proposed in this paper is considered in the measurement noise, 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.The rest of the paper is organized as follows: the basic principles of the pedestrian Inertial Navigation System are introduced in Section2, including the ZUPT method 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.


## 2. Basic Principles of Inertial Navigation System

### 2.1. Principle of ZUPT Algorithm

The 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:(1)φx=arctan−axaz,φy=arcsinaya,φz=arctanmxcosφx+mysinφxmxsinφx−mzcosφxsinφy+mycosφy,where ax, ay, and az are the axial accelerations measured in the x, y, and z directions, respectively, by the accelerometer; a=ax2+ay2+az2, mx, my, and mz 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.The initial attitude angle is updated by the quaternion method to obtain an updated attitude angle after initial alignment. The quaternions are represented in the following form:(2)Q=q0+q1i+q2j+q3k,where q0, q1, q2, and q3 are quaternions and i, j, and k are fundamental quaternion units. The quaternion differential equations related to the angular velocity of the carrier are established:(3)Q˙=12QΩ,(4)Ω=0ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0.The angular velocities ofx, 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. The 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.


### 2.2. Pedestrian Positioning Algorithm

The 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. The basic formula of the dead reckoning algorithm is as follows:(5)Vkn=Vk−1n+Cbknfkb+gnT,(6)Pkn=Pk−1n+Vkn+Vk−1nT2,where Cbn is the coordinate transformation matrix from the b frame to n frame, Vkn and Vk−1n are the velocities along the n frame at times k and k − 1, respectively, fkb is the projection of the acceleration on the b frame at time k, gn=00−gT is gravity, and T is sample time.The inertial pedestrian navigation system model is shown in Figure1, which shows the whole process from the data acquisition to the output of attitude, velocity, and position.Figure 1
Inertial pedestrian navigation system model.The 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. The attitude and position are corrected using the DEKF.


## 2.1. Principle of ZUPT Algorithm

The 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:(1)φx=arctan−axaz,φy=arcsinaya,φz=arctanmxcosφx+mysinφxmxsinφx−mzcosφxsinφy+mycosφy,where ax, ay, and az are the axial accelerations measured in the x, y, and z directions, respectively, by the accelerometer; a=ax2+ay2+az2, mx, my, and mz 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.The initial attitude angle is updated by the quaternion method to obtain an updated attitude angle after initial alignment. The quaternions are represented in the following form:(2)Q=q0+q1i+q2j+q3k,where q0, q1, q2, and q3 are quaternions and i, j, and k are fundamental quaternion units. The quaternion differential equations related to the angular velocity of the carrier are established:(3)Q˙=12QΩ,(4)Ω=0ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0.The angular velocities ofx, 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. The 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.


## 2.2. Pedestrian Positioning Algorithm

The 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. The basic formula of the dead reckoning algorithm is as follows:(5)Vkn=Vk−1n+Cbknfkb+gnT,(6)Pkn=Pk−1n+Vkn+Vk−1nT2,where Cbn is the coordinate transformation matrix from the b frame to n frame, Vkn and Vk−1n are the velocities along the n frame at times k and k − 1, respectively, fkb is the projection of the acceleration on the b frame at time k, gn=00−gT is gravity, and T is sample time.The inertial pedestrian navigation system model is shown in Figure1, which shows the whole process from the data acquisition to the output of attitude, velocity, and position.Figure 1
Inertial pedestrian navigation system model.The 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. The attitude and position are corrected using the DEKF.


## 3. Adaptive Gradient Descent Algorithm

The initial attitude angle can be obtained by the initial alignment algorithm, but the noise of the gyroscope 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 filter for obtaining high-precision attitude information. The purpose of this filter is to reduce the quaternion error through acceleration and magnetic field intensity. When the error of the acceleration and magnetic field intensity reaches the minimum, the quaternion is robust and the attitude error is eliminated. Therefore, The GDA algorithm can be used as attitude calculation in the standing phase. However, the accelerometer bias and gyroscope drift seriously affect 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. Thus, the step size is positively correlated with the angular velocity. The 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). The rotation matrix error of magnetic field intensity characterization is the error between the measurement vector of the magnetometer and the projection of local magnetic field intensity vector in frame b, as shown in equation (8):(7)Fa=2gq1q3−q0q22gq0q1+q2q3q02−q12−q22+q32g−axayaz,(8)Fm=2myq1q2+q0q3+2mzq1q3−q0q2myq02−q12+q22−q32+2mzq0q1−q2q32myq2q3+q0q1+mzq02−q12−q22+q32−mxmymz,where Fa is the rotation matrix error of acceleration characterization and Fm is the rotation matrix error of the magnetic field intensity characterization.The gradient value of the acceleration error function and the gradient value of the error function of the magnetic field intensity can be written as follows:(9)∇F=Ja′∗Fa,∇h=Jm′∗Fm,where Ja is the Jacobian matrix of the acceleration and Jm is the Jacobian matrix of the magnetic field intensity. Ja and Jm are expressed by equations (10) and (11), respectively:(10)Ja=−2q2g2q3g−2q0g2q1g2q1g2q0g2q3g2q2g2q0g−2q1g−2q2g2q3g,(11)Jm=2q3my−2q2mz2q2my+2q3mz2q1my−2q0mz2q0my+2q1mz2q0my+2q1mz−2q1my+2q0mz2q2my+2q3mz−2q3my+2q2mz−2q1my+2q0mz−2q0my−2q1mz2q3my−2q2mz2q2my+2q3mz.The 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. The gradient direction can be attained by normalizing the gradient. The gradient direction of acceleration error function and the gradient direction of the error function of magnetic field intensity can be obtained by the following equations:(12)∇F•=∇F∇F,(13)∇h•=∇h∇h,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 timek can be computed by the quaternion at time k − 1:(14)Qk=Qk−1−α1∗μ∗∇F•−α1∗μ∗∇h•,where Qk and Qk−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. The adaptive factor can be expressed as(15)α1=γ1∗ω+ε1Fa+ε1+β1Fm+β1,where ω is the angular velocity module, Fa and Fm are moduli of Fa and Fm, respectively, γ1 is a positive correlation parameter, and ε1 and β1 are negatively correlated parameters. This 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. Therefore, the AGDA improves the accuracy of attitude angle in the standing phase.


## 4. Pedestrian Attitude and Position Updating Based on Double-Constrained Kalman Filter

The 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. Then, 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.


### 4.1. Adaptive Gradient Descent Algorithm Based on the Stepping Phase

The acceleration and angular velocity of pedestrians change constantly during the stepping phase. According to the AGDA in the standing phase, the quaternion atk time can be obtained as follows:(16)Qk=Qk−1−α2∗μ∗∇h•,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. Thus, 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. The step size is negatively correlated with the measurement value of the accelerometer. α2 is set to adjust the step size adaptively as follows:(17)α2=γ2∗ω+ε2Fm+ε2+β2a+β2,where γ2 is a positive correlation parameter and ε2 and β2 are the negative correlation parameters. This algorithm uses the adaptive adjustment of the step size to reduce the influence of magnetometer disturbance, gyroscope drift, and high-frequency acceleration disturbance. Therefore, the AGDA improves the accuracy of attitude and position correction in the stepping phase.


### 4.2. 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, Figures2 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.Figure 2
Acceleration change during the walking cycle.Figure 3
Angular velocity change during the walking cycle.The 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. The state equation and observation equation of the navigation system model are shown in the following equations:(18)Xk−=FkXk−1−+Wk,(19)Zk=HXk+Vk,where Xk− and Xk−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, Wk and Vk 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):(20)X=q0q1q2q3ξxξyξz,(21)H=I4×4O4×3,(22)F=FωO4×3O3×4Fb,where ξx, ξy, and ξz are gyroscope drift of x, y, and z axes, respectively, and Fω and Fb can be written by equations (23) and (24), respectively:(23)Fω=1−ωxt2−ωyt2−ωzt2ωxt21ωzt2−ωyt2ωyt2−ωzt21ωxt2ωzt2ωyt2−ωxt21,(24)Fb=1−λx0001−λy0001−λz,where λx, λy, and λz are the gyroscope drift coefficients of the x, y, and z axes, respectively.The 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. Therefore, the DEKF is proposed in this study at the moment of motion. The core formula is as follows:(25)Kk=Pk−HkTHkPkHkT+τRk,τ=γ3∗a+ω+γ3,where Kk is the Kalman gain, τ is an adaptive parameter, γ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, Pk and Pk− are error covariance matrices and prediction error covariance matrices, respectively, and Pk, Pk−, and Xk are shown in the following formulas:(26)Pk−=FkPk−1FkT+Qk,Pk=I−KkHkPk−,Xk=Xk−+KkZk−HkXk−.In the DEKF,τ is used to estimate the high-frequency interference caused by the change of acceleration and angular velocity in the stepping phase. The measurement noise of the DEKF is constrained by adaptive adjustment of parameter τ. Therefore, the attitude and position errors of the pedestrian positioning system are reduced.


## 4.1. Adaptive Gradient Descent Algorithm Based on the Stepping Phase

The acceleration and angular velocity of pedestrians change constantly during the stepping phase. According to the AGDA in the standing phase, the quaternion atk time can be obtained as follows:(16)Qk=Qk−1−α2∗μ∗∇h•,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. Thus, 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. The step size is negatively correlated with the measurement value of the accelerometer. α2 is set to adjust the step size adaptively as follows:(17)α2=γ2∗ω+ε2Fm+ε2+β2a+β2,where γ2 is a positive correlation parameter and ε2 and β2 are the negative correlation parameters. This algorithm uses the adaptive adjustment of the step size to reduce the influence of magnetometer disturbance, gyroscope drift, and high-frequency acceleration disturbance. Therefore, the AGDA improves the accuracy of attitude and position correction in the stepping phase.


## 4.2. 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, Figures2 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.Figure 2
Acceleration change during the walking cycle.Figure 3
Angular velocity change during the walking cycle.The 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. The state equation and observation equation of the navigation system model are shown in the following equations:(18)Xk−=FkXk−1−+Wk,(19)Zk=HXk+Vk,where Xk− and Xk−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, Wk and Vk 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):(20)X=q0q1q2q3ξxξyξz,(21)H=I4×4O4×3,(22)F=FωO4×3O3×4Fb,where ξx, ξy, and ξz are gyroscope drift of x, y, and z axes, respectively, and Fω and Fb can be written by equations (23) and (24), respectively:(23)Fω=1−ωxt2−ωyt2−ωzt2ωxt21ωzt2−ωyt2ωyt2−ωzt21ωxt2ωzt2ωyt2−ωxt21,(24)Fb=1−λx0001−λy0001−λz,where λx, λy, and λz are the gyroscope drift coefficients of the x, y, and z axes, respectively.The 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. Therefore, the DEKF is proposed in this study at the moment of motion. The core formula is as follows:(25)Kk=Pk−HkTHkPkHkT+τRk,τ=γ3∗a+ω+γ3,where Kk is the Kalman gain, τ is an adaptive parameter, γ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, Pk and Pk− are error covariance matrices and prediction error covariance matrices, respectively, and Pk, Pk−, and Xk are shown in the following formulas:(26)Pk−=FkPk−1FkT+Qk,Pk=I−KkHkPk−,Xk=Xk−+KkZk−HkXk−.In the DEKF,τ is used to estimate the high-frequency interference caused by the change of acceleration and angular velocity in the stepping phase. The measurement noise of the DEKF is constrained by adaptive adjustment of parameter τ. Therefore, the attitude and position errors of the pedestrian positioning system are reduced.


## 5. Experimental Study on Pedestrian Positioning Algorithms

### 5.1. Structure of Improved Pedestrian Attitude and Position Algorithms

Figure4 introduces the flow chart from the original input data to the output data. The attitude correction is carried out in two phases by AGDA and DEKF methods.Figure 4
Principle of improved pedestrian location algorithm.The 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.


### 5.2. Experimental Results

According to the experiment, the validity of AGDA and DEKF is verified. MPU-9150 is used as the acquisition system to measure the static and motion parameters of pedestrian feet. The parameters of MPU-9150 sensor are shown in Table1.Table 1
MPU-9150 parameters.
Sensors
Typical
Gyro
Bias repeatability
deg
/
s
0.06
Noise density
deg
/
s
.
Hz
0.005
Full-scale range
deg
/
s
±1000
Accelerometer
Bias repeatability
m
/
s
2
0.0392
Noise density
μ
g
/
.
Hz
400
Full-scale range
(g)
±2
Magnetometer
Full-scale range
μ
T
±1200The experimental devices are utilized to collect the output and calculate the experimental results of the pedestrian, which are shown in Figure5. The red line is the reference trajectory (16 m × 16 m) in test 2. The sample frequency of sensors is 30 Hz.Figure 5
Hardware system and experimental area.The parameter selection of the algorithm proposed in the experiment is shown in Table2.Table 2
Control parameter.
Parameter
μ
γ
1
ε
1
β
1
γ
2
ε
2
β
2
γ
3
Value
0.02
0.018
0.02
0.0075
0.0015
0.003
0.001
0.001To 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 performance of the algorithm. The results of the attitude angle are collected by pedestrians at the static state during a stepping cycle. The attitude angles are corrected by GDA and AGDA methods, respectively, in the pedestrian standing phase. Figures6–8 show the contrast waveform of pitch angle error, roll angle error, and heading angle error, respectively. In addition, the legends for the curves in figures are summarized as follows:GDA: the traditional gradient descent algorithm discussed in Section3AGDA: the adaptive gradient descent algorithm proposed in Section3Figure 6
Pitch angle error results.Figure 7
Roll angle error results.Figure 8
Heading angle error results.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 figure, the attitude angle error can be reduced and stabilized by using the AGDA. To obtain better experimental verification, four different groups of people were tested and the experimental results of four groups were calculated. The average errors of the pitch angle, roll angle, and heading angle are shown in Table3. 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%.Table 3
Average error of attitude angle in test 1.
Test
Average error of pitch angle (°)
Average error of roll angle (°)
Average error of heading angle (°)
GDA
AGDA
GDA
AGDA
GDA
AGDA
Person 1
0.7503
0.0597
0.2606
0.0530
0.2511
0.0541
Person 2
0.5698
0.2173
0.7336
0.0733
0.6446
0.1192
Person 3
0.5171
0.0568
0.2932
0.0440
0.3617
0.0390
Person 4
0.6192
0.1787
0.3470
0.0987
0.6470
0.1180To 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. The AGDA proposed in Section3 is used to correct the attitude during the standing phase of the pedestrian walking cycle. The comparison experiments are carried out using different methods in the stepping phase. Figure 9 shows the results of the heading angle error obtained by different methods. Figure 10 shows the results of the position error obtained by different methods. In Figure 11, the trajectories are obtained by using different methods in the stepping phase. In addition, the legends of the curves in the figures are summarized as follows:Quaternion method: basic quaternion algorithmGDA + EKF: the attitude angles obtained by the quaternion algorithm and GDA are fused by thEKFAGDA + EKF: the attitude angles obtained by the quaternion algorithm and AGDA are fused by the EKFAGDA + DEKF: the attitude angles obtained by the quaternion algorithm and AGDA are fused by the DEKFFigure 9
Heading angle error results.Figure 10
Position error results.Figure 11
Contrast results of trajectories.Comparing the heading angle errors obtained by different methods, the results obtained by AGDA are better than those obtained by GDA. The heading angle error calculated by DEKF is smaller than that calculated by the EKF. The average errors of the heading angles are shown in Table4. The average error of using AGDA in motion time is reduced by 20.30% compared with using GDA. The average error of using DEKF in motion time is reduced by 21.70% compared with using EKF.Table 4
Average error of heading angle in test 2.
Test algorithm
Average error of heading angle (°)
Quaternion method
2.734
GDA + EKF
1.367
AGDA + EKF
1.164
AGDA + DEKF
0.947Compared with the position errors obtained by different methods, the position error is effectively reduced by using the DEKF fusion quaternion algorithm and AGDA. The trajectory in Figure11 reflects the reduction of position errors by AGDA and DEKF.The results show that compared with the reference trajectory, the AGDA performs better than GDA for trajectory correction and DEKF is more effective 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 quaternion methods, respectively, and this method calculated the trajectory closest to the real trajectory by correcting the attitude and position.


## 5.1. Structure of Improved Pedestrian Attitude and Position Algorithms

Figure4 introduces the flow chart from the original input data to the output data. The attitude correction is carried out in two phases by AGDA and DEKF methods.Figure 4
Principle of improved pedestrian location algorithm.The 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.


## 5.2. Experimental Results

According to the experiment, the validity of AGDA and DEKF is verified. MPU-9150 is used as the acquisition system to measure the static and motion parameters of pedestrian feet. The parameters of MPU-9150 sensor are shown in Table1.Table 1
MPU-9150 parameters.
Sensors
Typical
Gyro
Bias repeatability
deg
/
s
0.06
Noise density
deg
/
s
.
Hz
0.005
Full-scale range
deg
/
s
±1000
Accelerometer
Bias repeatability
m
/
s
2
0.0392
Noise density
μ
g
/
.
Hz
400
Full-scale range
(g)
±2
Magnetometer
Full-scale range
μ
T
±1200The experimental devices are utilized to collect the output and calculate the experimental results of the pedestrian, which are shown in Figure5. The red line is the reference trajectory (16 m × 16 m) in test 2. The sample frequency of sensors is 30 Hz.Figure 5
Hardware system and experimental area.The parameter selection of the algorithm proposed in the experiment is shown in Table2.Table 2
Control parameter.
Parameter
μ
γ
1
ε
1
β
1
γ
2
ε
2
β
2
γ
3
Value
0.02
0.018
0.02
0.0075
0.0015
0.003
0.001
0.001To 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 performance of the algorithm. The results of the attitude angle are collected by pedestrians at the static state during a stepping cycle. The attitude angles are corrected by GDA and AGDA methods, respectively, in the pedestrian standing phase. Figures6–8 show the contrast waveform of pitch angle error, roll angle error, and heading angle error, respectively. In addition, the legends for the curves in figures are summarized as follows:GDA: the traditional gradient descent algorithm discussed in Section3AGDA: the adaptive gradient descent algorithm proposed in Section3Figure 6
Pitch angle error results.Figure 7
Roll angle error results.Figure 8
Heading angle error results.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 figure, the attitude angle error can be reduced and stabilized by using the AGDA. To obtain better experimental verification, four different groups of people were tested and the experimental results of four groups were calculated. The average errors of the pitch angle, roll angle, and heading angle are shown in Table3. 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%.Table 3
Average error of attitude angle in test 1.
Test
Average error of pitch angle (°)
Average error of roll angle (°)
Average error of heading angle (°)
GDA
AGDA
GDA
AGDA
GDA
AGDA
Person 1
0.7503
0.0597
0.2606
0.0530
0.2511
0.0541
Person 2
0.5698
0.2173
0.7336
0.0733
0.6446
0.1192
Person 3
0.5171
0.0568
0.2932
0.0440
0.3617
0.0390
Person 4
0.6192
0.1787
0.3470
0.0987
0.6470
0.1180To 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. The AGDA proposed in Section3 is used to correct the attitude during the standing phase of the pedestrian walking cycle. The comparison experiments are carried out using different methods in the stepping phase. Figure 9 shows the results of the heading angle error obtained by different methods. Figure 10 shows the results of the position error obtained by different methods. In Figure 11, the trajectories are obtained by using different methods in the stepping phase. In addition, the legends of the curves in the figures are summarized as follows:Quaternion method: basic quaternion algorithmGDA + EKF: the attitude angles obtained by the quaternion algorithm and GDA are fused by thEKFAGDA + EKF: the attitude angles obtained by the quaternion algorithm and AGDA are fused by the EKFAGDA + DEKF: the attitude angles obtained by the quaternion algorithm and AGDA are fused by the DEKFFigure 9
Heading angle error results.Figure 10
Position error results.Figure 11
Contrast results of trajectories.Comparing the heading angle errors obtained by different methods, the results obtained by AGDA are better than those obtained by GDA. The heading angle error calculated by DEKF is smaller than that calculated by the EKF. The average errors of the heading angles are shown in Table4. The average error of using AGDA in motion time is reduced by 20.30% compared with using GDA. The average error of using DEKF in motion time is reduced by 21.70% compared with using EKF.Table 4
Average error of heading angle in test 2.
Test algorithm
Average error of heading angle (°)
Quaternion method
2.734
GDA + EKF
1.367
AGDA + EKF
1.164
AGDA + DEKF
0.947Compared with the position errors obtained by different methods, the position error is effectively reduced by using the DEKF fusion quaternion algorithm and AGDA. The trajectory in Figure11 reflects the reduction of position errors by AGDA and DEKF.The results show that compared with the reference trajectory, the AGDA performs better than GDA for trajectory correction and DEKF is more effective 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 quaternion methods, respectively, and this method calculated the trajectory closest to the real trajectory by correcting the attitude and position.


## 6. 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. The 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.



---

*Source: 4361812-2020-01-08.xml*