Enhancing Indoor Position Estimation Accuracy: Integration of Accelerometer, Raw Distance Data, and Extended Kalman Filter in Comparison to Vicon Motion Capture Data ^†

Abstract

Indoor positioning systems are a significant area of research and development, helping people navigate within buildings where GPS signals are unavailable. These systems have diverse applications, including aiding navigation in places like shopping malls, airports, and hospitals and improving emergency evacuation processes. The purpose of this study is to evaluate various technologies and algorithms used in indoor positioning. This study focuses on using raw distance data and Kalman filters to enhance indoor position accuracy. It employs a trilateration algorithm based on Recursive Least Squares (RLS) for initial position estimation and combines the results with accelerometer data. The designed algorithm using real sensor data collected in an ROS(Robot Operating System) environment was tested, and the results obtained were compared with data obtained from the Vicon Indoor Positioning System. In this comparison, the Root Mean Square Error metric was used. As a result of the comparison, it was observed that the error obtained from the designed algorithm is less than that of the Vicon system.

1. Introduction

In recent years, Unmanned Aerial Vehicles (UAVs) have found extensive applications in various fields, including military, industry, agriculture, and tasks like aerial photography and reconnaissance [1,2,3]. However, it is worth noting that these applications are primarily designed for the outdoors and rely on robust GPS signals for accurate positioning. In cases where GPS signals are unavailable or weak, the precision of UAV positioning is significantly impacted. Presently, there is a growing need for indoor UAV technology, particularly for inspection purposes, and this area is closely tied to control optimization and precise path tracking.

Ultra-Wideband (UWB) technology has garnered significant attention due to its high precision in indoor positioning. UWB systems leverage short-duration pulses of radio waves spread across a wide spectrum, enabling accurate distance measurements using time-of-flight and trilateration techniques. Aiello and Shalom [4] delved into the principles of UWB and its significance in achieving precise and real-time positioning. Heimovirta, Salantera, and Röning [5], discuss the practical implementation of a real-time indoor localization system based on UWB technology. This work focuses on the utilization of UWB technology for indoor positioning, providing insights into its application in real-world scenarios.

Vicon positioning systems are based on motion capture technology, utilizing cameras and markers to accurately track the movement and position of objects or individuals in a controlled environment. Rhea [6] assessed the accuracy and precision of Vicon motion capture systems for tracking movements in three dimensions (3D). This work focused on evaluating the reliability of Vicon systems in capturing 3D motion data. Gentil, de la Rouviere, and Elton [7] discuss Vicon as a computer vision solution for real-time 3D motion capture. Their work highlights the capabilities of Vicon technology in capturing and analyzing 3D motion data in real-time, particularly in the context of rehabilitation and neuro-engineering. Benn and Martin [8] provide insights into the utilization of Vicon systems for capturing and analyzing motion data in the context of sports and healthcare.

In this paper, an accurate position estimation was made by combining the IMU and the raw distance data with the help of the Extended Kalman Filter (EKF). Initially, position estimation was obtained using the RLS method employed via a trilateration algorithm. This solution was used as a starting point for RLS. Afterward, this position estimation was fused with the acceleration data. As a result, the estimated position obtained with the designed EKF was compared with the Vicon indoor positioning system, and the results are presented both graphically and in tabular form.

2. Position Estimation Algorithm

The position estimation algorithm is based on a geometric approach and EKF algorithms. After these algorithms were designed in a simulation environment, they were integrated into an ROS and tested with real sensor data, and the results were observed.

2.1. Geometric Approach

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

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

(1)

The system of equations given here can be 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, this expression can be shown in matrix form, as presented 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 can generally be expressed as follows:

$$\begin{gathered} A_0.x=b_0 \mathrm{x} \mathbf{\in } E \\ E=\{{ {{(x}_0,x_1,x_2,x_3)}^Tϵ }^{ }{ x}_0=x_1^2{+x}_2^2+x_3^2\} \end{gathered}$$

(4)

When examining the solution set, there are generally two different approaches. These are divided into three-reference-point-based and more-than-three-reference-point-based solutions. The details of these solution sets are elaborated in [9,10]. In brief, for the first scenario, the solution space varies depending on whether three reference points are in the same line. The second scenario involves having more than three reference points, which necessitates the use of the recursive least squares method for the solution set.

Distance data are used together with the recursive least square algorithm to aid in calculating the position. In the next section, the details of a more accurate position estimation with the help of EKF will be explained.

2.2. Sensor Fusion Algorithm

The system’s state vector was set as

$$X=\left[\begin{array}{c}p\left(w\right)\\ v\left(w\right)\\ a\left(w\right)\end{array}\right]$$

(5)

The vector p(w) denotes an object’s position in the world coordinate system, specifying its coordinates on the x, y, and z axes as [px, py, pz]. The object’s velocity along these axes in the world coordinate system is represented as v(w) = [vx, vy, vz], while its accelerometer data in the world coordinate system are denoted as a(w) = [ax, ay, az].

To elaborate further, the time interval between measurements is defined as ∆t, and (∆t)w(k) denotes the process noise related to acceleration. Specifically, $\frac{{∆t}^2}{2}w\left(k\right)$ represents the process noise affecting velocity, and $\frac{{∆t}^3}{6}w\left(k\right)$ indicates the process noise that influences position.

Equations of motion in the (k + 1) time interval can be expressed as follows:

$$\begin{gathered} p\left(k+1\right)=p\left(k\right)+v\left(k\right)\left(∆t\right)+a\left(k\right)\frac{{∆t}^2}{2}+\frac{{∆t}^3}{6}w\left(k\right) \\ v\left(k+1\right)=v\left(k\right)+a\left(k\right)\left(∆t\right)+\frac{{∆t}^2}{2}w(k) \\ a\left(k+1\right)=a\left(k\right)+\left(∆t\right)w\left(k\right) \end{gathered}$$

(6)

The state equation can be represented in matrix form as follows:

$$x\left(k+1\right)=Ax\left(k\right)+Gw\left(k\right)$$

(7)

The matrices A and G represent the transition matrix and noise process matrix, respectively. The process noise vector and Q covariance can be defined as follows:

$$\begin{gathered} w\left(k\right)=[w_x\left(k\right) w_y\left(k\right) w_z\left(k\right)], Q=diag\left(\right[{\sigma }_{ax}^2 {\sigma }_{ay}^2 {\sigma }_{az}^2\left]\right) \\ A=\left[\begin{array}{c} 1 \\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\begin{array}{c}0 \\ 1 \\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\begin{array}{c}0 \\ 0\\ 1\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\begin{array}{c}∆t \\ 0\\ 0\\ 1\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\begin{array}{c}0 \\ ∆t\\ 0\\ 0\\ 1\\ 0\\ 0\\ 0\\ 0\end{array}\begin{array}{c}0 \\ 1\\ ∆t\\ 0\\ 0\\ 1\\ 0\\ 0\\ 0\end{array}\begin{array}{c}{∆t}^2/2 \\ 0\\ 0\\ ∆t\\ 0\\ 0\\ 1\\ 0\\ 0\end{array}\begin{array}{c}0 \\ {∆t}^2/2\\ 0\\ 0\\ ∆t\\ 0\\ 0\\ 1\\ 0\end{array}\begin{array}{c}0 \\ 0\\ {∆t}^2/2\\ 0\\ 0\\ ∆t\\ 0\\ 0\\ 1\end{array}\right],G=\left[\begin{array}{ccc}\frac{{∆t}^3}{6}& 0& 0\\ 0& \frac{{∆t}^3}{6}& 0\\ 0& 0& \frac{{∆t}^3}{6}\\ \frac{{∆t}^2}{2}& 0& 0\\ 0& \frac{{∆t}^2}{2}& 0\\ 0& 0& \frac{{∆t}^2}{2}\\ ∆t& 0& 0\\ 0& ∆t& 0\\ 0& 0& ∆t\end{array}\right] \end{gathered}$$

(8)

The observation vector z(k) contains the distance values used in the geometric approach and an additional noise vector:

$$z\left(k\right)=\left[\begin{array}{c}{d}_1\left(k\right)+n_1\left(k\right)\\ d_2\left(k\right)+n_2\left(k\right)\\ d_3\left(k\right)+n_3\left(k\right)\\ d_4\left(k\right)+n_4\left(k\right)\end{array}\right]=H\left(k\right)x\left(k\right)+n(k)$$

(9)

The observation matrix is represented by H(k), and the vector of noise, with a mean of zero and a covariance matrix, is represented by n(k).

$$R=diag\left(\right[{\sigma }_{r1}^2{\sigma }_{r2}^2{\sigma }_{r3}^2{\sigma }_{r4}^2\left]\right)$$

(10)

Here, r1, r2, r3, and r4 are the distances between each of the values of the Marvelmind sensor. The H(k) observation matrix is a Jacobian matrix and is calculated as follows:

$$H\left(k\right)=\left[\begin{array}{ccc}\frac{\delta d_1\left(k\right))}{\delta p_x\left(k\right)}& \frac{\delta d_1\left(k\right)}{\delta p_x\left(k\right)}& \frac{\delta d_1\left(k\right)}{\delta p_x\left(k\right)}\\ \frac{\delta d_2\left(k\right)}{\delta p_x\left(k\right)}& \frac{\delta d_2\left(k\right)}{\delta p_x\left(k\right)}& \frac{\delta d_2\left(k\right)}{\delta p_x\left(k\right)}\\ \frac{\delta d_3\left(k\right)}{\delta p_x\left(k\right)}& \frac{\delta d_3\left(k\right)}{\delta p_y\left(k\right)}& \frac{\delta d_3\left(k\right)}{\delta p_z\left(k\right)}\\ \frac{\delta d_4\left(k\right)}{\delta p_x\left(k\right)}& \frac{\delta d_4\left(k\right)}{\delta p_x\left(k\right)}& \frac{\delta d_4\left(k\right)}{\delta p_x\left(k\right)}\end{array}\right]$$

(11)

The equations described in the structure of the applied EKF are utilized. The initialization stage of this filter is one of the crucial factors, where the initial values of the covariance matrices are assigned. Subsequently, the prediction and update steps are executed in sequence.

$$\begin{gathered} \overline{\mathrm{x}}\mathrm{ₖ}=\mathrm{f}\left(\widehat{\mathrm{x}}{\mathrm{ₖ}}^{-1},\mathrm{u}\mathrm{ₖ}\right) \\ \overline{\mathrm{P}}\mathrm{ₖ}=\mathrm{A}{\mathrm{ₖ}}^{-1}\widehat{\mathrm{P}}{\mathrm{ₖ}}^{-1}\mathrm{A}{\mathrm{ₖ}}^{-1}\mathrm{ᵀ}+\mathrm{Q}{\mathrm{ₖ}}^{-1} \\ {K}_k={{\overline{P}}_k{H}_k^T[H_k{\overline{P}}_k{H}_k^T+R_k]}^{-1} \\ \widehat{\mathrm{x}}\mathrm{ₖ}=\overline{\mathrm{x}}\mathrm{ₖ}+\mathrm{K}\mathrm{ₖ}\left(\mathrm{z}\mathrm{ₖ}-\mathrm{h}\left(\overline{\mathrm{x}}\mathrm{ₖ}\right)\right) \\ {\widehat{P}}_k=(I-K_k{H}_k){\overline{P}}_k \end{gathered}$$

(12)

In the following section, the integration of this geometric approach with EKF algorithms into the ROS platform is described, and its implementation with a real dataset is discussed. The obtained results are presented in detail.

3. Implementation and Simulation System Results

After the geometric approach and EKF structure were designed, they were transferred to the simulation environment using C++. The code structure was integrated into the PX4 code, making it compatible with the ROS environment. The acceleration data from UWB sensors are received at approximately 40 Hz, while the raw distance data arrive at around 80 Hz. To address this discrepancy, time synchronization was achieved between the acceleration and distance data. The acceleration data were upsampled to 80 Hz using interpolation. Additionally, since Vicon data are received at a rate of 20 Hz, it was necessary to perform time synchronization with the position data obtained from the EKF results. Therefore, the Vicon data were upsampled to 80 Hz using interpolation to ensure proper time synchronization.

When looking at Figure 2 and Figure 3, it is easily noticeable that the X, Y, and Z positions obtained from the EKF are closer to the reference results than those obtained from the Vicon system. Additionally, Figure 3 depicts the differences between the EKF and Vicon in relation to their respective reference values. Upon examining the position error values in

Table 1. Position error values.

	Minimum Error (m)	Mean Error (m)	Maximum Error (m)
Ref-EKF	0.019	0.205	0.424
Ref-Vicon	0.07	0.255	0.441

, it is evident that the average difference between the EKF and the reference is 0.205 m, while for the Vicon, this difference is 0.255 m. Moreover, when looking at the minimum and maximum errors, it is evident that the values obtained from the EKF are lower than those obtained from the Vicon system. In summary, through a numerical comparison of the results, it is apparent that the designed EKF algorithm provides better results compared to the Vicon positioning system.

4. Conclusions

In this study, position estimation was carried out by fusing raw distance data with the IMU using sensor fusion algorithms, specifically via the EFK. The fundamental structure of the designed algorithm incorporates a geometric approach. Firstly, a position calculation was derived from the geometric approach, and then the accuracy of this position was enhanced using accelerometer data and the EKF algorithm. The designed algorithm was transformed for incorporation into the C++ environment and integrated into an ROS. Real sensor data is used during the testing of the algorithm. Sensor data were collected using the ROS platform, and the algorithm was executed within the ROS to observe the obtained results. According to the results obtained, the designed EKF structure yielded more successful outcomes compared to the Vicon position system. These results are supported both graphically and by numerical tables.