Indoor Position Estimation Using Ultrasonic Beacon Sensors and Extended Kalman Filter ^†

Abstract

With the invention of GPS and related technologies, outdoor positional systems have become very accurate. However, there is still a need for efficient, reliable, and less expensive technology for indoor navigation. There are lots of techniques used for indoor navigation, such as acoustic, Wi-Fi-based, proximity-based, infrared systems and SLAM algorithms. In this study, accurate position estimation was attempted by combining the acceleration and gyroscope data and the raw distance data with the help of the Extended Kalman Filter (EKF). Initially, a position estimation was obtained using the Recursive Least Square (RLS) method with a trilateration algorithm. This solution was used as a starting point for RLS. Here, the first solution point is updated as the initial solution for each distance, and the result calculated by the RLS method is updated as the next solution. This approach enables the distance measurement and position estimation to be executed simultaneously, avoids the unnecessary waiting time, and speeds up the positioning estimation. After that, this position estimation is fused with the acceleration and gyroscope data. In order to test the designed algorithm, synthetic data were used. As a result of these tests, it has been observed that this EKF structure created for indoor navigation gives accurate results.

1. Introduction

In recent years, unmanned aerial vehicles (UAVs) have been widely used in military, industry, agriculture, and other areas, such as aerial photography and air reconnaissance [1,2,3]. All these specified areas are outdoors, so the UAVs usually receive the GPS signals properly. If there is no GPS signal or a weak GPS signal, however, positioning accuracy is directly affected. Nowadays, there is a great demand for UAV inspection based on indoor technology, and this demand is related to control optimization and path tracking.

There are a lot of techniques for indoor positioning, such as vision-based, lidar-based, Wi-Fi-based, Bluetooth-based, UWB-based and IMU-based techniques [4,5,6]. A motion-capturing system, which uses multiple high-speed cameras to obtain the relative position of the object, has the disadvantage of complex layout and difficult calibration. VICON and OptiTrack are examples of this system. Since positioning accuracy can now reach millimeter level, the disadvantages of the system have prompted the emergence of some simultaneous localization and mapping schemes, such as Oriented FAST and Rotated BRIEF SLAM (ORB-SLAM) [7], semi-direct visual odometry (SVO) [8] and direct sparse odometry (DSO) [9]; these schemes use a single monocular camera or a binocular camera placed on the UAV body to obtain the relative position of the UAV in the environment. In addition, Gmapping [10], Hector [11] and Cartographer [12] are examples of lidar-based positioning techniques. Due to the weight of the lidar, which is generally a single line applied to the UAV, only a two-dimensional position can be obtained. Using Wi-Fi for indoor positioning is well-established, and its accuracy can reach a few meters [13]. However, its requirements—in terms of the number of Wi-Fi access points associated with the costs and power consumption—make this solution impossible without consistent retrofitting. Bluetooth Low Energy (BLE) and Wi-Fi use the same frequency, but BLE is designed as a short-range, energy-efficient communication protocol, which allows devices to communicate through short messages [14]. BLE-based localization is typically performed by installing a set of proximity beacons at known locations. Receivers transmit the RSSI (distance from the sender) from the nearest beacons and calculate their own position using these values [15]. There are two categories for the BLE-based localization algorithm—distance-based and fingerprinting-based [16]. Distance-based algorithms directly translate RSSI values into the position coordinates. These methods require at least three RSSI measurements to estimate the position [17]. On the other hand, fingerprinting-based algorithms exploit a vector of RSSI measurements at known fingerprint positions to create a so-called reference fingerprint map (RFM). A machine learning regressor is then fed with the RFM data to create a relationship rule between new RSSI measurements and their corresponding position estimates [18]. UWB positioning is light in weight, simple in layout and stable in positioning, and the accuracy can reach to centimeter level [19,20]. Using only UWB cannot meet the requirements of an indoor high-precision operation. IMU is a common sensor for orientation estimation. IMU, however, estimates its position by integration, which accumulates errors due to drift.

In this paper, an accurate position estimation is calculated by combining the IMU and the raw distance data with the help of the Extended Kalman Filter (EKF). Initially, a position estimation is obtained using the Recursive Least Square (RLS) method with a trilateration algorithm. This solution is used as a starting point for RLS. This position estimation is then fused with acceleration and gyroscope data. These algorithm simulations are performed in a MATLAB environment. The average results show that the proposed algorithm gives accurate results within less than ten cm precision.

2. Position Estimation Algorithm

2.1. Geometric Approach

A geometric approach has been put forward in the basis of the study. As shown in the Figure 1 below, three reference points are given—B₁(x₁, y₁, z₁), B₂(x₂, y₂, z₂) and B₃(x₃, y₃, z₃)—and d₁, d₂ and d₃ interval measurements up to point A are given. The determination of the coordinates of point A is carried out by solving the system of quadratic equations.

$$\begin{gathered} {\left(x-x_1\right)}^2+{\left(y-y_1\right)}^2+{\left(z-z_1\right)}^2=d_1{}^2 \\ {\left(x-x_2\right)}^2+{\left(y-y_2\right)}^2+{\left(z-z_2\right)}^2=d_2{}^2 \\ {\left(x-x_3\right)}^2+{\left(y-y_3\right)}^2+{\left(z-z_3\right)}^2=d_3{}^2 \end{gathered}$$

(1)

The system of equations given here can expressed as follows:

$$\begin{gathered} \left(x^2+y^2+z^2\right)-2x_{1}x-2y_{1}y-2z_{1}z=d_1{}^2-x_1^2-y_1^2-z_1^2 \\ \left(x^2+y^2+z^2\right)-2x_{2}x-2y_{2}y-2z_{2}z =d_2{}^2-x_2^2-y_2^2-z_2^2 \\ \left(x^2+y^2+z^2\right)-2x_{3}x-2y_{3}y-2z_{3}z=d_3{}^2-x_3^2-y_3^2-z_3^2 \end{gathered}$$

(2)

In addition to that, this expression can be shown in matrix form as below.

$$\left[\begin{array}{c}1\\ 1\\ 1\end{array} \begin{array}{c}-2x_1\\ -2x_2\\ -2x_3\end{array} \begin{array}{c}-2y_1\\ -2y_2\\ -2y_3\end{array} \begin{array}{c}-2z_1\\ -2z_2\\ -2z_3\end{array}\right] \left[\begin{array}{c}{x}^2+y^2+z^2\\ x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}{s}_1^2-x_1^2-y_1^2-z_1^2\\ s_2^2-x_2^2-y_2^2-z_2^2\\ s_3^2-x_3^2-y_3^2-z_3^2\end{array}\right]$$

(3)

This matrix form generally can be expressed as follows.

$$\begin{gathered} A_0.x=b_0\hspace{1em}\hspace{1em}\hspace{1em}\mathrm{x}\in E \\ E=\left\{\hspace{1em}{(x_0,x_1,x_2,x_3)}^Tϵ{ }^{ } x_0=x_1^2+x_2^2+x_3^2\right\} \end{gathered}$$

(4)

While looking at the solution set of this system, it can be seen that there are two different approaches. The first approach is that points B₁, B₂ and B₃ are not on the same straight line, and the second approach is that the points are on the same straight line.

Case 1. B₁, B₂ and B₃ are not in a straight line

In this case, the following propositions are true. Rank(A₀) = 3, and dim(Kern(A₀)) = 1. Then the general solution of (4) can be shown as:

$$x=x_{p }+t.x_h$$

(5)

where t is a real coefficient, it is seen that x_p is the special solution of (4) and it is also the solution of the system $A_0. x=$ 0, which is a homogeneous system at x_h. The vectors x_p and x_h can be calculated using the Gaussian elimination method.

$$x_p={\left(x_{p0,}{x}_{p1,}{x}_{p2,}{x}_{p3}\right)}^T , x_h={\left(x_{h0,}{x}_{h1,}{x}_{h2,}{x}_{h3}\right)}^T ,x={\left(x_{0,}{x}_{1,}{x}_{2,}{x}_3\right)}^T$$

(6)

x_p, x_h and x are expressed as above. If we substitute these expressions in (6), we can obtain the expressions given below:

$$x_0=x_{p0}+t{x}_{h0, } x_1=x_{p1}+t{x}_{h1 , }{x}_2=x_{p2}+t{x}_{h2 } , x_3=x_{p3}+t{x}_{h3}$$

(7)

By using the constraint x ∈ E,

$$x_{p0}+t{x}_{h0}={\left(x_{p1}+t{x}_{h1}\right)}^2+{\left(x_{p2}+t{x}_{h2}\right)}^2+{\left(x_{p3}+t{x}_{h3}\right)}^2$$

(8)

$$\begin{gathered} t^2\left(x_{h1}^2+x_{h2}^2+x_{h3}^2\right)+t\left(2x_{p1}{x}_{h1}+2x_{p2}{x}_h+2x_{p3}{x}_{h3}-x_{h0}\right)+ \\ {x}_{p1}^2+x_{p2}^2+x_{p3}^2-x_{p0}=0 \end{gathered}$$

(9)

This is a quadratic equation in the form $a{t}^2+bt+c=0$ with the solutions.

$$t_{1/2}=\frac{-b\pm \sqrt{b^2-4ac}}{2a}$$

(10)

The solutions of the equation system can be shown below.

$$x_1=x_p+t_1{x}_h , x_2=x_p+t_2{x}_h$$

(11)

Case 2. B₁, B₂ and B₃ are in a straight line

Then the following propositions are true. Rank(A₀) = 2, and dim(Kern(A₀)) = 2. Then the general solution of (4) can be shown as:

$$x=x_{p }+t.x_{h1 }+k.x_{h2}$$

(12)

With real parameters t and k; x_p is a particular solution of (4), and x_h1 and x_h2 are two solutions of the homogeneous system A₀. x = 0. They are linearly independent solutions and, therefore, form a basis of Kern(A₀). If there are more than three reference points, the general solution can be found using the least square method as follows.

$$\widehat{x}={\left(A^{T}A\right)}^{-1}{A}^{T}b$$

(13)

The projection of p on the column space of A is

$$p=A{\left(A^{T}A\right)}^{-1}{A}^{T}b$$

(14)

In this case, the coordinates of p in the Col (A) column space represent the$\widehat{x}$ solution. Although, if the measurements are uncorrelated but have different uncertainties, Weighted Least Squares (WLS) is used. In this case, the solution of $\widehat{x}$ is found with the help of the following expression:

$$\widehat{x}={\left(A^T{V}^{-1}A\right)}^{-1}{A}^T{V}^{-1}b$$

(15)

This solution is used as a starting point for the Recursive Least Square (RLS). Let x₀ be the initial solution, and by every incoming distance, x₀ is updated in x₁ by using the RLS. The approach enables distance measurement and positioning calculation to be executed simultaneously. Hence, a position assignment can be initiated although not all distances are known. This avoids the unnecessary waiting time and speeds up the positioning calculation. More detail for this approach is discussed in [21]. Distance data is used together with the RLS algorithm to help in calculating the position. In the next section, the details of a more accurate position estimation with the help of EKF will be explained. A sensor fusion algorithm is used with accelerometer, gyroscope and distance data, and the position is calculated.

2.2. Sensor Fusion Algorithm

There are lots of sensor fusion algorithms, such as Feature Aggregation, Temporal Fusion, Support Vector Machine, Kalman Filter, etc. Although, in this system, Kalman Filter was selected for use to perform a more accurate position estimation. Kalman Filter gives good results in linear systems, but since there are very few linear systems in the real world, the Extended Kalman Filter (EKF) is used, which gives better results in non-linear systems. The EKF solves this problem by calculating the Jacobian of F and H around the estimated states, which in turn, yields a trajectory of the model function around the stated. The details of the EKF that are utilized in this work are presented. The nonlinear process model and noise used in EKF are as given:

$$x\left(k+1\right)=f\left(x\left(k\right),u\left(k\right)\right)+w\left(k\right)$$

(16)

In this equation, $x\left(k\right)$ and $x\left(k+1\right)$ represents the states of the system at $k$ and $k+1$, respectively. In addition, $u\left(k\right)$ and $w\left(k\right)$represent the control signal and the process noise (in Gaussian distribution), respectively. The process is expressed by f(.). The measurement model, which relates the state variables to the measurements, is expressed with the following equation:

$$z\left(k\right)=h\left(x\left(k\right)\right)+v\left(k\right)$$

(17)

In this equation, $h(.)$, $v\left(k\right)$ and $z\left(k\right)$ represent nonlinear measurement function, measurement noise (in Gaussian distribution) and measurements, respectively. In EKF, the filter gain is calculated in the same way as in the linear Kalman Filter. For this reason, the nonlinear process and measurement models are linearized around the current system states. This linearization is performed using the first terms of the Taylor series expansion of the function of interest.

$$x\left(k+1\right)\approx \tilde{x}\left(k+1\right)+F\left(x\left(k\right)-\tilde{x}\left(k\right)\right)+\mathsf{\Gamma }w\left(k\right)$$

(18)

The mean value of the noise is zero. (w = 0)

$$\tilde{x}\left(k+1\right)\approx f\left(x\left(k\right),0\right)$$

(19)

The F matrix is the Jacobian matrix of the process function ($f$), according to the states ($x$). The Γ matrix is the Jacobian matrix of the process function with respect to the noise ($w$).

$$F_{i,j}={\frac{\partial f_i}{\partial x_j}|}_{\left(\widehat{x}\left(k+1\right),0\right)},{\mathsf{\Gamma }}_{i,j}={\frac{\partial f_i}{\partial w_i}|}_{\left(\widehat{x}\left(k+1\right),0\right)}$$

(20)

Similarly, the nonlinear measurement function is linearized around the predicted states.

$$z\left(k+1\right)\approx \tilde{z}\left(k+1\right)+H\left(x\left(k+1\right)-\widehat{x}\left(k+1\right)\right)+\mathsf{\Phi }v\left(k+1\right)$$

(21)

The expected noise value is zero (v = 0):

$$\tilde{z}\left(k+1\right)=h\left(\tilde{x}\left(k+1\right),0\right)$$

(22)

The H matrix is the Jacobian matrix ($x$), according to the system states of the measurement function ($h$). Likewise, the Φ matrix is the Jacobian matrix with respect to the measurement noise ($v$) of the measurement function. The general schematic of the EKF structure used in the system is given in Figure 2 as follows:

The state vector of the system is given as:

$$x=\left[\begin{array}{c}po{s}_x\\ po{s}_y\\ po{s}_z\\ ac{c}_x\\ ac{c}_y\\ ac{c}_z\\ gyr{o}_x\\ gyr{o}_y\\ gyr{o}_z\end{array}\right]$$

(23)

The state transition of the system is given as:

$$A=\left[\begin{array}{ccccccccc}1& 0& 0& dt& 0& 0& {dt}^2/2& 0& 0\\ 0& 1& 0& 0& dt& 1& 0& {dt}^2/2& 0\\ 0& 0& 1& 0& 0& dt& 0& 0& {dt}^2/2\\ 0& 0& 0& 1& 0& 0& dt& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& dt& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& dt\\ 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$$

(24)

The measurement vector is given as:

$$z=\left[\begin{array}{c}distance\\ ac{c}_x\\ ac{c}_y\\ ac{c}_z\\ gyr{o}_x\\ gyr{o}_y\\ gyr{o}_z\end{array}\right]$$

(25)

The measurement noise covariance matrix, R, was determined based on the average noise levels of measurements. Assuming that the measurements are not correlated with each other, the diagonal matrix below is chosen. The standard deviation values of the measurements are calculated, and the measurement noise covariance matrix is decided by using these values.

R = diag([r₁ r₂ r₃ 0 0 0 r₇ r₈ r₉])

(26)

Here, r₁…r₉ values are the distances of the beacon sensors from each other. The resulting process noise covariance matrix is as follows:

$$Q=\left[\begin{array}{ccccccccc}0.01& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0.01& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0.0001& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0.25& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0.16& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0.01& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0.25& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0.16& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0.01\end{array}\right]$$

(27)

In the next section, the results obtained by applying the designed algorithm on the simulation will be explained.

3. Simulation System and Results

The MATLAB environment was used while creating the simulation system. The data were produced synthetically in MATLAB, and the algorithm was tested under the generated data. In addition, three different trajectories were used while generating synthetic data. These trajectories are shown in

Table 1. Trajectories.

Trajectory Name	Beacon Number	Trajectories
Trajectory 1	5
Trajectory 2	5
Trajectory 3	5

. In Figure 3, Figure 4 and Figure 5, both the position calculations were calculated with the trilateration algorithm alone, and the position calculations obtained as a result of using the IMU data together with the trilateration and EKF algorithms are shown. The minimum, maximum and average error amounts of the calculated positions are shown in detail in

Table 2. Error Comparison Table.

Trajectory	Algorithm	Min Error (m)	Mean Error (m)	Max Error (m)
Trajectory 1	Trilateration Algorithm	0.073409	0.335606	0.994064
Trajectory 1	Trilateration Algorithm + EKF	0.019016	0.061539	0.143751
Trajectory 2	Trilateration Algorithm	0.077657	0.453345	1.768042
Trajectory 2	Trilateration Algorithm + EKF	0.016976	0.082958	0.168863
Trajectory 3	Trilateration Algorithm	0.353665	1.258376	5.003108
Trajectory 3	Trilateration Algorithm + EKF	0.001041	0.083402	0.164720

. The RMS value was used while generating error amounts. According to the simplicity and complexity of the determined trajectories, the error amounts obtained by using only the trilateration algorithm differ. For example, the error amounts of position estimation and position estimation obtained by using only the trilateration algorithm between Trajectory 1 and Trajectory 3 differ considerably. However, with the inclusion of EKF in the calculation of position estimation, it is easily observed that the amount of error obtained decreases both in the relevant Figure 3, Figure 4 and Figure 5 and in the values given in Table 2.

4. Conclusions

In this study, position estimation was made by combining IMU and raw distance data with the help of the Extended Kalman Filter (EKF). Simulation of the system is carried on in the MATLAB environment. The simulation result shows that the proposed method gives the correct position in centimeter precision levels. First, a geometric solution method was used in the algorithm; then this method was combined with the EKF algorithm. When the results are examined, it is observed that the amount of error is quite high when only the geometric approach is used. It has been seen that the position estimation has reached the desired level with the use of EKF as well as the geometric approach. In the future, this designed algorithm will be tested with the real sensor data. If the obtained results are at the desired level, the integration of the algorithm into the UAV will be started.