In this section, we illustrate the target coarse estimation, which includes the ultrasonic and inertial sequences.
4.3.1. Time Difference of Arrival Estimation
We detail the TDOA estimation for the ultrasonic localization in
Figure 4. Let the system comprise anchors
A1 and
A2 and target
M.
Suppose at time anchor A1 transmits a message that arrives at anchor A1, A2 and M at and , respectively. At time , anchor A2 sends a message, which is received at times and , respectively. Here, it is unknown for and .
Consider to be the transmission interval between anchors A1 and A2 in a common reference time and and to be the distances from anchor A1’s speaker to the microphone and anchor A2’s speaker to the microphone, respectively. We use to express the distance from anchor Ai’s speaker to anchor Aj’s microphone. Let c be the speed of acoustic propagation in the air.
We can obtain the timestamps
t1 and
t2 at which anchor
A2 sends messages from anchors
A1 and
A2The timestamps
t3 and
t4 at which anchor
A1 sends messages from anchors
A1 and
A2 are obtained separately
Therefore, the transmission interval
can be expressed as follows:
Then, it can be further simplified as
After determining the transmission interval
, the TDOA from anchors
A1 and
A2 on target
M can be calculated. The time calculations are all based on the same device clock system for time subtraction. Therefore, the distances difference from anchors
A1 and
A2 to target
M can be computed as
where
c is the speed of acoustic signal propagation in the atmosphere.
At least three anchors are needed to achieve localization in the location estimation. The TDOA hyperbolic model [
38] is shown in
Figure 5. We assume the target location
and anchor locations
,
.
is the distance difference from anchors
Ai and
Aj on Target
M. The location can be achieved by using Equation (9).
Suppose
Ai’s location is
. Then, the distance between the target
M and anchor
Ai is
where
.
Let
denote the distance difference from anchors
Ai and
A1 on Target
M then
Equation (13) can be expanded and expressed as
When
i = 1, Equation (11) simplifies to
Equation (14) is subtracted from Equation (15) to obtain
where
,
.
From Equation (16), we obtain
Then, we can obtain the matrix
where
The location can be computed as
4.3.2. Improved Pedestrian Dead Reckon Estimation
In this part, we elaborate on the improved pedestrian dead reckoning algorithm, which includes step length estimation, heading direction, and dead reckon estimation.
While walking, the acceleration of the pedestrian changes approximately periodically. The alternating transformation of the peak and valley of the acceleration wave once is equivalent to one step, so peak detection can be used to estimate the step frequency and step length. The detection process is mainly divided into the following three parts:
If the acceleration at time t is greater than the acceleration at time and at time , then is considered the peak.
- (2)
Threshold setting:
To ensure the validity of these peaks and valleys, the peak threshold is set. If the detected peak is less than the preset peak threshold, it will be discarded.
- (3)
Step determination:
If , the peak is recorded as a valid peak. is the time of the (i − 1)-th step, and is the training value of the time taken by the pedestrian to walking one step.
The step detection algorithm is depicted in Algorithm 1.
Algorithm 1: Step Detection Algorithm |
Input: Acceleration sequence . |
Output: i-th step . |
1: Setting Threshold and one-step taken time ; |
2: for i = 1: do |
3: if is the Peak value then |
4: if and - then |
5: |
6: end if |
7: end if |
8: end for |
While walking, the step length in the current state is not only related to the current acceleration but also to the previous acceleration. It is not accurate to estimate the step length considering only the current motion state.
Inspired by the literature [
39], our method is not only related to the peak and valley values of acceleration in the current step but also accounts for the previous two steps. It can reflect the continuity and similarity of the moving process. Due to equipment errors during acceleration acquisition, we have added to compensate for the error caused by the equipment in the step length estimation. Meanwhile, during the estimation process, the attention mechanism scheme is also adaptively designed by calculating the difference between the maximum and minimum values of acceleration within each step, which has better adaptability than fixed weight estimation.
The improved step length estimation method integrates the previous two step lengths as follows:
where
,
are the (
i − 1)-th and (
i − 2)-th step length, respectively
is the weighting vector,
K is the model parameter,
and
are the peak and valley values of the acceleration in the
i-th step, and
is the accelerometer compensation, which is measured at a stationary time.
Tests with several static states have been conducted in experiments. We have carried out some experiments with Scarlet [
40], Kim [
41], Weinberg [
42] and our method.
Figure 6 shows the comparison of the step length estimation at the trial site when the pedestrians walk at 0.6 m per step. The mean step length of our improved step model is 0.5939 m. The mean step lengths of the Weinberg, Scarlet, and Kim models are 0.5645 m, 0.6228 m, and 0.5567 m, respectively. The experimental results show that our method has higher stability and accuracy. This result is mainly due to the improved method, which can extract more features related to the current step than the Scarlet, Kim, and Weinberg methods. At the same time, the different attention values of each step are calculated adaptively and can predict more accurate pedestrian states.
According to Euler’s theorem, the quaternion-based attitude matrix description [
43] is the transformation of the target coordinate system to the Earth coordinate system, as shown in Equation (24),
where
denotes the rotation matrix of the quaternions.
denotes the posture quaternion, which is the unit vector,
is the quaternion coefficient of the scalar part, whose value is equal to the cosine of half of the rotation angle of the coordinate system,
denote the part of the vector, and
satisfied the following constraint:
Since the gyroscope data can be used to estimate the heading angle from the differential equations of motion and gravitational acceleration and geomagnetic intensity can be used to correct the heading angle, the extended Kalman filter (EKF) is used to estimate the heading angle. The equation of state and the measurement equation of the EKF are expressed as Equations (26) and (27). The state vector is represented by the quaternion
, and the measurement equation is a combination of the acceleration and magnetometer measurement data.
where
and
measured by the acceleration and magnetometer sensors at time
k, respectively,
is the state transfer matrix,
is the process noise, and
is the measurement noise.
is the attitude rotation matrix,
is the normalized gravity vector, and
is the normalized magnetic field strength vector.
After the EKF prediction update, the optimal state vector can be iterated for each moment then the estimated optimal heading angle is as shown in Equation (28).
The dead reckon schematic diagram (considering only the two-dimensional case) is shown in
Figure 7.
Therefore, the current location
for the
i-th step can be estimated according to the previous step, step length and heading direction. It is computed by the following formula:
where
is the
i-th step length and
indicates the heading direction of the
i-th step.