#### 3.1. Generalized Likelihood Ratio Test (GLRT)

The output of MIMU can be expressed as

where the specific force measurement vector

${x}_{k}^{a}\in {\mathsf{\Omega}}^{3}$ and the angular rate measurements vector

${x}_{k}^{\omega}\in {\mathsf{\Omega}}^{3}$. Assuming a series of measured value

${y}_{n}={\left\{{x}_{k}\right\}}_{\mathsf{k}=\mathsf{n}}^{\mathsf{n}+\mathsf{N}-1}$. We employ a double hypothesis testing as such,

${\mathsf{H}}_{0}:$ MIMU stationary,

${\mathsf{H}}_{1}:$ MIMU moving. The false alarm probability is expressed as

The detection probability is ${\mathsf{P}}_{\mathsf{D}}=\mathsf{P}\left\{{\mathsf{H}}_{0}|{\mathsf{H}}_{0}\right\}$. Two hypotheses' observation data probability density functions are, respectively, defined as $p\left({y}_{n};{\mathsf{H}}_{0}\right)$ and $p\left({y}_{n};{\mathsf{H}}_{1}\right)$.

The mathematical sensor model can be expressed as ${x}_{k}={s}_{k}\left(\theta \right)+{v}_{k}$, where ${s}_{k}\left(\theta \right)={\left[\begin{array}{cc}{s}_{k}^{a}\left(\theta \right)& {s}_{k}^{\omega}\left(\theta \right)\end{array}\right]}^{\mathsf{T}}$ and ${v}_{k}={\left[\begin{array}{cc}{v}_{k}^{a}& {v}_{k}^{\omega}\end{array}\right]}^{\mathsf{T}}$, the force of MIMU is ${s}_{k}^{a}\left(\theta \right)\in {\mathsf{\Omega}}^{3}$, and MIMU angular rate is expressed as ${s}_{k}^{\omega}\left(\theta \right)\in {\mathsf{\Omega}}^{3}$. The symbol $\theta $ denotes the vector of unknown elements ${v}_{k}^{a}\in {\mathsf{\Omega}}^{3}$ accelerometers noise, ${v}_{k}^{\omega}\in {\mathsf{\Omega}}^{3}$ gyroscopes noise. Assume the noises follows zero mean Gaussian distribution, with noise covariance matrix $Z=E\left\{{v}_{k}{v}_{k}^{\mathsf{T}}\right\}=\left[\begin{array}{cc}{\sigma}_{a}^{2}{\mathsf{I}}_{3\times 3}& {0}_{3\times 3}\\ {0}_{3\times 3}& {\sigma}_{\omega}^{2}{\mathsf{I}}_{3\times 3}\end{array}\right]$, where ${\sigma}_{a}^{2}$ and ${\sigma}_{\omega}^{2}$, respectively, represent accelerometers and gyroscopes noise variance.

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

where:

GLRT is determined by the hypothesis

${\mathsf{H}}_{0}$ if

where

$\lambda $ denotes the threshold. In Equation (12),

${\widehat{\theta}}_{0}$ and

${\widehat{\theta}}_{1}$ represent the maximum likelihood estimate of the unknown element under the assumptions

${\mathsf{H}}_{0}$ and

${\mathsf{H}}_{1}$, respectively. Equation (12) can be simplified as

$T\left({y}_{n}\right)<\lambda $ means that the pedestrian is in a stationary state.

In practice, ZUPT can effectively aid inertial navigation system to remove long-time accumulated errors [

5,

17]. The velocity error of carrier is used as a concept [

18,

19]. When pedestrians stay static, the MIMU measured velocity is regarded as an error to correct the system using Kalman filtering.

The state error vector is defined as

which, respectively, represents the three-dimensional attitude error, gyro drift, position error, velocity error and accelerometer bias.

The zero-velocity correction Kalman filter model is

When the MIMU is stationary, the speed is zero, in theory; thus, the ZUPT speed measurement equation is

where the state transition matrix is given as

where

$S\left({f}_{k}^{n}\right)$ is the specific force anti-symmetric matrix, and

${H}_{k}=\left[\begin{array}{ccccc}{0}_{3\times 3}& {0}_{3\times 3}& {0}_{3\times 3}& {\mathsf{I}}_{3\times 3}& {0}_{3\times 3}\end{array}\right]$.

#### 3.2. The Ellipsoidal Constraint Method

Each foot are fixedly mounted by a MIMU. For regular human kinematics, the separation distance between the right and left feet cannot be larger than a quantity known as foot-to-foot maximum separation [

8,

9]. The maximum step size is a typical feature of pedestrian to walk and can be used to constrain the navigation error [

20,

21], namely, in addition to using ZUPT to improve the accuracy of pedestrian navigation. In specific, we decompose the constraint into three degrees of freedom and then use the obtained sub-constraints to correct the navigation system. Based on this intuition, we constrain the position estimate of right and left foot-mounted ZUPT-aided INSs.

According to the coordinate system identified of the MTI-G-700 units (3D motion tracking system, from Xsens Technologies B.V., Enschede, The Netherlands), the carrier coordinate system, as shown in

Figure 1, shows the

X_{b} axis is parallel to the surface of the MIMU, in the forward direction, and the

Z_{b} axis is perpendicular to the MIMU surface, in the upward direction. In this dual-MIMU integrated navigation system, the navigation coordinate system’s

X_{n} axis is forward, the

Y_{n} axis points to the right, and the

Z_{n} axis perpendicular to the

X_{n}OY_{n} plane, upwards. The coordinates of the navigation subsystem bound to the feet are defined in the same way.

For two MIMU navigation systems, the $i=\mathsf{L},\mathsf{R}$, system real state is described as ${x}_{k}^{i}$ (including position, velocity, and attitude), the estimated state as ${\widehat{x}}_{k}^{i}$ at the time $k$, where ${x}_{k}^{i}\in {\mathsf{\mathbb{R}}}^{{n}_{i}},{\widehat{x}}_{k}^{i}\in {\mathsf{\mathbb{R}}}^{{n}_{i}}$.

The joint state vector is defined as

where

${\widehat{x}}_{k}\in {\mathsf{\mathbb{R}}}^{m}\left({n}_{1}+{n}_{2}=m\right)$.

Letting the maximum step size of the pedestrian be given by

$\gamma $, the real displacement difference between the two navigation systems should be less than or equal to

$\gamma $. As the leg height is subject to certain constraints, during the pedestrian normal walking state, the positions of the right and left foot can be approximately constrained in a ellipsoid (

Figure 2). The position of one foot is constrained within the circle of radius

$\gamma $ in the

XOY plane, and is confined within the circle with leg-related radius

$h$ in

XOZ and

YOZ planes, both centered at the other foot (

Figure 3).

Assuming that left foot is on the ground and the right foot is in movement at moment

k (

Figure 3), then we can calculate

$\alpha $, defined as the angle between the position of the two feet in the

XOY plane (in navigation coordinate system):

As we can see from

Figure 2, there is a space azimuth

$\beta $ between the right and the static left foot, we can calculate this angle by the positional relationship between the feet:

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

Defining the matricies,

where

${\gamma}_{x}$ represents the real-time constraint value of the ellipsoid constraint on the

X_{n} axis, so

${\gamma}_{x}=\gamma \mathrm{sin}\alpha $;

${\gamma}_{y}$ represents the real-time constraint value of the ellipsoid constraint on the

Y_{n} axis, so

${\gamma}_{y}=\gamma \mathrm{cos}\alpha $;

${h}_{z}$ represents the real-time constraint value of the ellipsoid constraint on the

Z_{n} axis, so

${h}_{z}=h\mathrm{tan}\beta $.

We assume that two navigation systems attitude is accurate in the current moment when the decompose step size constraint. When

$\frac{{\Vert {L}_{s}\cdot {x}_{k}\Vert}^{2}}{{\gamma}^{2}}+\frac{{\Vert {L}_{h}\cdot {x}_{k}\Vert}^{2}}{{h}_{z}^{2}}>1$ can constraint the state to be satisfied with

$\left\{x\in {\mathbb{R}}^{m}:\frac{{\Vert {L}_{s}\cdot {x}_{k}\Vert}^{2}}{{\gamma}^{2}}+\frac{{\Vert {L}_{h}\cdot {x}_{k}\Vert}^{2}}{{{h}_{z}}^{2}}\le 1\right\}$ the state modification is recommended as

where

${P}_{k}^{-1}$ denotes the Kalman filter estimated covariance matrix state.

Defining

$L=\left[\begin{array}{cccc}\begin{array}{ccc}1/{\gamma}_{x}& 0& 0\\ 0& 1/{\gamma}_{y}& 0\\ 0& 0& 1/{h}_{z}\end{array}& {0}_{3\times 6}& \begin{array}{ccc}-1/{\gamma}_{x}& 0& 0\\ 0& -1/{\gamma}_{y}& 0\\ 0& 0& -1/{h}_{z}\end{array}& {0}_{3\times 6}\end{array}\right]$, Equation (22) can be written as

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

where

${Q}_{a}=\left[\begin{array}{ccc}{\sigma}_{ax}^{2}& 0& 0\\ 0& {\sigma}_{ay}^{2}& 0\\ 0& 0& {\sigma}_{az}^{2}\end{array}\right]$ ,

${Q}_{\omega}=\left[\begin{array}{ccc}{\sigma}_{\omega x}^{2}& 0& 0\\ 0& {\sigma}_{\omega y}^{2}& 0\\ 0& 0& {\sigma}_{\omega z}^{2}\end{array}\right]$ ,

${\sigma}_{a}={\left[\begin{array}{cc}0.5& \begin{array}{cc}0.5& 0.5\end{array}\end{array}\right]}^{\mathsf{T}}$ , and

${\sigma}_{\omega}={\left[\begin{array}{cc}0.5& \begin{array}{cc}0.5& 0.5\end{array}\end{array}\right]}^{\mathsf{T}}\times \pi /180$.

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

where

${\sigma}_{v}={\left[\begin{array}{cc}0.01& \begin{array}{cc}0.01& 0.01\end{array}\end{array}\right]}^{\mathsf{T}}$.

The sampling rate of the filter is 400 Hz.