An Optimized Pedestrian Inertial Navigation Method Based on the Birkhoff Pseudospectral Method

TL;DR

This paper introduces a new pedestrian navigation method that significantly improves accuracy by reducing IMU noise using a Birkhoff pseudospectral approach.

Contribution

The novel use of the Birkhoff pseudospectral method for post-processing pedestrian inertial navigation data to suppress IMU noise.

Findings

The proposed method reduces position error by approximately 90% compared to traditional EKF-based methods.

Simulation and physical experiments confirm the superior noise suppression capability of the new algorithm.

Abstract

Pedestrian inertial navigation is a pivotal technology for achieving seamless indoor and outdoor positioning. Traditional methods based on the Extended Kalman Filter (EKF) suffer from cumulative errors induced by inertial measurement unit (IMU) noise, which severely degrade the accuracy of pedestrian trajectory estimation over long durations. To address this critical limitation, a post-processing trajectory optimization approach for pedestrian inertial navigation based on the Birkhoff pseudospectral method is proposed in this paper. Leveraging the initial attitude and position estimates derived from the Zero-Velocity Update (ZUPT) technique and the EKF framework, the proposed method first parameterizes continuous-time acceleration measurements by adopting Chebyshev nodes as collocation points, and then formulates and solves the trajectory optimization problem via a Birkhoff pseudospectral framework, which effectively suppresses noise interference from the IMU accelerometer. Simulation experiments validate the superior noise suppression capability of the proposed algorithm. Furthermore, physical experiments conducted with a foot-mounted IMU demonstrate that the final position error is reduced by approximately 90% in comparison with the traditional EKF-based method.

Figures

Full text

1. Introduction

With the advancement of technology, pedestrian navigation systems have been widely deployed in various application domains, including indoor positioning, emergency rescue, and autonomous mobile robotics [1]. Pedestrian navigation systems can be categorized into non-autonomous and autonomous types according to their dependencies on external reference signals. Non-autonomous navigation relies on external reference signals from the Global Navigation Satellite System (GNSS) or other pre-deployed infrastructure (e.g., WiFi and Bluetooth) [2]. In contrast, autonomous navigation systems operate purely on onboard sensors, and inertial measurement units (IMUs) serve as their core sensing component. Among autonomous navigation systems, IMU-based pedestrian navigation systems (PNSs) play an irreplaceable role in GNSS-denied environments (e.g., indoor spaces, underground tunnels, and dense urban canyons) [3].

Autonomous IMU-based PNSs can be further divided into foot-mounted and non-foot-mounted configurations based on on-body sensor placement [4]. Non-foot-mounted systems typically adopt a step-and-heading system (SHS) approach, which detects gait cycles, estimates step length and heading, and then calculates the positional changes of pedestrians accordingly. Although this configuration allows the IMU to be worn on various body locations (e.g., the waist, wrist, or chest), it relies on idealized walking models and fails to fully exploit the rich acceleration and angular velocity information provided by the IMU [5]. In recent years, deep learning-based methods have been proposed for step length and heading angle estimation, which improve the adaptability of SHS to different gait patterns and motion states [6].

In contrast, foot-mounted pedestrian navigation systems track the real-time position and orientation of pedestrians by integrating measurements from gyroscopes and accelerometers. A key premise of this integration is that the foot-mounted IMU is generally treated as a rigid body [7] since the sensor is securely attached to the foot without relative sliding or displacement during gait, and the foot itself exhibits high mechanical stability with minimal deformation in typical walking scenarios. This fundamental assumption bridges foot-mounted PNS to Strapdown Inertial Navigation System (SINS) theory [8]. With the continuous development of Micro-Electro-Mechanical System (MEMS) technology, low-cost, miniaturized IMUs have been widely integrated into consumer electronics such as smartphones and wearable devices, providing a solid hardware foundation for the practical application of foot-mounted pedestrian navigation technologies [9].

However, achieving high-accuracy pedestrian positioning with MEMS IMUs remains a formidable challenge because the sensors are inherently affected by random noise, bias drift, and scale-factor errors [10]. In PNSs, the high-precision estimation of velocity and position is a core determinant of overall system performance. Traditional positioning methods, which conform to the core logic of SINSs, typically obtain velocity and position sequentially by double integrating the specific force measured by accelerometers [11]. Nevertheless, these approaches suffer from two inherent defects: (1) the noise of MEMS IMUs is accumulated and amplified during the integration process, leading to position errors that diverge at a rate proportional to the cube of time; and (2) in highly dynamic motion scenarios, residual errors in gravity compensation further exacerbate the inaccuracies of velocity and position estimation [3,12]. The existing research indicates that for low-cost MEMS IMUs, the positioning errors can reach 5% to 10% of the total traveled distance even over short periods, and such error divergence becomes more pronounced during complex motions such as running or variable-speed walking [13].

To enhance the accuracy of traditional integration methods, researchers have turned to approaches based on kinematic modeling and parametric fitting. A typical category of methods uses polynomials or other basis functions to locally fit specific force and angular velocity signals within sampling intervals. In [14], the authors constructed a continuous-time motion model, thereby replacing the approximate assumptions (e.g., piecewise constant or linear approximation) inherent in traditional methods. Yang et al. [15] proposed a velocity estimation algorithm based on specific-force polynomial modeling. By optimizing polynomial coefficients and constructing a high-precision attitude matrix, their method achieved significant improvement in velocity estimation accuracy within the geographic coordinate frame. On the other hand, Zhu [16] and Wu et al. [17] employed Chebyshev polynomials to transform the continuous-time navigation-state estimation problem into an optimization problem for polynomial coefficients, realizing error minimization through coefficient optimization. Such methods can effectively suppress dynamic error terms such as sculling errors, offering a promising solution for inertial navigation in highly dynamic environments.

In fact, the methods in [16,17] adopted a collocation strategy similar to the pseudospectral method widely used in optimal control and trajectory optimization. The core idea of the traditional pseudospectral method is to parameterize the state trajectory using global interpolating polynomials at a series of collocation points, which transforms continuous differential equation constraints into algebraic constraints and ultimately converts the original problem into a nonlinear programming (NLP) problem [18,19,20]. Unlike the traditional pseudospectral method, the Birkhoff pseudospectral method proposed in [21] leverages the characteristic of Birkhoff polynomials that perform interpolation based on the original function values at the boundaries and the derivatives at the internal collocation points. This enables it to conveniently incorporate the position constraints at the trajectory endpoints into the optimization problem, reduce the condition number of the transformed problem, and facilitate rapid solution [22].

Inspired by the collocation strategy of [17] and the Birkhoff pseudospectral method in [21], this paper introduces the Birkhoff pseudospectral method for solving the trajectory estimation problem of pedestrian inertial navigation, focusing on how to utilize its global approximation framework for high-precision estimation and optimization of pedestrian motion trajectories. To the best of our knowledge, there are no reported papers that apply the Birkhoff pseudospectral method to the solution of pedestrian navigation trajectory optimization. The main contributions of this paper lie in the following two aspects: (1) a two-step algorithm framework is constructed, which takes the bias-corrected state sequence from Zero Velocity Update (ZUPT)-aided Extended Kalman Filter (EKF) as initial input and realizes global trajectory optimization through window division based on gait cycles; (2) the Birkhoff pseudospectral method is used to model the pedestrian trajectory optimization problem with a carefully designed collocation strategy and objective function to ensure optimization accuracy and robustness.

The remainder of this paper is organized as follows. Section 2 outlines the theoretical foundations, including the principles of SINS, ZUPT, and EKF. Section 3 details the proposed trajectory optimization framework based on the Birkhoff pseudospectral method. Section 4 and Section 5 present the simulation and physical experiments conducted to validate the performance of the proposed algorithm, respectively; finally, Section 6 concludes the paper and discusses future research directions.

2. Theoretical Basis

2.1. Principles of SINS

The core principle of SINS is to estimate the navigation states of a carrier in real time by solving kinematic equations based on the specific-force and angular velocity information output by a six-axis IMU, which integrates three orthogonal accelerometers for specific-force detection and three orthogonal gyroscopes for angular velocity measurement. For the clarity of subsequent derivation, three coordinate frames are defined in this paper: (1) the body frame (denoted as $[eqn]$ -frame), which is rigidly fixed to the IMU and moves with the carrier; (2) the navigation frame (denoted as $[eqn]$ -frame), which adopts the local-level frame (east–north–up, ENU) for pedestrian navigation; (3) the inertial frame (denoted as $[eqn]$ -frame), which takes the Earth-centered inertial (ECI) frame as the reference [23].

For navigation solutions over short time intervals, the Earth’s rotation and the curvature of the Earth can be neglected, and the system dynamics can be described by the following set of differential equations:

$$[eqn]$$ $$[eqn]$$ $$[eqn]$$

where $[eqn]$ denotes the true angular velocity of the $[eqn]$ -frame with respect to the $[eqn]$ -frame. $[eqn]$ denotes the rotation matrix from the $[eqn]$ -frame to the $[eqn]$ -frame. $[eqn]$ denotes the true specific force acting on the carrier in the $[eqn]$ -frame. $[eqn]$ denotes the velocity vector of the carrier in the $[eqn]$ -frame. $[eqn]$ and $[eqn]$ denote the gravity acceleration vector in the $[eqn]$ -frame and the position vector of the carrier in $[eqn]$ -frame, respectively. $[eqn]$ represents the attitude quaternion from the $[eqn]$ -frame to the $[eqn]$ -frame. The operator $[eqn]$ denotes quaternion multiplication, which for two quaternions $[eqn]$ and $[eqn]$ is defined as

$$[eqn]$$

Two quaternion multiplication matrices $[eqn]$ and $[eqn]$ are defined respectively as

$$[eqn]$$

where $[eqn]$ represents a $[eqn]$ identity matrix. The skew-symmetric matrix $[eqn]$ is defined to satisfy the cross-product relation $[eqn]$ . The transformation relationship between the attitude matrix $[eqn]$ and the attitude quaternion is given by

$$[eqn]$$

2.2. Principles of the EKF

The EKF provides a widely adopted framework for real-time state estimation in nonlinear systems. In this work, an error-state EKF is employed to correct the drifting inertial navigation solution by fusing IMU measurements with external observations. Its error-state vector generally includes error terms such as attitude, velocity, position, and sensor biases, which are specifically defined as [24]

$$[eqn]$$

where $[eqn]$ denotes the position error vector in the navigation frame, $[eqn]$ is the velocity error vector, $[eqn]$ represents the attitude error vector, and $[eqn]$ and $[eqn]$ are the measurement bias error vectors of the accelerometer and gyroscope, respectively.

The continuous-time error dynamics are linearized as:

$$[eqn]$$

where $[eqn]$ is the continuous-time state-transition matrix, $[eqn]$ is the noise input matrix, and $[eqn]$ is its corresponding power spectral density matrix.

Discretizing over a sampling interval $[eqn]$ yields:

$$[eqn]$$

where $[eqn]$ is the state-transition matrix and $[eqn]$ is the discrete-time process noise covariance. The error covariance is propagated as:

$$[eqn]$$

where $[eqn]$ is the estimation error covariance matrix. When external measurements are available, the filter corrects the error state using a linearized observation model:

$$[eqn]$$

where $[eqn]$ is the measurement residual vector, and $[eqn]$ is the measurement Jacobian. $[eqn]$ is the measurement noise covariance matrix. The Kalman gain $[eqn]$ is computed in the usual way, and the error-state estimate and its covariance are updated.

The EKF operates recursively. During the prediction phase, the IMU measurements propagate the nominal state via the nonlinear kinematic equations, while the error-state covariance is updated linearly using the system Jacobian. In the update phase, the zero-velocity observations identified by ZUPT serve as measurement constraints. The Kalman gain is computed to fuse this information, correcting the error-state estimates and calibrating the sensor biases in real-time.

Thus, the EKF leverages the periodic zero-velocity constraints to suppress the cubic drift of the pure inertial solution and provides a smoothed, bias-corrected state sequence [25]. This sequence serves as the initial trajectory and prior information for the subsequent window-based Birkhoff pseudospectral optimization.

2.3. ZUPT Algorithm

ZUPT is a core method for suppressing error accumulation in inertial navigation systems, and its effectiveness hinges on the accurate detection of zero-velocity intervals. The principle of ZUPT is rooted in the physical constraint that, during the stance phase of pedestrian gait, the foot-mounted IMU is momentarily stationary relative to the ground, yielding a true velocity of zero. By detecting these intervals, ZUPT injects a “virtual measurement” of zero velocity into the navigation filter, which corrects accumulated velocity, position, and attitude errors propagated from inertial sensor noise and bias.

The Generalized Likelihood Ratio Test (GLRT) provides a statistically rigorous and practically implementable framework for this detection task [26]. It formulates zero-velocity detection as a hypothesis-testing problem. Under the null hypothesis $[eqn]$ , the IMU measurements consist only of sensor noise:

$$[eqn]$$

where $[eqn]$ and $[eqn]$ represent the three-axis acceleration and three-axis angular velocity output by the IMU, respectively; $[eqn]$ is the mean vector of accelerometer measurements within the corresponding detection window. $[eqn]$ and $[eqn]$ are the variances of accelerometer measurement noise and gyroscope measurement noise, respectively, which are calibrated using Allan variance analysis. Under the alternative hypothesis $[eqn]$ (foot in motion), the measurements contain both the true motion signals and sensor noise.

Assuming independent and identically distributed samples, the log-likelihood ratio for a window of length $[eqn]$ is:

$$[eqn]$$

Substituting the Gaussian densities and simplifying under the assumption that $[eqn]$ has no deterministic structure (i.e., the signal can take any value), the GLRT statistic reduces to:

$$[eqn]$$

The test compares $[eqn]$ to a threshold $[eqn]$ , declaring a zero-velocity interval if $[eqn]$ .

In pedestrian navigation, foot-mounted or wearable IMU data exhibit distinct statistical characteristics between the stance and swing phases. During stance, accelerometer and gyroscope measurements can be approximated as zero-mean Gaussian noise. In contrast, swing-phase data includes deterministic signals from pedestrian dynamics alongside high-intensity random noise.

Compared to heuristic threshold or handcrafted feature-based methods, GLRT eliminates cumbersome parameter tuning. By maximizing the likelihood function, it automatically adapts to user-specific gait patterns, sensor installation variations, and environmental noise, thereby enhancing detection robustness, improving the detection probability, and reducing false-alarm rates. Integrating the GLRT into ZUPT robustly captures the brief static moments in the pedestrian stance phase [27,28], providing reliable zero-velocity observations for EKF measurement updates and effectively suppressing velocity and position error accumulation.

3. Pedestrian Navigation Algorithm Based on the Birkhoff Pseudospectral Method

The pseudospectral methods are a class of efficient numerical optimization methods, whose core lies in discretizing and controlling variables at a finite number of collocation points and using global orthogonal polynomials for interpolation, thereby transforming continuous-time optimal control problems into discrete NLP problems. The Birkhoff pseudospectral method is an important variant of the spectral method. It is inherently well-suited for handling various boundary conditions—a characteristic that makes it highly suitable for problems such as navigation trajectory optimization, which typically involve explicit initial values, terminal values, and process constraints [29].

Based on this, this paper designs a pedestrian navigation trajectory optimization method based on the Birkhoff pseudospectral method, whose overall workflow is shown in Figure 1.

3.1. Core Theory of the Birkhoff Pseudospectral Method

3.1.1. Birkhoff Interpolation Principle

Birkhoff interpolation differs from Lagrange interpolation, which only constrains function values. It imposes mixed constraints on function values and multi-order derivatives, making it more suitable for complex dynamic systems. Let $[eqn]$ serve as collocation points, with the interpolation interval being $[eqn]$ . Given $[eqn]$ known real numbers $[eqn]$ , determine the unique (highest) $[eqn]$ -th order interpolation polynomial $[eqn]$ that exists and is unique, and satisfies the following relations:

$$[eqn]$$

where $[eqn]$ denotes the order of the derivative. All cases are classified as Birkhoff interpolation except for those with $[eqn]$ .

3.1.2. Birkhoff Pseudospectral Method Discretization

In practical application scenarios, dynamic models generally do not involve integral operations of order three and above; thus, this paper only elaborates on the first-order and second-order Birkhoff integral matrices.

The Chebyshev–Gauss–Lobatto (CGL) collocation points are adopted, which are defined as the zeros of the derivative of the $[eqn]$ -th order Chebyshev polynomial and include the interval endpoints:

$$[eqn]$$

The higher density of collocation points near the boundaries improves the approximation accuracy of boundary constraints and is well-suited to handling initial/terminal state constraints.

For the optimization problem defined over the time interval $[eqn]$ , it is transformed into the standard interval $[eqn]$ via the time normalization mapping:

$$[eqn]$$

and CGL collocation points $[eqn]$ are set within the interval to discretize the state variables.

Any function $[eqn]$ defined over $[eqn]$ can be fitted by a series of orthogonal Lagrange polynomials, i.e.,

$$[eqn]$$

where $[eqn]$ denotes the Kronecker delta function, which satisfies $[eqn]$ = $[eqn]$ . Differentiating both sides of the above equation yields

$$[eqn]$$

Let $[eqn]$ , $[eqn]$ , $[eqn]$ , then Equation (19) can be rewritten as

$$[eqn]$$

The differentiation matrix $[eqn]$ is a core tool for derivative approximation in the pseudospectral method, and it is constructed based on the derivative properties of Chebyshev polynomials.

Birkhoff polynomials are an extension of Lagrange polynomials. The first-order and second-order Birkhoff polynomials of $[eqn]$ are defined as

$$[eqn]$$ $$[eqn]$$

where the first-order Birkhoff interpolation basis polynomials $[eqn]$ satisfy

$$[eqn]$$

and the second-order Birkhoff interpolation basis polynomials $[eqn]$ satisfy

$$[eqn]$$

Let $[eqn]$ and $[eqn]$ and then the first-order and second-order Birkhoff integral matrices can be obtained:

$$[eqn]$$ $$[eqn]$$

Combining Equations (20) and (21), the first row of $[eqn]$ is reconstructed as the unit impulse vector $[eqn]$ to enforce the satisfaction of the initial boundary constraints. For the first-order differential equation $[eqn]$ , the input $[eqn]$ can be directly converted into the constraint for the state variable $[eqn]$ via $[eqn]$ :

$$[eqn]$$

Similarly, combining Equation (22), the first row and last row of $[eqn]$ are respectively reconstructed as the unit impulse vectors $[eqn]$ and $[eqn]$ to enforce the satisfaction of the initial and terminal boundary constraints. For the second-order differential equation $[eqn]$ that satisfies $[eqn]$ , the input $[eqn]$ can be directly converted into the constraint for the state variable $[eqn]$ :

$$[eqn]$$

3.2. Quaternion and Specific-Force Reconstruction

To meet the discretized input requirement of the Birkhoff pseudospectral method, the quaternions, angular velocity, and specific force need to be projected onto Chebyshev points. Based on the ZUPT detection results, the pedestrian walking trajectory is divided into continuous optimization windows by gait cycles, where each window corresponds to a single gait cycle.

For each window, its time interval is denoted as $[eqn]$ , which is mapped to the standard interval $[eqn]$ via a linear transformation in Equation (17). At the equal-time-interval moments $[eqn]$ within the current window, the specific forces $[eqn]$ are resampled on the CGL collocation points $[eqn]$ via the cubic spline interpolation technique, resulting in the aligned $[eqn]$ .

Since the quaternion $[eqn]$ , directly interpolating the quaternion in the Euclidean space will destroy its unitarity, leading to an uneven distribution of angular velocity after interpolation [30,31]. Hence, the Squad interpolation method on the $[eqn]$ group is used for the quaternion reconstruction on the CGL collocation points. For the quaternions $[eqn]$ , an auxiliary point $[eqn]$ is assigned to each interpolation point to smooth the angular velocity of the interpolation curve. $[eqn]$ is defined by the logarithmic mapping difference of adjacent quaternions:

$$[eqn]$$

When computing auxiliary points for $[eqn]$ , the adjacent quaternions $[eqn]$ and $[eqn]$ are undefined at the endpoints $[eqn]$ and $[eqn]$ . These virtual quaternions are constructed via spherical mirroring on $[eqn]$ : $[eqn]$ is obtained by reflecting $[eqn]$ across the boundary quaternion $[eqn]$ , while $[eqn]$ is formed by reflecting $[eqn]$ across $[eqn]$ , preserving the spherical geometric properties of unit quaternions.

The Slerp interpolation is given by

$$[eqn]$$

where $[eqn]$ and $[eqn]$ is defined as a normalized interpolation parameter confined to the interval $[eqn]$ . Then, the Squad interpolation within the interval $[eqn]$ is given by

$$[eqn]$$

where $[eqn]$ .

The quaternion obtained through the above transformation is denoted as $[eqn]$ , and $[eqn]$ is the rotation matrix corresponding to $[eqn]$ .

3.3. Velocity–Position Optimization

Velocity–position optimization is a key process that estimates the position $[eqn]$ , velocity $[eqn]$ , and accelerometer bias $[eqn]$ of the carrier within the optimization time interval $[eqn]$ , based on the discrete specific force $[eqn]$ and rotation matrix $[eqn]$ . Given the known attitude sequence, this process converts the complex nonlinear coupling problem into a relatively simple optimization problem and achieves high-precision trajectory reconstruction via the global discretization framework of the Birkhoff pseudospectral method.

In the navigation frame, the continuous-time dynamic equation of the carrier’s motion can be expressed as shown in Equations (2) and (3). In Equation (2), the true specific force $[eqn]$ can be approximated by its corresponding state estimate $[eqn]$ . The velocity–position optimization problem is defined as a constrained dynamic optimization problem, where the system state variable $[eqn]$ is defined as $[eqn]$ , and the control variable $[eqn]$ is defined as $[eqn]$ .

The objective function of velocity–position optimization aims to balance boundary prior information, dynamic constraints, and smoothing terms. Its mathematical formulation is as follows:

$$[eqn]$$

The prior term $[eqn]$ in Equation (32) is designed to deal with the boundary conditions provided by ZUPT, as follows:

$$[eqn]$$

where $[eqn]$ , $[eqn]$ and $[eqn]$ represent the initial position, initial velocity, and terminal velocity transferred from the previous optimization window, respectively. In practical implementations, the initial velocity $[eqn]$ and terminal velocity $[eqn]$ are typically set to $[eqn]$ . Through Equation (33), we can enforce the initial state of the optimized trajectory to be consistent with physical rules. Here, the designed parameter $[eqn]$ is the prior term’s weight coefficient: selected for trajectory parameterization, it balances ZUPT boundary constraint credibility and trajectory optimization flexibility—ensuring constraint reliability and the initial state’s consistency with physical rules.

The measurement term $[eqn]$ is designed to minimize the difference between the raw specific force measured by the IMU and the theoretical specific force derived from the numerically differentiated trajectory, as follows:

$$[eqn]$$

where $[eqn]$ is the accelerometer noise covariance matrix, and $[eqn]$ denotes the weighting coefficient of the measurement term. It is adjusted to match the noise characteristics of MEMS-IMUs, thereby effectively reducing deviations between the measured and theoretical acceleration data.

Since the accelerometer bias is treated as a state variable, its variation is physically slow and does not fluctuate sharply in a short time. Therefore, a smoothing term $[eqn]$ is added to constrain the estimated bias to maintain continuous and smooth variation in the time dimension:

$$[eqn]$$

here, $[eqn]$ denotes the variation of the accelerometer bias, and $[eqn]$ is the weight coefficient of the smoothing term. $[eqn]$ is tuned to constrain the amplitude of $[eqn]$ , thereby restricting abrupt fluctuations of the accelerometer bias—this ensures the temporal continuity of the bias and its consistency with the slow time-varying physical characteristic.

The periodicity of pedestrian gait provides natural boundary conditions for zero-velocity moments. By using the ZUPT algorithm to accurately detect the static moment at the end of the stance phase, the long-endurance navigation trajectory can be divided into a series of continuous optimization windows. For each window, its boundary conditions are given by Equation (33). Based on the above discretization and window division, the velocity–position trajectory optimization problem can be transformed into a finite-dimensional NLP problem of the following form:

$$[eqn]$$

Based on the properties of the Birkhoff integral matrix and pedestrian boundary conditions, $[eqn]$ and $[eqn]$ can be approximated as

$$[eqn]$$ $$[eqn]$$

Then the dynamic constraint in Equation (36) can be rewritten as

$$[eqn]$$ $$[eqn]$$

here, $[eqn]$ is defined as the time scaling coefficient, with its value given by $[eqn]$ .

To improve the robustness of the optimization problem and avoid infeasible solutions caused by measurement noise or model errors, slack variables are introduced to convert the above dynamic hard constraints into soft constraints. The constraint conditions after slack processing are as follows:

$$[eqn]$$ $$[eqn]$$

here, $[eqn]$ and $[eqn]$ are the velocity slack coefficient and position slack coefficient, respectively.

For the constraints after slack processing, a penalty term $[eqn]$ needs to be added to the objective function to penalize deviations from the dynamic constraints. The expression of this penalty term is as follows:

$$[eqn]$$

where $[eqn]$ is the weight of the penalty term. It is tuned via experimental tuning to effectively penalize deviations from dynamic constraints, while ensuring the robustness of the optimization problem and preventing infeasible solutions induced by MEMS-IMU measurement noise or model mismatches.

The values of these weighting coefficients are set via systematic experimental tuning, ensuring that the gradients of all terms in the objective function remain within comparable orders of magnitude, thereby promoting stable and balanced convergence during optimization.

Thus, the discretized velocity–position trajectory optimization problem can be formulated as the following NLP problem:

$$[eqn]$$

Algorithm 1 outlines the core procedures of the pedestrian navigation trajectory optimization method presented in this work. The method operates in an iterative workflow, as detailed below: Algorithm 1: Birkhoff pseudospectral optimization algorithm initialized via ZUPT/EKFInput:  $[eqn]$ .Output: Globally optimized state sequence $[eqn]$ . Initialize states as $[eqn]$ , $[eqn]$ , $[eqn]$ . Compute the Birkhoff integral matrix $[eqn]$ . Compute the zero-velocity intervals $[eqn]$ . $[eqn]$ Estimate $[eqn]$ via EKF. Compute $[eqn]$ and $[eqn]$ via Equation (31) and cubic spline interpolation. $[eqn]$ Compute $[eqn]$ via Equation (44) by employing the IPOPT solver. Compute the primal residual $[eqn]$ and the dual residual $[eqn]$ . $[eqn]$ $[eqn]$ . $[eqn]$ $[eqn]$ $[eqn]$

4. Simulation Experiments

4.1. Simulation Settings

This section constructs a simulation scenario in which a pedestrian equipped with a single-foot-mounted IMU walks along a closed horizontal square trajectory. The scenario is designed to conduct a comparative performance analysis between the traditional ZUPT-aided EKF algorithm and the proposed ZUPT-aided Birkhoff pseudospectral optimization algorithm.

In the simulation, the pedestrian is assumed to walk along straight-line segments (forming the sides of the square trajectory) on a horizontal plane. The initial position and velocity of the IMU-mounted foot are set as

$$[eqn]$$

the Euler angles are set as

$$[eqn]$$

where $[eqn]$ , $[eqn]$ and $[eqn]$ represent the roll, yaw, and pitch angles, respectively.

In pedestrian navigation, a single gait cycle is divided into the swing phase and the stance phase. This characteristic forms the basis for the application of techniques such as ZUPT. During the stance phase, the foot is typically assumed to be instantaneously stationary relative to the ground, which constitutes the physical foundation for zero-velocity updates. In the $[eqn]$ -th swing phase, the positional displacement of the foot relative to its location at the end of the previous stance phase can be modeled as $[eqn]$ .

$$[eqn]$$ $$[eqn]$$ $$[eqn]$$

where $[eqn]$ represents the single-step stride length, $[eqn]$ and $[eqn]$ denote the duration of the swing phase and the complete gait cycle, respectively, and $[eqn]$ indicates the maximum clearance height of the foot movement. After constructing the position trajectory function of the foot during the swing phase based on these parameters, the theoretical velocity and acceleration in the $[eqn]$ -frame can be obtained by differentiating with respect to time.

Similarly, the foot’s attitude $[eqn]$ during each stance phase can be set as

$$[eqn]$$

where $[eqn]$ denotes the maximum elevation angle during walking. In the stance phase, the position, velocity, and attitude are consistent with the state at the end of the final swing phase. The angular velocity formula can be derived via calculation:

$$[eqn]$$

Figure 2 simulates the trajectory of the pedestrian moving from south to north in the simulation, while Figure 3 presents the three-axis velocity of the pedestrian during the south-to-north movement:

To enable the pedestrian to walk along a square trajectory, a turning maneuver is implemented, which alters only the heading angle without changing the position. If the initial heading at the start of the turn is $[eqn]$ , the attitude of the left foot is given by

$$[eqn]$$

where $[eqn]$ is the turning duration, and $[eqn]$ denotes the start time of the turn. During the turning process, the position remains unchanged, and the velocity is set to zero. The angular velocity during the turn can be calculated as

$$[eqn]$$

4.2. Experimental Results

In the simulation experiment, the pedestrian is set to walk along a square trajectory with a side length of 3 $[eqn]$ , completing three laps before returning to the starting point and coming to a stop. The simulation parameters are listed in Table 1. The IMU sampling frequency is set to 200 Hz. Assuming an initial heading angle of zero, the pedestrian makes a 90° left turn at each vertex of the square. The total travel distance is 46.8 m, and the total duration is 45.1 s. The reference trajectory within the horizontal plane is shown in Figure 4a.

This study compares the proposed Birkhoff pseudospectral method with two established filtering baselines: the EKF and the Second-Order Extended Kalman Filter (EKF2). The EKF2 serves as a more rigorous benchmark for pedestrian navigation with ZUPT. Unlike the standard EKF, the EKF2 operates on the error state, not the full navigation state. This framework avoids singularities in attitude representation, reduces linearization errors, and improves numerical stability [32]. This stability is especially critical in foot-mounted IMU applications, where frequent stance-phase ZUPT corrections are applied.

For fair comparison, all algorithms adopt a unified attitude input, specifically the attitude estimated by the EKF. The EKF2 only observes velocity states, with Rauch–Tung-Striebel (RTS) smoothing integrated. As a classic post-processing global optimization method for recursive filters, RTS smoothing is employed herein to optimize the EKF2-estimated trajectory by utilizing full-time-domain measurement information [33]. This enables a valid performance comparison between the EKF2 and the Birkhoff pseudospectral method in the global optimization framework.

To evaluate the trajectory estimation accuracy of the Birkhoff pseudospectral method under noise interference, white noise with different power spectral densities was added to the accelerometer of the IMU in the simulation. Under given attitude error conditions, the performance of the Birkhoff pseudospectral method with different numbers of collocation points, the EKF algorithm, and the EKF2 algorithm in position estimation was compared, focusing on the final position error and the root mean square error (RMSE) over the entire trajectory. Table 2 summarizes the comparison results of the position RMSE and final position error for the three algorithms under different numbers of collocation points and different noise levels.

Under identical attitude error conditions, the RMSE and final position error of the EKF algorithm are approximately 10 times greater than those of the Birkhoff pseudospectral method. Meanwhile, the EKF2 still yields RMSE and final position error values that are roughly 3–4 times higher than the corresponding results of the Birkhoff method. Even in scenarios with an accelerometer noise intensity as high as $[eqn]$ , the Birkhoff pseudospectral method can still maintain reliable positioning accuracy.

This indicates that the state estimation accuracy of the Birkhoff pseudospectral method under noisy measurement conditions is significantly superior to that of the EKF algorithm and EKF2 algorithm. This advantage stems from the global optimization nature of the pseudospectral method: by discretizing the entire time domain with collocation points and employing orthogonal polynomial approximations, it transforms the dynamic differential constraints into algebraic constraints. Consequently, when solving the NLP problem, it can effectively smooth out error fluctuations induced by noise. This principle inherently avoids the problem of error accumulation over time, which is common in recursive filters like the EKF.

This advantage of the Birkhoff pseudospectral method stems from its inherent full-time-domain global optimization property. The algorithm discretizes the entire time domain via collocation points and converts dynamic differential constraints into algebraic constraints with orthogonal polynomial approximation. It can thus effectively smooth out noise-induced error fluctuations when solving NLP problems. This approach fundamentally avoids the time-dependent error accumulation issue of the EKF2-RTS combined algorithm. The EKF2-RTS still relies on step-by-step recursive estimation for state calculation in the filtering stage, and RTS smoothing only performs post-processing correction on the recursively obtained state sequence, rather than reconstructing the trajectory from the source.

To investigate the impact of the number of collocation points on algorithm performance, a comparative experiment was conducted with different collocation-point settings. Theoretically, increasing the number of collocation points helps improve discretization accuracy; however, in practical, noisy scenarios, an excessive number of points may lead to overfitting. The experimental results show that even with a relatively low number of collocation points, the Birkhoff pseudospectral method still achieves significantly higher estimation accuracy than the EKF and the EKF2. Furthermore, the RMSE decreases with the increase of $[eqn]$ , indicating that insufficient collocation points introduce truncation errors in approximating highly dynamic motions. Therefore, a suitable number of collocation points should be chosen by balancing positioning accuracy and computational cost in practical applications. Nevertheless, the proposed method maintains stable and superior estimation performance over a wide range of collocation points. It exhibits strong numerical robustness and engineering practicality. Figure 4b presents a comparison of the planar trajectories generated by the Birkhoff pseudospectral method with $[eqn]$ collocation points, the EKF algorithm, and the EKF2 algorithm, under an accelerometer noise intensity of $[eqn]$ .

5. Real-World Experiments

To comprehensively evaluate the real-world performance of the proposed algorithm, this section conducts experiments in both a controlled indoor environment and an open outdoor scenario. The proposed method is compared with the EKF algorithm and the EKF2 algorithm. All comparisons are based on the same sensor data, attitude information, and ZUPT detection results to ensure fairness, with a focus on evaluating the algorithms’ performance in terms of position estimation accuracy and accumulated travel error.

In the experiments, a commercial IMU (MTi-G-710 from Xsens, Enschede, Netherlands) was securely attached to the tester’s foot. Based on the experience gained from the preliminary simulations, the number of collocation points for the Birkhoff pseudospectral method was uniformly set to 60 to prevent inadequate approximation of the motion states due to an insufficient number of points.

Indoor Experiment: The tester walked along a rectangular closed path with a side length of 9.6 m. The actual scene is shown in Figure 5a. This environment, effectively shielded from satellite signals, was used to validate the algorithm’s pure inertial navigation performance.

Outdoor Experiment: The tester walked one complete lap along the innermost lane of a standard 400-m-perimeter athletics track. GPS data was collected simultaneously to serve as the reference trajectory for evaluating positioning accuracy. The actual movement trajectory under GPS reference is shown in Figure 6a.

Figure 5b and Figure 6b present the estimated planar motion trajectories generated by the three algorithms in the indoor and outdoor environments, respectively. Visually, the trajectory produced by the proposed algorithm aligns more closely with the reference path, exhibiting significantly smoother motion and less divergence compared to the EKF results and the EKF2. Notably, Figure 7 overlays the outdoor estimated trajectory onto a satellite map, providing further evidence of the proposed algorithm’s advantage in mitigating cumulative drift and enhancing the global consistency of the estimated path.

For precise quantitative comparison, Table 3 summarizes multiple error metrics from the indoor and outdoor experiments, including the start-point error, final position error, absolute distance error, and relative distance error.

In both indoor and outdoor experiments, the proposed algorithm outperforms both the EKF and EKF2.

It significantly reduces the final position error and relative distance error compared with the EKF, with reduction rates of approximately 70% and 90% in indoor and outdoor scenarios, respectively.

Compared with the EKF2, the proposed method achieves a comparable final position error in indoor tests and reduces the relative distance error by about 20% compared with the EKF2. In the outdoor scenario, it decreases the relative distance error by roughly 70% and the final position error by approximately 15%.

These experimental results demonstrate that the proposed Birkhoff pseudospectral optimization algorithm effectively utilizes global gait cycle information and suppresses the cumulative noise errors of MEMS-IMU. It outperforms both the traditional ZUPT-aided EKF and the advanced EKF2 in trajectory smoothness, distance estimation accuracy, and final position precision.

6. Conclusions

This paper proposes a trajectory optimization method for pedestrian inertial navigation based on the Birkhoff pseudospectral method. Drawing on initial navigation states from ZUPT and EKF, the method discretizes the state trajectory via Chebyshev collocation points and solves a global optimization problem using the Birkhoff pseudospectral method.

To validate the effectiveness of the proposed algorithm and its applicability to pedestrian navigation, both simulation and physical experiments are designed and conducted. The experimental results demonstrate that the proposed algorithm outperforms both the traditional ZUPT-aided EKF and the advanced EKF2, achieving significant reductions in final position error and relative distance error. Specifically, the relative distance error is reduced by approximately 70% and 90% compared with the EKF in indoor and outdoor scenarios, respectively, and clear accuracy improvements are also obtained over the EKF2.

Theoretically, the results originate from the time-domain global optimization property of the Birkhoff pseudospectral method. Different from the recursive estimation of EKF/EKF2 that tends to induce error accumulation, the proposed method performs centralized optimization on short-duration pedestrian trajectories. It corrects local deviations caused by IMU noise from a global perspective and effectively suppresses noise interference in navigation within a short time.

Future work will explore integrating this method with other advanced estimation frameworks, such as factor graph optimization, to further enhance the performance of pedestrian navigation systems in complex environments.