A Novel Kalman Filter with State Constraint Approach for the Integration of Multiple Pedestrian Navigation Systems

Abstract

Numerous solutions/methods to solve the existing problems of pedestrian navigation/localization have been proposed in the last decade by both industrial and academic researchers. However, to date there are still major challenges for a single pedestrian navigation system (PNS) to operate continuously, robustly, and seamlessly in all indoor and outdoor environments. In this paper, a novel method for pedestrian navigation approach to fuse the information from two separate PNSs is proposed. When both systems are used at the same time by a specific user, a nonlinear inequality constraint between the two systems’ navigation estimates always exists. Through exploring this constraint information, a novel filtering technique named Kalman filter with state constraint is used to diminish the positioning errors of both systems. The proposed method was tested by fusing the navigation information from two different PNSs, one is the foot-mounted inertial navigation system (INS) mechanization-based system, the other PNS is a navigation device that is mounted on the user’s upper body, and adopting the pedestrian dead reckoning (PDR) mechanization for navigation update. Monte Carlo simulations and real field experiments show that the proposed method for the integration of multiple PNSs could improve each PNS’ navigation performance.

1. Introduction

The last few years have witnessed major progress in the development of pedestrian navigation systems (PNS). Common users of PNS mainly include the following:

• First responders (e.g., emergency search and rescue personnel, police and military forces).
• Recreational users (e.g., self-guided tourists, hikers, athletic trainers).
• Other personnel (e.g., elderly people, visually impaired).

Micro-Electro-Mechanical System (MEMS) inertial sensors, miniature Global Navigation Satellite System (GNSS) receivers and wireless transmitters, Radio Frequency Identification Devices (RFID) chips, Bluetooth devices, and so on, are now widely used in PNS. MEMS inertial sensors, including accelerometers, gyroscopes, magnetometers, and barometers, are usually called self-contained sensors or inertial measurement units (IMU). Currently, most of the sensors mentioned above can be easily found in various consumer products, especially in the well-developed market for smart devices such as smart phones, smart watches, smart glasses, tablet computers, etc. Based on these sensors and devices, numerous researchers have been dedicated to PNS research. However, to date there are still major challenges for PNS to continuously, robustly, and seamlessly operate in all indoor and outdoor environments.

2. Literature Review and Problem Description

Most of the proposed techniques in recently published literature for PNS is concentrated in one of the following three groups:

• Radio-navigation systems (GNSS, Cellular Networks).
• Indoor infrastructure-based systems (RFID, Wi-Fi, Bluetooth, UWB (Ultra-wideband)).
• MEMS sensor-based systems (inertial navigation system (INS), pedestrian dead reckoning (PDR)).

2.1. The Radio-Navigation Systems

GNSS is the primary source of information to provide the position of a PNS. However, GNSS can provide position information only under ideal conditions which require open environments (i.e., open space areas). In other words, the system works sporadically in urban and canopy areas due to signal blockage and attenuation [1]. Cellular network-based PNS were firstly described in [2], however, according to [3], in signal degraded areas such a system could suffer from poor accuracy and very low vertical resolution. This is a serious problem as in some extreme applications (e.g., indoor rescue), the location of the user on specific floors or in specific rooms is of great importance [4].

2.2. The Indoor Infrastructure-Based Systems

Numerous solutions to pedestrian navigation in indoor environments have been suggested in related literature. One effective way to meet this requirement is to use a RFID-based system [5]. In such an operation, the RFID tags should be installed beforehand at known locations; also, a device containing a RFID reader and an antenna should be carried by the user. When the user passes by a tag, the reader could then retrieve its location. However, such service is not always useful and applicable when a seamless location over large areas is required. Similarly, the Wi-Fi-, Bluetooth-, and UWB-based navigation methods could also provide efficient indoor navigation services [6]. However, the main reason for not always employing these methods for PNS is the obvious requirement of building dependent infrastructures.

2.3. The Micro-Electro-Mechanical System (MEMS) Sensor Based Systems

For MEMS sensor-based systems, due to the fact that MEMS sensors still have high noise ratio, the use of unaided Inertial Navigation System (INS) mechanization for PNS is very limited [7]. Considering the unique features of human walking steps, an effective approach to restrict the INS derived navigation errors is to mount the MEMS sensors on a foot of a user [8]. In such a mode of operation, when stance phases of the foot are periodically detected, the Zero Velocity Update (ZUPT) technique will be triggered to estimate and correct the INS navigation solution errors, as well as the inertial sensors’ inherent biases using a Kalman Filter (KF). Although the INS/ZUPT method could sometimes produce a relatively good navigation solution, however, due to the low observability of the heading error from the ZUPT, as well as the relatively large deterministic and random biases of the MEMS inertial sensors [9], and in cases of running scenarios, zero velocity intervals cannot be easily detected; thus, the deviations of navigation solutions continue to grow with time.

The other MEMS sensor-based approach is the Pedestrian Dead Reckoning (PDR) mechanization which also exploits the kinematics of human walking [10,11]. The main idea of PDR mechanization is to use accelerometer measurements to detect the user’s walking steps, estimate step length and propagate the user’s position using a measured azimuth between every two consecutive steps [12]. To data, various methods of step detection and step length calculation have already reached practical levels of accuracy whether the user walks in low-speed or runs in high-speed; thus, only the long-term azimuth accuracy remains challenging [13,14,15]. Moreover, it is noteworthy that the misalignment of the MEMS IMU with respect to a user’s body needs to be known [16,17]. Therefore, the PDR-based PNS is better rigidly mounted on the user’s upper body (e.g., chest and waist) since the misalignment can be almost unchangeable [18]. In addition, the user’s upper body does not block the GNSS signals, and the integration of PDR with a GNSS receiver could provide a PNS with superior performance in comparison with either stand-alone GNSS receivers or PDRs. GNSS-derived positions are excellent external measurements for updating the PDR, thus improving the PDR’s long-term accuracy. Similarly, the PDR can provide precise position solutions for GNSS signal reacquisition after outages in a short period of time. Moreover, it is noteworthy that why we prefer to choose this PDR/GNSS approach for the upper body mounted PNS instead of the INS/GNSS approach is that during GNSS outage periods, the INS positional errors will typically drift as a function of the cubical time, due to gyro drift. For instance, during 5 min of GNSS outage, the position error of the INS (e.g., the MEMS IMU embedded in the Samsung Note 2) could diverge by hundreds of kilometers [19].

2.4. The Multiple Systems Integration

From the above literature review, we can conclude that there is no navigation technology that, on its own, can provide a reliable, robust, and infrastructure-free solution to the problem of positioning a pedestrian in all kinds of indoor environments. Only a navigation system that acts in cooperation with a multitude of navigation technologies with complementary properties has the potential to fully solve pedestrian navigation problems. Because navigation technologies with complementary properties are generally based upon different physical phenomena, the most favorable location of the different sub navigation systems on the body of the user also differs. For example, GNSS-/Wi-Fi-/Bluetooth-/UWB-based systems are all best mounted or held on the upper body of the user because then it is least likely that other body parts will block the external signals; for using the INS mechanization-based PNS, the IMU is best mounted on a foot of a user, since the foot could then periodically become stationary whenever the user walks or runs; meanwhile for PDR-based PNS, it is best mounted on the user’s upper body, so that the user’s body does not block the GNSS signals, as well, the misalignment of the IMU with respect to the user’s body can be kept almost unchangeable, so if all navigation systems are located on different points of the user’s body, they will track the position of different points of the user’s body. Hence, there is a question about how to combine and make full use of these navigation solutions at the same time to further improve the accuracies of each sub-system. Also, considering the fact that in safety-related applications, it is imprudent to depend on a single navigation technique. Consequently, the concept of combining complementary navigation systems should be accepted for commercial applications.

Several forms of architecture have been proposed for the integration of multiple systems based PNS. Brand and Phillips [20] introduced the use of two MEMS IMUs for PNS fastened to each foot. The relative position updates between them are used to further improve both systems’ navigation results. Their method set up a local radio frequency (RF) infrastructure which uses RF phase changes between a reference signal located in a waist pack and the one coming from a transmitter located on each boot. Finally, a series of foot-to-foot range measurements can be updated. A drawback of this method is the use of RF infrastructure which is not available in most consumer devices. Bancroft [21] also placed two IMUs placed on the feet with the centralized KF method. The relative time-varying distances between them were regarded as a constant, which means the errors between the real distance and the assumed constant were ignored. Skog et al. [22] derived an inequality constraint to accurately describe the relative time-varying distances between the two foot-mounted IMUs. In addition, different from the centralized KF approach, they first introduced the idea of the state constraint KF into this problem, and changed the dual IMUs’ information-fusing problem into a weighted least squares problem. Laverne et al. [23] have also experimented with utilizing dual foot-mounted IMUs. Extremely high navigation accuracy has been reported by them, where ultra-sonic ranging was performed between two high-end IMUs (HG1930, Honeywell, Phoenix, AZ, USA) with gyro bias drifts only a few degrees per hour). Also, they announced that foot-to-foot range improved the heading estimate dramatically. However, these high-end IMUs are too bulky and expensive for regular pedestrian use, but the results indicate that high accuracy could also be achieved in future mass-market systems when small MEMS IMUs have improved in accuracy. Zampella et al. [24] proposed a new indoor PNS prototype which fuses the navigation results of a UWB-based system and a foot INS/ZUPT system through a probability density function (PDF) truncation approach, which requires a lot of calculations to approximate the truncated PDF mean and covariance. Also, this method does not consider the UWB non-line-of-sight conditions, namely when there exists the UWB signal outages.

In this research, we investigate the integration of multiple PNSs with constraints between them and propose a novel information fusion architecture that can be used to further improve the navigation accuracies of each PNS. In our mode of operation, if multiple PNSs are used at the same time by a user, the relative positions between any two PNSs are not fixed. However, there should be an upper bound on how far apart the two PNSs can be, so that the navigation solutions of the two systems together can compose a combined navigation solution which will explore the maximum spatial separation information of the two systems. The fusion of the two systems’ navigation solutions can be regarded as a KF with a state constraint problem. This requires the modifications of the two systems’ error state KF to exploit this additional information in order to estimate the optimal navigation solutions based on the state constrained KF algorithm. This method was first proposed by Simon and Chia [25], which considers a linear system and measurements describing movement of a land-based vehicle.

The remainder of this paper is organized as follows: a brief description of the state constrained KF is provided in Section 3. The INS/ZUPT and PDR/GNSS systems are then respectively discussed in Section 4 and Section 5, followed by the state constrained KF approach on the combined PNSs in Section 6. In Section 7, the performance of the new approach is analyzed through both simulation and the operational field experiments.

3. State Constraint Kalman Filter (KF)

This section first provides a brief summary of the unconstrained KF state estimate. Then the state constrained KF is shown as well as the method to solve the constrained KF problem. Finally, the solutions to four different constraint scenarios are separately presented.

3.1. State Unconstrained KF

Assuming $\mathbf{x}$ is a random vector with the dimension n, and has a nonsingular covariance matrix, $\mathbf{P}$. We say that $\mathbf{x}$ is with Gaussian distribution only if its probability density function (PDF), $\mathrm{p}(\mathbf{x})$, satisfies:

$$\mathrm{p}(\mathbf{x})=\frac{1}{{(2\mathrm{\pi })}^{\mathrm{n}/\mathrm{2}}}\mathrm{\hspace{0.17em}}\frac{1}{|\mathbf{P}{|}^{1/2}}\mathrm{\hspace{0.17em}}\exp [-\frac{1}{2}{(\mathbf{x}-\mathbf{m})}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}(\mathbf{x}-\mathbf{m})]$$

(1)

in which $\mathbf{m}=\mathrm{E}[\mathbf{x}]$, $\mathbf{P}=\mathrm{E}[(\mathbf{x}-\mathbf{m}){(\mathbf{x}-\mathbf{m})}^{\mathrm{T}}]$, $\mathrm{E}[\cdot ]$ is the expectation operator. For the standard linear KF system model:

$$\{\begin{array}{l}{\mathbf{x}}_{\mathrm{k}}=\mathbf{F\hspace{0.17em}}{\mathbf{x}}_{\mathrm{k}-1}+{\mathbf{w}}_{\mathrm{k}-1}\\ {\mathbf{z}}_{\mathrm{k}}=\mathbf{H}\mathrm{\hspace{0.17em}}{\mathbf{x}}_{\mathrm{k}}+{\mathbf{v}}_{\mathrm{k}}\end{array}$$

(2)

${\mathbf{w}}_{\mathrm{k}-1}$ and ${\mathbf{v}}_{\mathrm{k}}$ are jointly Gaussian. When measurement ${\mathbf{z}}_{\mathrm{k}}$ is given, through the application of KF, we could get the optimal estimate, ${\widehat{\mathbf{x}}}_{\mathrm{k}}$, i.e., the minimum variance estimate. Referring [26], ${\widehat{\mathbf{x}}}_{\mathrm{k}}$ is given by:

$${\widehat{\mathbf{x}}}_{\mathrm{k}}=\mathrm{E}[{\mathbf{x}}_{\mathrm{k}}|{\mathbf{z}}_{\mathrm{k}}]={\mathbf{m}}_{\mathrm{k}}+{\mathbf{P}}_{\mathrm{k}}^{\mathrm{x}\mathrm{z}}\mathrm{\hspace{0.17em}}{\mathrm{(}{\mathbf{P}}_{\mathrm{k}}^{\mathrm{z}\mathrm{z}})}^{-1}\mathrm{\hspace{0.17em}}({\mathbf{z}}_{\mathrm{k}}-{\overline{\mathbf{z}}}_{\mathrm{k}})$$

(3)

where ${\mathbf{m}}_{\mathrm{k}}=\mathrm{E}[{\mathbf{x}}_{\mathrm{k}}]$, ${\mathbf{P}}_{\mathrm{k}}^{\mathrm{x}\mathrm{z}}$ is the covariance matrix of ${\mathbf{x}}_{\mathrm{k}}$ and ${\mathbf{z}}_{\mathrm{k}}$, ${\mathbf{P}}_{\mathrm{k}}^{\mathrm{z}\mathrm{z}}$ is the covariance matrix of ${\mathbf{z}}_{\mathrm{k}}$. As ${\widehat{\mathbf{x}}}_{\mathrm{k}}$ is conditionally Gaussian given ${\mathbf{z}}_{\mathrm{k}}$, so we say ${\widehat{\mathbf{x}}}_{\mathrm{k}}$ and ${\mathbf{z}}_{\mathrm{k}}$ are jointly Gaussian. Similar to Equation (1), $\mathrm{p}(\mathbf{x}|\mathbf{z})$ is the conditional PDF of $\mathbf{x}$ given $\mathbf{z}$:

$$\mathrm{p}(\mathbf{x}|\mathbf{z})=\frac{1}{{(2\mathrm{\pi })}^{\mathrm{n}/\mathrm{2}}}\mathrm{\hspace{0.17em}}\frac{1}{|\mathbf{P}{|}^{1/2}}\mathrm{\hspace{0.17em}}\exp [-\frac{1}{2}{(\mathbf{x}-\widehat{\mathbf{x}})}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}(\mathbf{x}-\widehat{\mathbf{x}})]$$

(4)

where $\mathbf{P}$ is the covariance matrix of the KF estimate, which can be calculated as:

$$\mathbf{P}={\mathbf{P}}^{\mathrm{xx}}-{\mathbf{P}}^{\mathrm{x}\mathrm{z}}\mathrm{\hspace{0.17em}}{\mathrm{(}{\mathbf{P}}^{\mathrm{z}\mathrm{z}})}^{-1}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{\mathrm{z}\mathrm{x}}$$

(5)

and the KF estimate $\widehat{\mathbf{x}}$ is the calculation of $\mathbf{x}$ that maximizes $\text{p}(\mathbf{x}|\mathbf{z})$.

3.2. State Constrained KF

Assuming that the state $\mathbf{x}$ of the linear system model given Equation (2) satisfies the equality constraint:

$$\mathrm{\Gamma }(\mathbf{x})=\mathbf{d}$$

(6)

or the inequality constraint,

$$\mathrm{\Gamma }(\mathbf{x})\le \mathbf{d}$$

(7)

where $\mathrm{\Gamma }(\cdot )$ is a known linear or nonlinear operator for $\mathbf{x}$, and $\mathbf{d}$ is a known vector. After normal KF process, if the state does not satisfy the above given constraints, we need to find a new state estimate, $\tilde{\mathbf{x}}$.

3.3. Maximum Probability Algorithm

Obtaining the new constrained state can be solved through the maximum probability algorithm [25]. We know that the KF estimate is the value of $\mathbf{x}$ that maximizes $\mathrm{p}(\mathbf{x}|\mathbf{z})$ in Equation (4). Similarly, the constrained state can be derived by calculating the new estimate, $\tilde{\mathbf{x}}$, so that $\mathrm{p}(\mathbf{x}|\mathbf{z})$ is further maximized and $\tilde{\mathbf{x}}$ satisfies the constraints. Maximizing $\mathrm{p}(\mathbf{x}|\mathbf{z})$ is the same as maximizing its natural logarithm:

$$\begin{array}{l}\tilde{\mathbf{x}}=\underset{\tilde{\mathbf{x}}}{\max }\mathrm{\hspace{0.17em}p}(\tilde{\mathbf{x}}|\mathbf{z})\\ \mathrm{\hspace{0.17em}\hspace{0.17em}}=\underset{\tilde{\mathbf{x}}}{\max }\mathrm{\{ln[}\mathrm{p}(\tilde{\mathbf{x}}|\mathbf{z})]\}\\ \mathrm{\hspace{0.17em}\hspace{0.17em}}=\underset{\tilde{\mathbf{x}}}{\max }\left\{-\frac{{(\tilde{\mathbf{x}}-\widehat{\mathbf{x}})}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}(\tilde{\mathbf{x}}-\widehat{\mathbf{x}})}{2}\right\}\ln \left\{\frac{1}{{(2\mathbf{\pi })}^{\mathbf{n}/\mathrm{2}}}\frac{1}{|\mathbf{P}{|}^{1/2}}\right\}\\ \mathrm{\hspace{0.17em}\hspace{0.17em}}=\underset{\tilde{\mathbf{x}}}{\min }\mathrm{[}{(\tilde{\mathbf{x}}-\widehat{\mathbf{x}})}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}(\tilde{\mathbf{x}}-\widehat{\mathbf{x}})]\\ \mathrm{\hspace{0.17em}\hspace{0.17em}}=\underset{\tilde{\mathbf{x}}}{\min }[{\tilde{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}-2\mathrm{\hspace{0.17em}}{\widehat{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}+{\widehat{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\widehat{\mathbf{x}}]\end{array}$$

(8)

in which $\widehat{\mathbf{x}}$ is the KF state estimate, thus ${\widehat{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\widehat{\mathbf{x}}$ is a known component before applying the constraint. The constrained estimate $\tilde{\mathbf{x}}$ can therefore be calculated using the following Equations (9) and (10):

$$\tilde{\mathbf{x}}=\underset{\tilde{\mathbf{x}}}{\min }\mathrm{[}{\tilde{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}-2\mathrm{\hspace{0.17em}}{\widehat{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}]$$

(9)

$$\mathrm{s}.\mathrm{t}.\mathrm{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}}\mathrm{\Gamma }(\tilde{\mathbf{x}})=\mathbf{d}\mathrm{\hspace{0.17em}\hspace{0.17em}}\mathrm{o}\mathrm{r}\mathrm{\hspace{0.17em}\hspace{0.17em}}\mathrm{\Gamma }(\tilde{\mathbf{x}})\le \mathbf{d}$$

(10)

3.4. The Solutions to the State Constrained KF

The constraints given in Equations (9) and (10) can be divided into four different scenarios. If the operator $\mathrm{\Gamma }(\cdot )$ is linear, namely, $\mathrm{\Gamma }(\tilde{\mathbf{x}})=\mathbf{\kappa }\tilde{\mathbf{x}}$, we then have the linear equality constraint, $\mathbf{\kappa }\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}=\mathbf{d}$, or the linear inequality constraint, $\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{x}\le \mathbf{d}$. Worth noting is that $\mathbf{\kappa }$ is a known constant $\mathrm{r}\times \mathrm{n}$ matrix with full rank $\mathrm{r}$, and $\mathbf{d}$ is a known $\mathrm{r}\times 1$ vector, namely the number of rows in $\mathbf{\kappa }$ is the number of constraints. If $\mathbf{\kappa }$ is a square matrix, the state is fully constrained, but $\mathbf{\kappa }$ is not full rank which means we have redundant state constraints, at this time we need to remove the linearly dependent rows from $\mathbf{\kappa }$ until $\mathbf{\kappa }$ is full rank. Similarly, if $\mathrm{\Gamma }(\cdot )$ is a non-linear operator, we assume $\mathrm{\Gamma }(\tilde{\mathbf{x}})=\mathrm{\eta }(\tilde{\mathbf{x}})$, in which $\mathrm{\eta }(\tilde{\mathbf{x}})$ is the nonlinear expression of $\tilde{\mathbf{x}}$. We then have the non-linear equality constraint, $\mathrm{\eta }(\tilde{\mathbf{x}})=\mathbf{d}$, or the nonlinear inequality constraint, $\mathrm{\eta }(\tilde{\mathbf{x}})\le \mathbf{d}$. The solutions to the four different state constrained KF scenarios are separately given as follows.

3.4.1. The State Linear Equality Constrained Solution

According to Equations (9) and (10), the problem we want to solve here can be written as:

$$\tilde{\mathbf{x}}=\underset{\tilde{\mathbf{x}}}{\min }\mathrm{[}{\tilde{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}-2\mathrm{\hspace{0.17em}}{\widehat{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}]$$

(11)

$$\mathrm{s}.\mathrm{t}.\mathbf{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\kappa \hspace{0.17em}}\tilde{\mathbf{x}}=\mathbf{d}$$

(12)

To get the solution $\tilde{\mathbf{x}}$, we form the Lagrange function [25]:

$$\mathbf{L}(\tilde{\mathbf{x}},\mathbf{\hspace{0.17em}\lambda })={\tilde{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}-2\mathrm{\hspace{0.17em}}{\widehat{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}+{\mathbf{\lambda }}^{\mathrm{T}}\mathrm{\hspace{0.17em}}(\mathbf{2\hspace{0.17em}\kappa \hspace{0.17em}}\tilde{\mathbf{x}}-\mathbf{2\hspace{0.17em}d})$$

(13)

For the convenience of calculation, the constraint of Equation (12) was written as $2\mathbf{\hspace{0.17em}\kappa \hspace{0.17em}}\tilde{\mathbf{x}}=2\mathbf{\hspace{0.17em}d}$. The first order conditions for the minimum of Equation (11) are:

$$\{\begin{array}{l}\frac{\partial (\tilde{\mathbf{x}},\mathrm{\hspace{0.17em}}\mathbf{\lambda })}{\partial \tilde{\mathbf{x}}}=2\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}(\tilde{\mathbf{x}}-\widehat{\mathbf{x}})-2\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}}\mathrm{\hspace{0.17em}}\mathbf{\lambda }=0\\ \frac{\partial (\tilde{\mathbf{x}},\mathrm{\hspace{0.17em}}\mathbf{\lambda })}{\partial \mathbf{\lambda }}=\mathbf{\kappa }\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}-\mathbf{d}=0\end{array}$$

(14)

Then the Equation (14) gives the following solutions $(\mathbf{\lambda },\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}})$:

$$\{\begin{array}{l}\mathbf{\lambda }={(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{P}\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}})}^{-1}\mathrm{\hspace{0.17em}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\widehat{\mathbf{x}}-\mathbf{d})\\ \tilde{\mathbf{x}}=\widehat{\mathbf{x}}-\mathbf{P}\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{P}\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}})}^{-1}\mathrm{\hspace{0.17em}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\widehat{\mathbf{x}}-\mathbf{d})\end{array}$$

(15)

After we get the new constrained state, $\tilde{\mathbf{x}}$, we also need to update the new state error covariance, ${\mathbf{P}}^{*}$. From Equation (15), we have:

$$\begin{array}{cc}\hfill \mathbf{x}-\tilde{\mathbf{x}}& =\mathbf{x}-\widehat{\mathbf{x}}+\mathbf{P}\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{(\mathbf{\kappa \hspace{0.17em}}\mathbf{p\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}})}^{-1}\mathrm{\hspace{0.17em}}[\mathbf{\kappa }\mathrm{\hspace{0.17em}}\widehat{\mathbf{x}}-\mathbf{d}-(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{x}-\mathbf{d})]\hfill \\ \mathrm{\hspace{0.17em}\hspace{0.17em}}& =[\mathrm{{\rm I}}-\mathbf{P}\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{(\mathbf{\kappa \hspace{0.17em}}\mathbf{p\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}})}^{-1}\mathrm{\hspace{0.17em}}\mathbf{\kappa }]\mathrm{\hspace{0.17em}}(\mathbf{x}-\widehat{\mathbf{x}})\hfill \\ \mathrm{\hspace{0.17em}\hspace{0.17em}}& =(\mathrm{{\rm I}}-\mathbf{\mu })\mathrm{\hspace{0.17em}}(\mathbf{x}-\widehat{\mathbf{x}})\hfill \end{array}$$

(16)

in which $\mathbf{P\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{P}\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}})}^{-1}\mathbf{\hspace{0.17em}\kappa }$ is denoted by $\mathbf{\mu }$, and $\mathbf{x}$ is the true state. Then the constrained new state error covariance, ${\mathbf{P}}^{*}$, can be expressed as:

$$\begin{array}{l}{\mathbf{P}}^{*}=\mathrm{E}(\mathbf{x}-\tilde{\mathbf{x}})\mathrm{\hspace{0.17em}}{(\mathbf{x}-\tilde{\mathbf{x}})}^{\mathrm{T}}]\\ \mathrm{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}}=\mathrm{E}\{[(\mathrm{{\rm I}}-\mathbf{\mu })\mathrm{\hspace{0.17em}}(\mathbf{x}-\widehat{\mathbf{x}})]\mathrm{\hspace{0.17em}}{[(\mathrm{{\rm I}}-\mathbf{\mu })\mathrm{\hspace{0.17em}}(\mathbf{x}-\widehat{\mathbf{x}})]}^{\mathrm{T}}\}\\ \mathrm{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}}=(\mathrm{{\rm I}}-\mathbf{\mu })\mathrm{\hspace{0.17em}}\mathbf{P}\mathrm{\hspace{0.17em}}{(\mathrm{{\rm I}}-\mathbf{\mu })}^{\mathrm{T}}\end{array}$$

(17)

3.4.2. The State Linear Inequality Constrained Solution

Assuming that the KF state satisfies the inequality constraint, the constrained state, $\tilde{\mathbf{x}}$, can be derived as:

$$\tilde{\mathbf{x}}=\underset{\tilde{\mathbf{x}}}{\min }({\tilde{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}-2\mathrm{\hspace{0.17em}}{\widehat{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}})$$

(18)

$$\mathrm{s}.\mathrm{t}.\mathbf{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\kappa \hspace{0.17em}}\tilde{\mathbf{x}}\le \mathbf{d}$$

(19)

The above problem is known as a Quadratic Programming (QP) of the mathematical optimization [27]. QP problems can be solved using the active set algorithm, which reduces the number of constraints to those which are active within the solution to the QP problem. The basic strategy of active set algorithm is that first we make an explicit estimate of the active set, and then take a step toward the solution using a simplified QP problem in which the constraints in this estimated active set are satisfied as equalities, a brief explanation to this algorithm can be seen in [27] (pp. 240–245). For instance, we take into consideration the Equations (18) and (19) as an example of the problem. Supposing we have s inequality constraints, q of the s inequality constraints are active at the solution of Equation (18), denoted by ${\mathbf{\kappa }}^{*}$. The q rows of $\mathbf{\kappa }$ that correspond to the active constraints, are denoted by ${\mathbf{d}}^{*}$, and the q components of $\mathbf{d}$ correspond to the active constraints. If the correct set of active constraints is known a priori then the solution to the problem given in Equations (18) and (19) is also the solution to the equality constraint problem:

$$\tilde{\mathbf{x}}=\underset{\tilde{\mathbf{x}}}{\min }({\tilde{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}-2\mathrm{\hspace{0.17em}}{\widehat{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}})$$

(20)

$$\mathrm{s}.\mathrm{t}.\mathrm{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}}{\mathbf{\kappa }}^{*}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}={\mathbf{d}}^{*}$$

(21)

So it is demonstrated that the inequality constraint problem of Equations (18) and (19) is equivalent to the equality constraint problem only and if only we know the correct a priori active sets, at this time the solution-solving method given in Section 3.4.1 also applies to the state inequality constraint problem.

3.4.3. The State Non-Linear Equality Constrained Solution

The problem we want to solve at this time can be expressed as:

$$\tilde{\mathbf{x}}=\underset{\tilde{\mathbf{x}}}{\min }({\tilde{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}-2\mathrm{\hspace{0.17em}}{\widehat{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}})$$

(22)

$$\mathrm{s}.\mathrm{t}.\mathrm{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}}\mathrm{\eta }(\tilde{\mathbf{x}})=\mathbf{d}$$

(23)

in which $\mathrm{\eta }(\tilde{\mathbf{x}})$ is the non-linear expression of $\tilde{\mathbf{x}}$. We suppose that the linear model in Equation (2) has been estimated by the KF procedure, and yields an unconstrained estimate $\widehat{\mathbf{x}}$. Assuming that $\widehat{\mathbf{x}}$ is “close” to the constrained estimate $\tilde{\mathbf{x}}$, at this time the non-linear constraint problem can be linearized (Taylor’s expansion) around the current unconstrained state estimate, $\widehat{\mathbf{x}}$:

$$\mathrm{\eta }(\tilde{\mathbf{x}})-\mathbf{d}=\mathrm{\eta }(\widehat{\mathbf{x}})+{[{\mathrm{\eta }}^{\prime }(\widehat{\mathbf{x}})]}^{\mathrm{T}}\mathrm{\hspace{0.17em}}(\tilde{\mathbf{x}}-\widehat{\mathbf{x}})+\frac{1}{2}{(\tilde{\mathbf{x}}-\widehat{\mathbf{x}})}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathrm{\eta }}^{″}(\widehat{\mathbf{x}})\mathrm{\hspace{0.17em}}(\tilde{\mathbf{x}}-\widehat{\mathbf{x}})+\cdot \cdot \cdot -\mathbf{d}$$

(24)

in which ${\mathrm{\eta }}^{\prime }(\widehat{\mathbf{x}})$ and ${\mathrm{\eta }}^{″}(\widehat{\mathbf{x}})$ denote the first and second partial derivatives of $\mathrm{\eta }(\mathbf{x})$ with respect to $\mathbf{x}$, replacing $\mathbf{x}$ with $\widehat{\mathbf{x}}$. Neglecting the second-order and higher terms yields:

$${\mathrm{\eta }}^{\prime }(\widehat{\mathbf{x}})\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}\approx \mathbf{d}-\mathrm{\eta }(\widehat{\mathbf{x}})+{[{\mathrm{\eta }}^{\prime }(\widehat{\mathbf{x}})]}^{\mathrm{T}}\mathrm{\hspace{0.17em}}\widehat{\mathbf{x}}$$

(25)

The above equation is equivalent to the linear constraint $\tilde{\mathbf{\kappa }}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}=\tilde{\mathbf{d}}$, in which:

$$\tilde{\mathbf{\kappa }}={[{\mathrm{\eta }}^{\prime }(\widehat{\mathbf{x}})]}^{\mathrm{T}}$$

(26)

$$\tilde{\mathbf{d}}=\mathbf{d}-\mathrm{\eta }(\widehat{\mathbf{x}})+{[{\mathrm{\eta }}^{\prime }(\widehat{\mathbf{x}})]}^{\mathrm{T}}\mathrm{\hspace{0.17em}}\widehat{\mathbf{x}}$$

(27)

So the method presented in Section 3.4.1 can also be used to solve the nonlinear inequality constraint problem after the constraint is linearized.

3.4.4. The State Non-Linear Inequality Constrained Solution

The problem we want to solve is shown as:

$$\tilde{\mathbf{x}}=\underset{\tilde{\mathbf{x}}}{\min }({\tilde{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}}-2\mathrm{\hspace{0.17em}}{\widehat{\mathbf{x}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{x}})$$

(28)

$$\mathrm{s}.\mathrm{t}.\mathrm{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}}\mathrm{\eta }(\widehat{\mathbf{x}})\le \mathbf{d}$$

(29)

Similar to Section 3.4.3, the non-linear constraint can first be linearized, then the problem will be changed into the linear inequality constraint problem. If the set of the active constraints was known a priori, then the linear inequality constraint will be equivalent to the equality constraint, and the method given in Section 3.4.1 is also applicable.

4. The Inertial Navigation System/Zero Velocity Update (INS/ZUPT) System

We fuse different PNSs to evaluate the state constrained KF algorithm. One PNS is a foot-mounted INS/ZUPT system, which operates in two parts: INS Mechanization to compute the position, velocity and attitude; and an EKF-based ZUPT is triggered to estimate and composite the navigation state errors.

4.1. INS Mechanization

The continuous model (differential equation) of INS mechanization can be given as follows [19,28]:

$$\left[\begin{array}{c}{\dot{\mathbf{r}}}^{\mathrm{n}}\\ {\dot{\mathbf{v}}}^{\mathrm{n}}\\ {\dot{\mathbf{T}}}_{\mathrm{b}}^{\mathrm{n}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{D}}^{-1}\mathrm{\hspace{0.17em}}{\mathbf{v}}^{\mathrm{n}}\\ {\mathbf{T}}_{\mathrm{b}}^{\mathrm{n}}\mathrm{\hspace{0.17em}}{\mathbf{f}}^{\mathrm{b}}-2\mathrm{\hspace{0.17em}}({\mathbf{\Omega }}_{\mathrm{i}\mathrm{e}}^{\mathrm{n}}+{\mathbf{\Omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}})+{\mathbf{g}}^{\mathrm{n}}\\ {\mathbf{T}}_{\mathrm{b}}^{\mathrm{n}}\mathrm{\hspace{0.17em}}({\mathbf{\Omega }}_{\mathrm{i}\mathrm{b}}^{\mathrm{b}}-{\mathbf{\Omega }}_{\mathrm{i}\mathrm{n}}^{\mathrm{b}})\end{array}\right]$$

(30)

where the navigation frame, n, is the local-level frame (local North, East, and Down, i.e., N-E-D), ${\mathbf{r}}^{\mathrm{n}}={[\begin{array}{ccc}{\mathbf{r}}^{\mathrm{x}}& {\mathbf{r}}^{\mathrm{y}}& {\mathbf{r}}^{\mathrm{z}}\end{array}]}^{\mathrm{T}}$ is the INS derived position, ${\mathbf{v}}^{\mathrm{n}}={[\begin{array}{ccc}{\mathbf{v}}^{\mathrm{x}}& {\mathbf{v}}^{\mathrm{y}}& {\mathbf{v}}^{\mathrm{z}}\end{array}]}^{\mathrm{T}}$ denotes the velocity, and ${\mathbf{T}}_{\mathrm{b}}^{\mathrm{n}}$ is the transition matrix describing the body frame b relative to n frame, the attitude vector, ε (pitch, roll and heading), can be derived from ${\mathbf{T}}_{\mathrm{b}}^{\mathrm{n}}$, ${\mathbf{f}}^{\mathrm{b}}$ denotes the accelerometer output, and ${\mathbf{\omega }}^{\mathrm{b}}$ the angular velocity measured by the gyroscope. To get the navigation solution, we assume the sampling period of INS is $\mathrm{\Delta }\mathrm{t}$, the non-linear function ξ denotes INS mechanization. At time step k, the navigation solution ${\mathbf{x}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}}={[\begin{array}{ccc}{\mathbf{r}}_{\mathrm{k}}^{\mathrm{n}}& {\mathbf{v}}_{\mathrm{k}}^{\mathrm{n}}& {\mathbf{\epsilon }}_{\mathrm{k}}\end{array}]}^{\mathrm{T}}$ can be updated as:

$${\mathbf{x}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}}=\mathrm{\xi }\mathrm{\hspace{0.17em}}({\mathbf{x}}_{\mathrm{k}-1},\mathrm{\hspace{0.17em}}{\mathbf{f}}_{\mathrm{k}}^{\mathrm{b}},\mathrm{\hspace{0.17em}}{\mathbf{\omega }}_{\mathrm{k}}^{\mathrm{b}}),\mathrm{\hspace{0.17em}\hspace{0.17em}}{\mathbf{x}}_0^{\mathrm{i}\mathrm{n}\mathrm{s}}=[\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{a}\mathrm{l}\mathrm{\hspace{0.17em}}\mathrm{v}\mathrm{a}\mathrm{l}\mathrm{u}\mathrm{e}]$$

(31)

in which ${\mathbf{x}}_0$ is the initial value of INS. More details about the discrete representation of INS mechanization can be found in [29,30].

4.2. INS Error Model

The INS error model used in this research is given in the following compact form [29]:

$$\mathrm{\delta }{\dot{\mathbf{x}}}^{\mathrm{i}\mathrm{n}\mathrm{s}}=\left[\begin{array}{c}{\mathbf{D}}^{-1}\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{v}}^{\mathrm{n}}\\ -{\mathbf{N}}^{\mathrm{n}}\mathrm{\hspace{0.17em}}\mathbf{\epsilon }-(2\mathrm{\hspace{0.17em}}{\mathbf{\Omega }}_{\mathrm{i}\mathrm{e}}^{\mathrm{n}}+{\mathbf{\Omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}})\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{v}}^{\mathrm{n}}+{\mathbf{V}}^{\mathrm{n}}\mathrm{\hspace{0.17em}}(2\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{\omega }}_{\mathrm{i}\mathrm{e}}^{\mathrm{n}}+\mathrm{\delta }{\mathbf{\omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}})+\mathrm{\delta }{\mathbf{g}}^{\mathrm{n}}\\ -{\mathbf{\Omega }}_{\mathrm{i}\mathrm{n}}^{\mathrm{n}}\mathrm{\hspace{0.17em}}\mathbf{\epsilon }-\mathrm{\delta }{\mathbf{\omega }}_{\mathrm{i}\mathrm{n}}^{\mathrm{n}}\end{array}\right]+{{G}}^{\mathrm{i}\mathrm{n}\mathrm{s}}\mathrm{\hspace{0.17em}}{\mathrm{w}}^{\mathrm{i}\mathrm{n}\mathrm{s}}$$

(32)

where the vector $\mathrm{\delta }{\mathbf{x}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}}={[\begin{array}{ccc}\mathrm{\delta }{\mathbf{r}}_{\mathrm{k}}^{\mathrm{n}}& \mathrm{\delta }{\mathbf{v}}_{\mathrm{k}}^{\mathrm{n}}& \mathrm{\delta }{\mathbf{\epsilon }}_{\mathrm{k}}\end{array}]}^{\mathrm{T}}$ is the INS error state including error components of position, velocity and attitude. ${\mathrm{w}}^{\mathrm{i}\mathrm{n}\mathrm{s}}$ is the sensor noise vector, and ${\mathbf{G}}^{\mathrm{i}\mathrm{n}\mathrm{s}}$ is the noise distribution matrix. Equation (32) can be briefly represented by:

$$\mathrm{\delta }{\dot{\mathbf{x}}}^{\mathrm{i}\mathrm{n}\mathrm{s}}={\mathbf{F}}^{\mathrm{i}\mathrm{n}\mathrm{s}}\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{x}}^{\mathrm{i}\mathrm{n}\mathrm{s}}+{\mathbf{G}}^{\mathrm{i}\mathrm{n}\mathrm{s}}\mathrm{\hspace{0.17em}}{\mathrm{w}}^{\mathrm{i}\mathrm{n}\mathrm{s}}$$

(33)

The above equation is the first-order differentiation of the error state $\mathrm{\delta }{\mathbf{x}}^{\mathrm{i}\mathrm{n}\mathrm{s}}$. A detailed expression of Equation (33) can be found in [29]. ${\mathbf{F}}^{\mathrm{i}\mathrm{n}\mathrm{s}}$ is the dynamic coefficient matrix which contains the INS error models for the position, velocity, and attitude. The discrete-time form of Equation (33) can be written as:

$$\mathrm{\delta }{\mathbf{x}}_{\mathrm{k}+1}^{\mathrm{i}\mathrm{n}\mathrm{s}}={\mathbf{\Phi }}_{\mathrm{k}+1,\mathrm{\hspace{0.17em}k}}^{\mathrm{i}\mathrm{n}\mathrm{s}}\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{x}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}}+{\mathbf{G}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}}\mathrm{\hspace{0.17em}}{\mathbf{w}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}}$$

(34)

where ${\mathbf{\Phi }}_{\mathrm{k}+1,\mathrm{\hspace{0.17em}k}}^{\mathrm{i}\mathrm{n}\mathrm{s}}$ is the discrete-time error state transition matrix from epoch k to k + 1.

4.3. INS Error Correction Using ZUPT

The discrete measurement model at time step k is expressed as:

$$\mathrm{\delta }{\mathbf{z}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}/\mathrm{z}\mathrm{u}\mathrm{p}\mathrm{t}}={\mathbf{H}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}/\mathrm{z}\mathrm{u}\mathrm{p}\mathrm{t}}\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{x}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}}+{\mathbf{\nu }}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}/\mathrm{z}\mathrm{u}\mathrm{p}\mathrm{t}}$$

(35)

where $\mathrm{\delta }{\mathbf{z}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}/\mathrm{z}\mathrm{u}\mathrm{p}\mathrm{t}}$ is the measurement vector, which is the true velocity (zero velocity) of the system minus the velocity vector derived from the INS during the system’s static mode. ${\mathbf{\nu }}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}/\mathrm{z}\mathrm{u}\mathrm{p}\mathrm{t}}$ is the measurement noise matrix. The measurement matrix ${\mathbf{H}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}/\mathrm{z}\mathrm{u}\mathrm{p}\mathrm{t}}$ is:

$${\mathbf{H}}_{\mathrm{k}}^{\mathrm{i}\mathrm{n}\mathrm{s}/\mathrm{z}\mathrm{u}\mathrm{p}\mathrm{t}}=[\begin{array}{ccc}{\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}]$$

(36)

Every time the state errors $\mathrm{\delta }{\mathbf{x}}^{\mathrm{i}\mathrm{n}\mathrm{s}}$ are updated, these are applied to the INS navigation solution, then $\mathrm{\delta }{\mathbf{x}}^{\mathrm{i}\mathrm{n}\mathrm{s}}$ is reset, which means the error state prediction procedure is no longer required.

In this research, we have achieved good zero-velocity detection results using the detection method described in [31] and [32]. Their detection methods are checked through the magnitude of the acceleration, magnitude of gyroscope, and local acceleration variance. Several papers, including [33] and [34], etc., have also discussed zero-velocity detection methods in detail. Readers can refer to this literature for more information on the detection procedure.

5. The Pedestrian Dead Reckoning/Global Navigation Satellite System (PDR/GNSS) System

This section will presents the PDR/GNSS system prototype. The information on user’s travelled distance and heading given by PDR mechanization are merged with the GNSS-derived observations through standard KF.

5.1. PDR Mechanization

The PDR algorithm can be implemented in the following mechanization [35]:

$$\{\begin{array}{l}{\mathbf{E}}_{ \mathrm{k}+1}={\mathbf{E}}_{\mathrm{k}}+{\mathbf{S}}_{\mathrm{k}}\sin ({\mathbf{\psi }}_{\mathrm{k}})\\ {\mathbf{N}}_{ \mathrm{k}+1}={\mathbf{N}}_{\mathrm{k}}+{\mathbf{S}}_{\mathrm{k}}\cos ({\mathbf{\psi }}_{\mathrm{k}})\end{array}$$

(37)

in which E_k+1 is the PDR navigation estimation along local east at time step k + 1, N_k+1 is the PDR navigation estimation along local north at time step k + 1, S_k is the estimated pedestrian step length from time step k to k + 1, ${\mathbf{\psi }}_{\mathrm{k}}$ is the estimated pedestrian step heading at time step k. Worth noting is that the given PDR mechanization only represents a 2-dimension (2D) navigation scheme.

5.2. PDR Error Model

The complete discrete PDR error transition model is given as follows [36]:

$$\{\begin{array}{l}\mathrm{\delta }{\mathbf{E}}_{\mathrm{k}+1}=\mathrm{\delta }{\mathbf{E}}_{\mathrm{k}}+{\mathbf{S}}_{\mathrm{k}}\mathrm{\hspace{0.17em}}\cos ({\mathbf{\psi }}_{\mathrm{k}})\mathrm{\hspace{0.17em}}{\mathbf{\psi }}_{\mathrm{k}}+\sin ({\mathbf{\psi }}_{\mathrm{k}})\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{S}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathbf{N}}_{\mathrm{k}+1}=\mathrm{\delta }{\mathbf{N}}_{\mathrm{k}}-{\mathbf{S}}_{\mathrm{k}}\mathrm{\hspace{0.17em}}\sin ({\mathbf{\psi }}_{\mathrm{k}})\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{\psi }}_{\mathrm{k}}+\cos ({\mathbf{\psi }}_{\mathrm{k}})\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{S}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathbf{\psi }}_{\mathrm{k}+1}=\mathrm{\delta }{\mathbf{\psi }}_{\mathrm{k}}+\mathrm{\Delta }\mathrm{t\hspace{0.17em}}\mathrm{\delta }{\mathbf{b}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathbf{S}}_{\mathrm{k}+1}=\mathrm{\delta }{\mathbf{S}}_{\mathrm{k}}+{({\mathbf{w}}_{\mathrm{S}})}_{\mathrm{k}}\\ \mathrm{\delta }{\mathbf{b}}_{\mathrm{k}+1}=(1-\mathrm{\alpha }\mathrm{\hspace{0.17em}}\mathrm{\Delta }\mathrm{t})\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{b}}_{\mathrm{k}}+{({\mathbf{w}}_{\mathrm{b}})}_{\mathrm{k}}\end{array}$$

(38)

where $\mathrm{\delta }\mathrm{E}$ and $\mathrm{\delta }\mathbf{N}$ are respectively the eastern and northern position errors; $\mathrm{\delta }\mathbf{\psi }$ is the error on the heading induced by the Z (up) gyro bias error $\mathrm{\delta }\mathbf{b}$, which is considered as the random process with zero mean, the correlation time ${\mathrm{T}}_{\mathrm{c}}={\mathrm{\alpha }}^{-1}$ and the driven noise ${\mathbf{w}}_{\mathrm{b}}$; the step length error $\mathrm{\delta }\mathbf{S}$ is considered as a white noise ${\mathbf{w}}_{\mathrm{S}}$ with the variance ${\mathbf{\sigma }}_{\mathrm{S}}^2$. Similar to the INS error model, then Equation (38) can be written as:

$$\mathrm{\delta }{\mathbf{x}}_{\mathrm{k}+1}^{\mathrm{pdr}}={\mathbf{\Phi }}_{\mathrm{k}+1,\mathrm{\hspace{0.17em}k}}^{\mathrm{pdr}}\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{x}}_{\mathrm{k}}^{\mathrm{pdr}}+{\mathbf{G}}_{\mathrm{k}}^{\mathrm{pdr}}\mathrm{\hspace{0.17em}}{\mathbf{w}}_{\mathrm{k}}^{\mathrm{pdr}}$$

(39)

where the vector $\mathrm{\delta }{\mathbf{x}}^{\mathrm{pdr}}={[\begin{array}{ccccc}\mathrm{\delta }\mathbf{E}& \mathrm{\delta }\mathbf{N}& \mathrm{\delta }\mathbf{\psi }& \mathrm{\delta }\mathbf{S}& \mathrm{\delta }\mathbf{b}\end{array}]}^{\mathrm{T}}$ is the PDR error state vector. Then the transition matrix, ${\mathbf{\Phi }}_{\mathrm{k}+1,\mathrm{\hspace{0.17em}k}}^{\mathrm{pdr}}$, from time step k to k + 1 can be written as:

$${\mathbf{\Phi }}_{\mathrm{k}+1,\mathrm{\hspace{0.17em}k}}^{\mathrm{pdr}}=\left[\begin{array}{ccccc}1& 0& {\mathbf{S}}_{\mathrm{k}}\mathrm{\hspace{0.17em}}\cos ({\mathbf{\psi }}_{\mathrm{k}})& \sin ({\mathbf{\psi }}_{\mathrm{k}})& 0\\ 0& 1& -{\mathbf{S}}_{\mathrm{k}}\mathrm{\hspace{0.17em}}\sin ({\mathbf{\psi }}_{\mathrm{k}})& \cos ({\mathbf{\psi }}_{\mathrm{k}})& 0\\ 0& 0& 1& 0& \mathrm{\Delta }\mathrm{t}\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& (1-\mathrm{\alpha }\mathrm{\hspace{0.17em}}\mathrm{\Delta }\mathrm{t})\end{array}\right]$$

(40)

5.3. PDR Error State Correction Using GNSS

The measurement model has the following linear form:

$$\mathrm{\delta }{\mathbf{z}}_{\mathrm{k}}^{\mathrm{pdr}/\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}={\mathbf{H}}_{\mathrm{k}}^{\mathrm{pdr}/\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}\mathrm{\hspace{0.17em}}\mathrm{\delta }{\mathbf{x}}_{\mathrm{k}}^{\mathrm{pdr}}+{\mathbf{\nu }}_{\mathrm{k}}^{\mathrm{pdr}/\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}$$

(41)

where the measurement noise vector, ${\mathbf{\nu }}^{\mathrm{pdr}/\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}$, is assumed to be a zero-mean white Gaussian noise process. The measurement matrix, ${\mathbf{H}}_{\mathrm{k}}^{\mathrm{pdr}/\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}$, should have the form:

$${\mathbf{H}}_{\mathrm{k}}^{\mathrm{pdr}/\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}=[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 2}\end{array}]$$

(42)

The measurement vector is:

$$\mathrm{\delta }{\mathbf{z}}_{\mathrm{k}}^{\mathrm{pdr}/\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}={[\begin{array}{ccc}{\mathbf{E}}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}-{\mathbf{E}}_{\mathrm{k}}^{\mathrm{P}\mathrm{d}\mathrm{r}}& {\mathbf{N}}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}-{\mathbf{N}}_{\mathrm{k}}^{\mathrm{P}\mathrm{d}\mathrm{r}}& {\mathbf{\psi }}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}\end{array}-{\mathbf{\psi }}_{\mathrm{k}}^{\mathrm{pdr}}]}^{\mathrm{T}}$$

(43)

in which ${\mathbf{E}}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}$ is the GNSS-measured east position at time step k; ${\mathbf{N}}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}$ is the measured north position from the GNSS; ${\mathbf{\psi }}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}$ is the GNSS heading. The common method to determine ${\mathbf{\psi }}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}$ is the use of GNSS velocities, and is given by:

$${\mathbf{\psi }}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s}}=\arctan \left(\frac{{\mathbf{V}}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s},\mathrm{\hspace{0.17em}}\mathrm{E}}}{{\mathbf{V}}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s},\mathrm{\hspace{0.17em}}\mathrm{n}}}\right)$$

(44)

where ${\mathbf{V}}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s},\mathrm{\hspace{0.17em}}\mathrm{E}}$ is the east GNSS-derived velocity at time step k, and ${\mathbf{V}}_{\mathrm{k}}^{\mathrm{g}\mathrm{n}\mathrm{s}\mathrm{s},\mathrm{\hspace{0.17em}n}}$ is the north GNSS velocity.

6. Information Fusion of Two Navigation Systems Using State Constraint KF

In this section, we first formulate the constraint between two PNSs’ navigation states, then derive the solution to the constraint problem by applying the method given in Section 3.

6.1. Constraint between Two Navigation Systems

Assuming that we have two PNSs: one is the INS/ZUPT system, the other one is the PDR/GNSS system. We denote the first two elements of INS/ZUPT system’s navigation state, ${\mathbf{X}}^{\mathrm{i}\mathrm{n}\mathrm{s}}={[\begin{array}{ccc}{\mathbf{r}}^{\mathrm{n}}& {\mathbf{v}}^{\mathrm{n}}& \mathbf{\epsilon }\end{array}]}^{\mathrm{T}}$, as ${\mathbf{X}}_1={[\begin{array}{cc}{\mathbf{r}}^{\mathrm{x}}& {\mathbf{r}}^{\mathrm{y}}\end{array}]}^{\mathrm{T}}$, in which ${\mathbf{r}}^{\mathrm{x}}$ and ${\mathbf{r}}^{\mathrm{y}}$ are respectively the position solutions along the direction of local East and North. The first two states of PDR/GNSS system’s navigation state, ${\mathbf{X}}^{\mathrm{pdr}}={[\begin{array}{ccccc}\mathbf{E}& \mathbf{N}& \mathbf{\psi }& \mathbf{S}& \mathbf{b}\end{array}]}^{\mathrm{T}}$, is expressed as ${\mathbf{X}}_2={[\begin{array}{cc}\mathbf{E}& \mathbf{N}\end{array}]}^{\mathrm{T}}$. We denote the combined navigation solution of the two PNSs as:

$$\mathbf{X}={[\begin{array}{cc}{\mathbf{X}}^{\mathrm{i}\mathrm{n}\mathrm{s}}& {\mathbf{X}}^{\mathrm{p}\mathrm{d}\mathrm{r}}\end{array}]}^{\mathrm{T}}$$

(45)

As depicted in Section 1, the INS/ZUPT system is mounted on a user’s foot, the PDR/GNSS system is held on the upper body of the user. During the navigation process, there is always a maximum horizontal distance between the two PNSs, namely:

$$\left|\right|{\mathbf{X}}_1-{\mathbf{X}}_2|{|}^2\le {\mathbf{d}}^2$$

(46)

where $\mathbf{d}$ is a constant representing the maximum distance. For the convenience of the further matrix calculations, Equation (46) can be simplified by:

$$\left|\right|\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{X}|{|}^2={(\mathrm{\kappa \hspace{0.17em}}\mathbf{X})}^{\mathrm{T}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{X})\le {\mathbf{d}}^2$$

(47)

in which matrix $\mathbf{\kappa }$ is expressed as:

$$\mathbf{\kappa }=[\begin{array}{cccc}{\mathbf{I}}_{\mathrm{2}\times 2}& {\mathbf{0}}_{\mathrm{2}\times 7}& -{\mathbf{I}}_{2\times 2}& {\mathbf{0}}_{\mathrm{2}\times 3}\end{array}]$$

(48)

The combined position solution $\mathbf{X}$ should always satisfy Equation (47), if not, when $\left|\right|\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{X}|{|}^2>{\mathbf{d}}^2$, we should employ the state constraint KF to restrict $\mathbf{X}$ onto the space of Equation (47). So the constrained new estimate, $\tilde{\mathbf{X}}$, can be calculated as:

$$\tilde{\mathbf{X}}=\underset{\tilde{\mathbf{X}}}{\min }({\tilde{\mathbf{X}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{X}}-2\mathrm{\hspace{0.17em}}{\widehat{\mathbf{X}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{X}})$$

(49)

$$\mathrm{s}.\mathrm{t}.\mathrm{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}}{(\mathbf{\kappa \hspace{0.17em}}\tilde{\mathbf{X}})}^{\mathrm{T}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\tilde{\mathbf{X}})\le {\mathbf{d}}^2$$

(50)

where $\mathbf{P}$ in Equation (49) is the covariance matrix of the combined position solution, $\mathbf{X}$, which is obtained from the EKF-based INS. Worth noting is that for each navigation system, the position part of the navigation state covariance matrix is updated separately for each EKF. When the combined navigation state does not satisfy ${(\mathbf{\kappa \hspace{0.17em}}\mathbf{X})}^{\mathrm{T}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{X})\le {\mathbf{d}}^2$, the state constraint algorithm will be applied. At this time, the combined covariance matrix Cov for Equation (49) can then be expressed as:

$$\mathbf{P}=\left[\begin{array}{cc}{\mathbf{P}}_{9\times 9}^{\mathrm{i}\mathrm{n}\mathrm{s}}& {\mathbf{0}}_{9\times 5}\\ {\mathbf{0}}_{5\times 9}& {\mathbf{P}}_{5\times 5}^{\mathrm{p}\mathrm{d}\mathrm{r}}\end{array}\right]$$

(51)

in which ${\mathbf{P}}_{9\times 9}^{\mathrm{i}\mathrm{n}\mathrm{s}}$ and ${\mathbf{P}}_{5\times 5}^{\mathrm{pdr}}$ are the covariance matrices respectively representing the navigation error standard deviations of the INS/ZUPT system and PDR/GNSS system.

6.2. The Solution to the Constraint Problem

From Equation (50) we can see the given constraint is an in-equality problem, and both ${(\mathbf{\kappa \hspace{0.17em}}\mathbf{X})}^{\mathrm{T}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{X})$ and ${\mathbf{d}}^2$ can be regarded as the $1\times 1$ matrices, so Equation (50) cannot be written in the form of several rows, which means there is only one constraint set. So the optimal solution ${\mathbf{X}}^{*}$ (a feasible point of the constraint) to the problem given in Equation (50) should definitely satisfy the equality constraint:

$${(\mathbf{\kappa \hspace{0.17em}}{\mathbf{X}}^{*})}^{\mathrm{T}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}{\mathbf{X}}^{*})={\mathbf{d}}^2$$

(52)

such that the in-equality constraint, ${(\mathbf{\kappa \hspace{0.17em}}\mathbf{X})}^{\mathrm{T}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\mathbf{X})\le {\mathbf{d}}^2$, is an active constraint of ${\mathbf{X}}^{*}$. Thus, the problem given in Equations (49) and (50) can be changed into:

$$\tilde{\mathbf{X}}=\underset{\tilde{\mathbf{X}}}{\min }({\tilde{\mathbf{X}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{X}}-2\mathrm{\hspace{0.17em}}{\widehat{\mathbf{X}}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{P}}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{X}})$$

(53)

$$\mathrm{s}.\mathrm{t}.\mathrm{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}}{(\mathbf{\kappa \hspace{0.17em}}\tilde{\mathbf{X}})}^{\mathrm{T}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\tilde{\mathbf{X}})={\mathbf{d}}^2$$

(54)

Then the constraint in Equation (54) can be linearized (Taylor’s expansion) about the current unconstrained state estimate, $\widehat{\mathbf{X}}$. Neglecting the second-order and higher terms of the Taylor’s expansion, we have:

$${(2\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}}\mathbf{\hspace{0.17em}\kappa \hspace{0.17em}}\widehat{\mathbf{X}})}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{\mathbf{X}}^{*}={\mathbf{d}}^2-{(\mathbf{\kappa \hspace{0.17em}}\widehat{\mathbf{X}})}^{\mathrm{T}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\widehat{\mathbf{X}})+{(2\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}}\mathbf{\hspace{0.17em}\kappa \hspace{0.17em}}\widehat{\mathbf{X}})}^{\mathrm{T}}\mathrm{\hspace{0.17em}}\widehat{\mathbf{X}}$$

(55)

The Equation (55) is equivalent to the linear constraint, $\tilde{\mathbf{\kappa }}\mathrm{\hspace{0.17em}}{\mathbf{X}}^{*}=\tilde{\mathbf{d}}$, in which:

$$\tilde{\mathbf{\kappa }}={(2\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}}\mathbf{\hspace{0.17em}\kappa \hspace{0.17em}}\widehat{\mathbf{X}})}^{\mathrm{T}}$$

(56)

$$\tilde{\mathbf{d}}={\mathbf{d}}^2-{(\mathbf{\kappa \hspace{0.17em}}\widehat{\mathbf{X}})}^{\mathrm{T}}(\mathbf{\kappa }\mathrm{\hspace{0.17em}}\widehat{\mathbf{X}})+{(2\mathrm{\hspace{0.17em}}{\mathbf{\kappa }}^{\mathrm{T}}\mathbf{\hspace{0.17em}\kappa \hspace{0.17em}}\widehat{\mathbf{X}})}^{\mathrm{T}}\mathrm{\hspace{0.17em}}\widehat{\mathbf{X}}$$

(57)

So the solution to the problem in Equations (53) and (54) is given as:

$${\mathbf{X}}^{*}=\widehat{\mathbf{X}}-\mathbf{P}\mathrm{\hspace{0.17em}}{\tilde{\mathbf{\kappa }}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{(\tilde{\mathbf{\kappa }}\mathrm{\hspace{0.17em}}\mathbf{P\hspace{0.17em}}{\tilde{\mathbf{\kappa }}}^{\mathrm{T}})}^{-1}(\tilde{\mathbf{\kappa }}\mathrm{\hspace{0.17em}}\widehat{\mathbf{X}}-\tilde{\mathbf{d}})$$

(58)

According to Section 3.4.1, the new covariance of the constrained navigation solution, represented by ${\mathbf{P}}^{*}$, is updated as follows:

$${\mathbf{P}}^{*}=(\mathrm{{\rm I}}-\mathbf{\mu })\mathrm{\hspace{0.17em}}\mathbf{P}\mathrm{\hspace{0.17em}}{(\mathrm{{\rm I}}-\mathbf{\mu })}^{\mathrm{T}}$$

(59)

in which $\mathbf{\mu }$ is expressed as:

$$\mathbf{\mu }=\mathbf{P}\mathrm{\hspace{0.17em}}{\tilde{\mathbf{\kappa }}}^{\mathrm{T}}\mathrm{\hspace{0.17em}}{(\tilde{\mathbf{\kappa }}\mathrm{\hspace{0.17em}}\mathbf{P\hspace{0.17em}}{\tilde{\mathbf{\kappa }}}^{\mathrm{T}})}^{-1}\mathrm{\hspace{0.17em}}\tilde{\mathbf{\kappa }}$$

(60)

In terms of the system implementation, as well as theoretically compared to the existing techniques given in the literature review those also use spatial constraints, it can be seen from the above Equations (58) and (59) that our proposed method only requires slightly more computational effort than the unconstrained filter. That is all because our proposed approach is quite similar to the way of implementing an Extended Kalman filter (EKF), which relies on the linearization of the system and measurement equations. The linearization of the constraint is done beforehand, such that the implementation procedure of the constraint filter is as easy as substituting the KF-derived navigation state $\widehat{\mathbf{X}}$ and state covariance $\mathbf{P}$ into Equations (58) and (59).

However, the constraint filter given in [21] requires much higher computational complexity because of the additional quadratic programming problem that needs to be solved. To get the solution to the constraint filter, first, the authors changed the quadratic problem into a Lagrange multiplier λ-based quadratic equation root-finding problem. A nonlinear polynomial function was then constructed, and to find the roots to the nonlinear polynomial function, one must resort to some numerical root finding methods, in [21], Newton’s method is used, then, based on the iteration of the Newton’s method, the Lagrange multiplier λ can be found, consequently the solution to the constraint filter can then be further resolved. For implementing such a constraint filter, the computational burden is mainly caused by the multiple iterative calculations to find a converged Lagrange multiplier λ.

Similarly, compared to the method given by Zampella et al. [24], their constraint filter adopts a probability density function (PDF) truncation approach, which requires a lot of calculations to approximate the truncated PDF mean and covariance. To be specific, their constraint filter uses a particle filter way to calculate the propagation of the mean and covariance of the propagated states of the constraint filter. Moreover, in the conclusion part of [24], the authors also address that it requires many calculation burdens to implement their constraint filter.

7. Results and Discussion Experimental Evaluation

For evaluating the proposed PNS approach, we conducted both the Monte Carlo simulation as well as the real field experiments. For the real experiments, first a basic experiment conducted in a GNSS available environments illustrating how the proposed algorithm works was conducted, the basic experiment was done in a GNSS available area. Then the multiple paths-based experiments were done in a GNSS-denied indoor environment.

7.1. Monte Carlo Simulation

We consider the positioning of two PNSs mounted on two different parts of the user’s body. The combined positioning solutions of both systems can be expressed as:

$$\mathbf{x}=[\begin{array}{cccc}{\mathbf{r}}_1^{\mathrm{E}}(\mathrm{k})& {\mathbf{r}}_1^{\mathrm{N}}(\mathrm{k})& {\mathbf{r}}_2^{\mathrm{E}}(\mathrm{k})& {\mathbf{r}}_2^{\mathrm{N}}(\mathrm{k})\end{array}]$$

(61)

where ${\mathbf{r}}_{\mathrm{j}}^{\mathrm{i}}\mathrm{(k)}$ means the coordinate of the j-th system along the direction of i (local East or North) at time step k. We assume ${\mathbf{r}}_{\mathrm{j}}^{\mathrm{i}}(\mathrm{k})$ is updated recursively as:

$$\{\begin{array}{l}{\mathbf{r}}_1^{\mathrm{E}}(\mathrm{k})={\mathbf{r}}_1^{\mathrm{E}}(\mathrm{k}-1)+{\mathbf{v}}_1^{\mathrm{E}}(\mathrm{k}-1)\cdot \mathrm{\Delta }\mathrm{t}+{\mathbf{s}}_1^{\mathrm{E}}(\mathrm{k}-1)\\ {\mathbf{r}}_1^{\mathrm{N}}(\mathrm{k})={\mathbf{r}}_1^{\mathrm{N}}(\mathrm{k}-1)+{\mathbf{v}}_1^{\mathrm{N}}(\mathrm{k}-1)\cdot \mathrm{\Delta }\mathrm{t}+{\mathbf{s}}_1^{\mathrm{N}}(\mathrm{k}-1)\\ {\mathbf{r}}_2^{\mathrm{E}}(\mathrm{k})={\mathbf{r}}_2^{\mathrm{E}}(\mathrm{k}-1)+{\mathbf{v}}_2^{\mathrm{E}}(\mathrm{k}-1)\cdot \mathrm{\Delta }\mathrm{t}+{\mathbf{s}}_2^{\mathrm{E}}(\mathrm{k}-1)\\ {\mathbf{r}}_2^{\mathrm{N}}(\mathrm{k})={\mathbf{r}}_2^{\mathrm{N}}(\mathrm{k}-1)+{\mathbf{v}}_2^{\mathrm{N}}(\mathrm{k}-1)\cdot \mathrm{\Delta }\mathrm{t}+{\mathbf{s}}_2^{\mathrm{N}}(\mathrm{k}-1)\end{array}$$

(62)

in which ${\mathbf{v}}_{\mathrm{j}}^{\mathrm{i}}(\mathrm{k}-1)$ is the speed of the j-th navigation system along the direction of i between time step k − 1 and k. Due to the errors of the used sensors, ${\mathbf{r}}_{\mathrm{j}}^{\mathrm{i}}$ is updated in consideration of noise. ${\mathbf{s}}_{\mathrm{j}}^{\mathrm{i}}(\mathrm{k}-1)$ is the position increment error of the j-th system along the direction of i. We assume the disturbed position increment, ${\mathbf{s}}_{\mathrm{j}}^{\mathrm{i}}$, is with Gaussian distribution and can thus be expressed as:

$${\mathbf{s}}_{\mathrm{j}}^{\mathrm{i}}\in \mathbf{N}\{0,\mathrm{\hspace{0.17em}\hspace{0.17em}}{\mathbf{q}}_{\mathrm{j}}^{\mathrm{i}}\}$$

(63)

in which ${\mathbf{q}}_{\mathrm{j}}^{\mathrm{i}}$ is the error variance of the disturbed position increment. At this time, the position error covariance, ${\mathbf{P}}_{\mathrm{k}}$, of the combined state $\mathbf{x}$ is updated as:

$${\mathbf{P}}_{\mathrm{k}}={\mathbf{P}}_{\mathrm{k}-1}+{\mathbf{Q}}_{\mathrm{k}}$$

(64)

where ${\mathbf{P}}_0$ is the initial position error covariance, and $\mathbf{Q}$ has the form:

$$\mathbf{Q}=\left[\begin{array}{cccc}{\mathbf{q}}_1^{\mathrm{E}}& 0& 0& 0\\ 0& {\mathbf{q}}_1^{\mathrm{N}}& 0& 0\\ 0& 0& {\mathbf{q}}_2^{\mathrm{E}}& 0\\ 0& 0& 0& {\mathbf{q}}_2^{\mathrm{N}}\end{array}\right]$$

(65)

At this time, the maximum relative distance of the two systems should be a priori known, which means:

$$\sqrt{\left|\right|{\mathbf{r}}_1^{\mathrm{E}}(\mathrm{k})-{\mathbf{r}}_2^{\mathrm{E}}(\mathrm{k})|{|}^2+||{\mathbf{r}}_1^{\mathrm{N}}(\mathrm{k})-{\mathbf{r}}_2^{\mathrm{N}}(\mathrm{k})|{|}^2}\le \mathbf{d}$$

(66)

where $\mathbf{d}$ is the maximum distance between the two systems. We assume that the user walks straight toward local East from the start coordinate of $(0,\mathrm{\hspace{0.17em}}0)$; the update rates of both PNSs are 100 Hz; the walking speed is set as 1 m/s, namely ${\mathbf{v}}_1^{\mathrm{E}}={\mathbf{v}}_2^{\mathrm{E}}=1$ m/s, ${\mathbf{v}}_1^{\mathrm{N}}={\mathbf{v}}_2^{\mathrm{N}}=0$ m/s; ${\mathbf{q}}_{\mathrm{j}}^{\mathrm{i}}={0.05}^2$ m²; ${\mathbf{P}}_0$ is initialized as a $4\times 4$ null matrix; $\mathbf{d}$ is set as 1.5 m. We perform 30 Monte Carlo simulations, each time for 500 s.

Figure 1a shows both systems’ RMSE (Root-Mean-Square Error) values over time before and after applying the constraint approaches. The RMSE values are averaged over 30 Monte Carlo simulations. Figure 1b shows the results of one representative simulation. From Figure 1 and Figure 2 we can see that after applying the two constraint approaches, the state estimation errors could be effectively constrained.

Figure 1. (a) RMSE (Root-Mean-Square Error) versus time; (b) Trajectories of one simulation.

Figure 1. (a) RMSE (Root-Mean-Square Error) versus time; (b) Trajectories of one simulation.

Figure 2. (a) Reference trajectory and (b) GPS (Global Position System) estimated trajectory (from Pedestrian Dead Reckoning/Global Navigation Satellite System (PDR/GNSS) system).

Figure 2. (a) Reference trajectory and (b) GPS (Global Position System) estimated trajectory (from Pedestrian Dead Reckoning/Global Navigation Satellite System (PDR/GNSS) system).

7.2. Experiments in Outdoor Environments

Two identical MEMS IMUs (embedded in two Samsung Note 2 smart-phones) including GPS (Global Position System) receivers were used to conduct the experiment. One phone was mounted on the right shoe, the other was held in the user’s right hand (keep levelled during the walking). The sampling rate of the IMU is 100 Hz, and the GPS update rate is 1 Hz. The experiment was conducted for 8 min on a standard football field (half size) within the University of Calgary’s campus area. The experiment was carried out along a rectangle-shaped line. The side of the rectangle was 61 m in length and 50 m in width, resulting in a total walking distance of 222 m. The sides of the rectangle were either parallel or vertical to the local true North and East. The ground truth, walking direction, and start and end points (which were the same) can be seen in Figure 2a.

In this research, as we have to integrate different sources of measurements together at the same time, precise synchronization or time correction between different sensors/devices is needed. For a single smartphone, because sensor sampling rate in an Android-based device is predefined and is not very stable. Programmatically, we cannot change that, at least not from application level. For solving this problem, we use the interpolation method to compensate for the uneven sensor sampling rate reflected in our smart-phones. Through interpolation, basically, we can get the sensor data with any pre-defined sampling rate.

For the synchronization issue of the sensors between two Android-based devices, there always exist an almost constant time difference between any two deceives, we need to do the temporal calibration before-hand. The temporal calibration procedure can be done easily in an open sky environment. We then use the precise GPS time recorded by two smartphones as a time reference, based on it, we can calibrate the almost unchangeable temporal bias of the sensors between the two smartphones. Due to some uncertain factors of the hardware layers of the smartphones, every time when we start recording the raw sensor data, a quite small/limited change may be added on the calibrated temporal bias. We have done tens of tests and experimentally found that such limited changes are always less than 0.1 s. If the user walks with a normal walking speed of 1 m/s, the resulting constant error distances between the two phones will always be controlled within 0.1 m. Such an error level is totally acceptable, and can be regarded as a variance of the distance constraint between the two phones.

As the GPS receiver embedded in the smart-phone is the low-cost and miniature device, this leads to limited positional accuracy. From Figure 2b we can see that the GPS could continuously provide the trajectory within ±5 m level positional accuracy during the experiments. Worth noting is that all the flowing experiment results are shown in the local geographic frame.

The following Figure 3a shows the INS-only navigation solution of foot mounted INS/ZUPT system. It can be seen that without any aiding information, the position errors diverge to hundreds of kilometers after 8 min of the experiment, due to the relative large noise ratio of the smart-phone’s low-cost MEMS inertial sensors. Figure 3b shows the PDR-only result when using the gyroscope-derived heading. As explained above that azimuth estimation is the most challenging aspect of PDR. Due to the in-run gyroscope bias, the ultimate accuracy and quality of the navigation solution degraded gradually with the walked distance. The position error at the end of the test is around 20 m.

Figure 4a shows the INS/ZUPT system’s navigation result. When the INS was frequently aided by ZUPTs, the error accumulation is well bounded for a short period of time (within 5 min). However, when the user walked for a long time, small error accumulation was noticed, these errors were primarily driven by the errors in the heading estimation as well as the large in-run bias instability of the inertial sensor, leading to position error on the order of 5 m after about 5 min of walking. The maximum error was seen at the end of the walk with almost 10 m. Such error level is reasonable given the quality of the used smart-phone’s cheap MEMS inertial sensors. As we don’t have reference trajectory with which to evaluate the entire trip’s absolute accuracy, however, the user ended the walking very close to the original starting point (0, 0, 0), the INS/ZUPT derived position was displaced about 10 m, indicating a probable accuracy of approximately 4 percent of the traveled distance.

Figure 4b shows the navigation results obtained from the PDR/GNSS sub-system when there is no GPS signal outages. As can be seen from the figure that the integrated system performed quite well which was actually expected when aided by periodical GPS derived positions. Since the satellite visibility is relatively good over the trajectory, to assess the performance of the constraint algorithm’s positioning capability during GPS signal outage periods, the GPS outages are manually simulated for 10, 20, 30 and 40 s, respectively.

Figure 3. (a) INS Only navigation solution (from Inertial Navigation System/Zero Velocity Update (INS/ZUPT) system); (b) PDR only navigation solution (from PDR/GNSS system).

Figure 3. (a) INS Only navigation solution (from Inertial Navigation System/Zero Velocity Update (INS/ZUPT) system); (b) PDR only navigation solution (from PDR/GNSS system).

Figure 4. (a) INS/ZUPT system’s navigation solution; (b) PDR/GNSS system’s integrated navigation solution.

Figure 4. (a) INS/ZUPT system’s navigation solution; (b) PDR/GNSS system’s integrated navigation solution.

Figure 5 shows the PDR/GNSS sub-system’s navigation solutions for the four GPS outage scenarios, we can see that when there is no GPS update, the position result will diverge up to 20 m during the GPS outage period for 40 s. With the same divergence trend, worse results will obtained when simulating GPS outage for longer duration. Figure 6 below shows the KF derived positions after applying the state constraint algorithm. Figure 6a presents the constrained position estimates when there is no GPS signal outage, Figure 6b–d, respectively, give the constrained position estimates when GPS outages are simulated by 10, 30 and 60 s. The constraint’s update rate is the same as the user’s step-rate, namely the constraint is applied when a step is detected as well as the horizontal distance between the two systems is larger than the predefined $\mathbf{d}$ (1.5 m).

As can be noted from Figure 6, the positioning results from both PDR/GNSS and INS/ZUPT systems are similar, both performed quite well and remained closely aligned with the reference solution with acceptable deviations visible at different portions of the trajectories. The maximum position errors of PDR/GNSS and INS/ZUPT systems are on the order of 5 m. The state constraint algorithm allows the correct adjustments in situations when detecting the spatial distances between the two sub-systems are larger than 1.5 m, then diminishing the position errors in each system, all of the track closure errors of both systems significantly bounded. As visible from Figure 5, when there is GPS outage, through applying the state constraint algorithm, the PDR/GNSS sub-system’s horizontal position errors can be greatly corrected during the GPS outage period comparing with the results shown in Figure 5 (without applying the constraint algorithm). Similarly, the position errors remained under 5 m, which is significant given that the GPS outage lasted for about 60 s.

From the above experimental results, we can conclude that when GPS is available, the GPS position will be periodically used to update the hand-held smart-phone’s PDR/GNSS estimate, therefore, the PDR/GNSS system’s state error covariance ${\mathbf{P}}^{\mathrm{pdr}}$ could always be restricted in a relatively small value. However, for the foot-mounted INS/ZUPT system, due to the reason that the heading of the system is not observable during ZUPT as well as the large in-run bias instability of the inertial sensor, both could result into the INS/ZUPT system’s position error covariance ${\mathbf{P}}^{\mathrm{ins}}$ diverges slowly and gradually. According to Equation (3.8), ${\mathbf{P}}^{-1}$ can be regarded as the weighting factor, which is the inverse of the combined navigation state covariance matrix $\mathbf{P}$. At this time, the inverse of ${\mathbf{P}}^{\mathrm{pdr}}$ is much bigger than the inverse of ${\mathbf{P}}^{\mathrm{ins}}$, therefore the position of PDR/GNSS system is much accurate than that of the INS/ZUPT system, then the final positions of the two systems when using the state constraint algorithm will more “trust” the PDR/GNSS system’s result.

Figure 5. PDR/GNSS system’s navigation solutions for simulated different GPS Outages of (a) 10 s; (b) 20 s; (c) 30 s; (d) 40 s.

Figure 5. PDR/GNSS system’s navigation solutions for simulated different GPS Outages of (a) 10 s; (b) 20 s; (c) 30 s; (d) 40 s.

Figure 6. Two systems’ navigation solutions with state constraint for simulated GPS outage of (a) 10 s; (b) 20 s; (c) 30 s; (d) 40 s.

Figure 6. Two systems’ navigation solutions with state constraint for simulated GPS outage of (a) 10 s; (b) 20 s; (c) 30 s; (d) 40 s.

On the contrary, when GPS is not available, the INS/ GNSS system’s position error covariance (standard deviation) ${\mathbf{P}}^{\mathrm{pdr}}$ will increase quickly, and benefiting from the preceding position correction by the state constraint algorithm as well as the periodical usage of ZUPT, the INS/ZUPT system’s position error covariance ${\mathbf{P}}^{\mathrm{ins}}$ will not increase quickly and at this time, during the GPS outage, the inverse of ${\mathbf{P}}^{\mathrm{ins}}$ is much bigger, therefore the position estimate of INS/ZUPT system is much accurate than that of the PDR/GNSS system, then the final positions of the two systems when using the state constraint algorithm will more “trust” the INS/ZUPT system’s result.

7.3. Experiments in Indoor Environments

The preceding open sky-based field experiment as well as its results have well illustrated the basic working principle and the efficacy of the proposed constraint approach. Although the results seem promising with/without the simulated GPS outages, however, in general, the one experimental trajectory is not enough to support the significant conclusions. Also, some researchers may challenge that why not fuse the GNSS and INS/ZUPT directly. In order to adequately back up the conclusions drawn from the above experiment, as well as demonstrate that the navigation solution derived through the integration of multiple PNSs is much more accurate and robust than that of using only a standalone PNS when there is no external aiding information, such as GNSS, WiFi, etc.

In this subsection, we performed several indoor experiments when GNSS was totally unavailable. Two late model smart-phones (Xiaomi^™ Mi 4 (Xiaomi, Beijing, China)) using STMicroelectronics^™ MEMS IMUs (STMicroelectronics, Geneva, Switzerland) were used to conduct the multiple paths-based experiments in an indoor area. The gyroscope embedded in the Mi 4 has much lower noise density as well as bias stability compared with the gyroscope embedded in the Samsung Note 2 (Samsung, Suwon, South Korea) for the experiment given in Subsection 7.2. The experiments were conducted in the Energy Environment Experiential Learning (EEEL) at the University of Calgary. The ground truth for the experiment is about 80 m × 16.5 m. Six identical trajectories were used to test the performance of proposed constraint approach. The reference trajectory and the first floor plan of the EEEL building can be seen in the following Figure 7.

Figure 7. Reference trajectory at the energy environment experiential learning (EEEL).

Figure 7. Reference trajectory at the energy environment experiential learning (EEEL).

Also, in order to quantitatively show the performance of the proposed algorithm, we need to obtain reference trajectories/coordinates for further error analysis. In this research, we adopt the indoor map-aided PDR method for generating relatively precise reference trajectories. We make full use of the fact that most of the indoor corridors are straight as well as the positions of corners and intersections are known on specific indoor maps. At any moment, when detecting the user is walking along a straight line/corridor, the user’s walking heading for the PDR update will be “locked” in a specific value according to the indoor map. In this experiment, any momentary headings derived from the gyroscope are all corresponding to one of the four dominate directions (in degree), 0/360, 90, 180 and 270. Meanwhile, at any time when detecting the heading change of the user, the specific corners and intersections will also be detected, which can be regarded as “landmarks” to further correct the PDR-derived positions.

The following Figure 8, Figure 9, Figure 10, Figure 11, Figure 12 and Figure 13 respectively show the six trajectories-derived navigation solutions. Each figure gives both the positioning results of both PDR and INS/ZUPT navigation systems before and after applying the proposed state constraint algorithm. Correspondingly, the positioning performances using different approaches in the six trajectories can be seen in Table 1.

Figure 8. Trajectory I: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 8. Trajectory I: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 9. Trajectory II: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 9. Trajectory II: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 10. Trajectory III: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 10. Trajectory III: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 11. Trajectory IV: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 11. Trajectory IV: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 12. Trajectory V: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 12. Trajectory V: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 13. Trajectory VI: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Figure 13. Trajectory VI: Two systems’ navigation solutions. (a) Before constraint; (b) After constraint.

Table 1. Positioning performance using different algorithms in the six different trajectories.

Trajectory	System	Algorithm	Error (m)
			Maximum	Minimum	Mean	RMS
Trajectory I	Hand-held	Before Constraint	13.863	0.319	6.852	7.515
		After Constraint	9.639	0.258	4.600	5.036
	Foot-Mounted	Before Constraint	15.028	0.319	6.029	7.098
		After Constraint	9.882	0.319	4.652	5.077
Trajectory II	Hand-held	Before Constraint	8.149	0.541	3.607	4.149
		After Constraint	6.778	0.872	2.704	2.978
	Foot-Mounted	Before Constraint	14.838	0.039	5.394	6.641
		After Constraint	7.198	0.039	2.690	2.988
Trajectory III	Hand-held	Before Constraint	12.161	1.261	4.749	5.841
		After Constraint	3.689	0.178	2.042	2.276
	Foot-Mounted	Before Constraint	7.457	0.981	4.394	4.569
		After Constraint	3.720	0.194	2.098	2.326
Trajectory IV	Hand-held	Before Constraint	13.671	0.115	5.172	6.498
		After Constraint	2.671	0.021	1.177	1.347
	Foot-Mounted	Before Constraint	10.932	0.027	3.683	4.813
		After Constraint	2.857	0.074	1.171	1.313
Trajectory V	Hand-held	Before Constraint	15.825	0.043	5.730	7.414
		After Constraint	6.606	0.043	2.583	3.210
	Foot-Mounted	Before Constraint	1.791	0.014	0.716	0.832
		After Constraint	6.923	0.316	2.458	3.235
Trajectory VI	Hand-held	Before Constraint	20.319	0.011	8.409	10.123
		After Constraint	5.390	0.011	2.425	2.689
	Foot-Mounted	Before Constraint	12.869	0.295	3.686	5.555
		After Constraint	5.192	0.319	2.334	2.611

Table 1. Positioning performance using different algorithms in the six different trajectories.

Trajectory	System	Algorithm	Error (m)
			Maximum	Minimum	Mean	RMS
Trajectory I	Hand-held	Before Constraint	13.863	0.319	6.852	7.515
		After Constraint	9.639	0.258	4.600	5.036
	Foot-Mounted	Before Constraint	15.028	0.319	6.029	7.098
		After Constraint	9.882	0.319	4.652	5.077
Trajectory II	Hand-held	Before Constraint	8.149	0.541	3.607	4.149
		After Constraint	6.778	0.872	2.704	2.978
	Foot-Mounted	Before Constraint	14.838	0.039	5.394	6.641
		After Constraint	7.198	0.039	2.690	2.988
Trajectory III	Hand-held	Before Constraint	12.161	1.261	4.749	5.841
		After Constraint	3.689	0.178	2.042	2.276
	Foot-Mounted	Before Constraint	7.457	0.981	4.394	4.569
		After Constraint	3.720	0.194	2.098	2.326
Trajectory IV	Hand-held	Before Constraint	13.671	0.115	5.172	6.498
		After Constraint	2.671	0.021	1.177	1.347
	Foot-Mounted	Before Constraint	10.932	0.027	3.683	4.813
		After Constraint	2.857	0.074	1.171	1.313
Trajectory V	Hand-held	Before Constraint	15.825	0.043	5.730	7.414
		After Constraint	6.606	0.043	2.583	3.210
	Foot-Mounted	Before Constraint	1.791	0.014	0.716	0.832
		After Constraint	6.923	0.316	2.458	3.235
Trajectory VI	Hand-held	Before Constraint	20.319	0.011	8.409	10.123
		After Constraint	5.390	0.011	2.425	2.689
	Foot-Mounted	Before Constraint	12.869	0.295	3.686	5.555
		After Constraint	5.192	0.319	2.334	2.611

The navigation performances using the two different approaches (with/without constraint) in the six trajectories are depicted in the above Table 1. As can be seen from Figure 8, Figure 9, Figure 10, Figure 11, Figure 12 and Figure 13 and Table 1, the constraint approach has improved the navigation performances of both PDR and INS/ZUPT systems in the trajectories of I, II, III, IV and VI, especially has significantly improved those in the trajectories of III, IV and VI. Only the INS/ZUPT navigation system’ performance in trajectory V has degraded, with RMS (Root-Mean-Square) values degraded from 0.732 to 3.235 m, mean values degraded from 0.616 to 2.458 m, however, such error level is still acceptable given the application of pedestrian indoor navigation. On the contrary, in the same trajectory V, the PDR performance has been greatly improved, with RMS values improved from 7.414 to 3.210 m, mean values improved from 5.730 to 2.583 m. From the robustness point of view, the integrated navigation systems are more robust than each individual system through applying the proposed constraint KF approach.

From Figure 8, Figure 9, Figure 10, Figure 11, Figure 12 and Figure 13 and Table 1, it can be summarized that on average the navigation results using the constraint approach offer obvious improvement over the navigation results without using the proposed constraint approach, six out of six being improved with the PDR system, and five out of six being improved with the INS/ZUPT system. Due to our limited sample size of six experiments, the above given numbers should not be regarded as having any significance in the aspect of statistics, however, the six experiments do show the improvement that is possible with the proposed constraint approach for the integration of multiple pedestrian navigation systems.

8. Conclusions

This paper presents a novel pedestrian navigation prototype through integrating the navigation solutions from two individual PNSs, namely the upper body mounted PDR/GNSS system and the foot-mounted INS/ZUPT system. A nonlinear inequality distance constraint between the two systems’ positioning estimates are explored. The state constraint Kalman filter is used to fuse the two systems’ navigation information and to diminish the positioning errors of both systems. The proposed new pedestrian navigation prototype was tested through both the Monte Carlo simulations and several real field experiments. Through using the constraint algorithm, both navigation systems’ positioning accuracy could be well improved. Future work includes the integration of three or more systems to compose a more robust PNS prototype. Also, long term indoor/outdoor experiments will be done to evaluate the performance of the integrated PNS prototype.