1.
INTRODUCTION
The demand for systems that enable navigating objects in closed spaces has been constantly increasing in recent years. Due to limited capability of using Global Navigation Satellite Systems (GNSS) in such conditions, many institutions and tech companies are looking for solutions that guarantee accurate and continuous positioning inside buildings. Among the available solutions, Inertial Navigation Systems (INS) are often found. In such systems, positioning is conducted by processing data about accelerations and angular rates. They are obtained from Inertial Measurement Units (IMU), which consist of accelerometers and gyroscopes1. Other solutions use some local infrastructure that is deployed around or inside the building. For this purpose, ultrawideband (UWB) modules can be used, whose numerous advantages have made them commonly used in navigation2-4.
When designing a navigation system, a choice of positioning algorithm is very important. A Kalman Filter (KF) and its modifications are frequently used for this purpose. Their performance strongly depends on the adjustment of the algorithm to a given system5. In this paper, an Unscented Kalman Filter (UKF) for personal navigation system, developed by the authors, is proposed.
2.
DESCRIPTION OF THE PERSONAL NAVIGATION SYSTEM
The developed personal navigation system is based on ultrawideband modules, whose main task is to measure the distances between each other. Thanks to many advantages of the ultrawideband technology, it is possible to achieve high accuracy of the measurements and, consequently, of the positioning4. The obtained measurements are subsequently processed using algorithms such as Kalman Filter or one of its many modifications.
The proposed system is composed of two types of devices with different tasks. The first group, which consists of the base stations (beacons), is responsible for determining the local coordinate system. These stations are deployed around the building or around the other area of interest. The user is equipped with a mobile station, which is an essential element of the entire system. Its task is to initiate distance measurements to the deployed beacons and to process acquired data.
The knowledge of the coordinates of the beacons and the distances of the mobile station from them makes it possible to estimate the user’s position2. The developed system consists of four base stations and one mobile unit (Figure 1), however, the number of both base and mobile stations could be easily increased, and this would allow to operate in a larger area.
Each of the four beacons contains a PulsON 440 UWB radio module6, STM32 NUCLEO-L432KC microcontroller board and a power bank. The microcontroller is tasked to configure the UWB module by sending commands to set the desired operation mode.
The mobile station is much more complex than the beacon. It also contains a PulsON 440 UWB module and a microcontroller, however, their tasks are different than the ones in the beacons. The microcontroller configures the radio module and periodically forces it to measure ranges to the base stations. Furthermore, it processes collected data with the implemented positioning algorithm. The mobile station is also equipped with the visualization system which is composed of a single-board computer RaspberryPi and an LCD screen. All the data inside a single base station and inside the mobile station of the designed system are sent via USART interfaces.
3.
UNSCENTED KALMAN FILTER
The Kalman Filter algorithm uses a dynamics model describing object’s motion and it is the optimal algorithm for its state estimation if the model of the system is linear, and when disturbances of the process and measurements errors are additive and Gaussian. However, in most systems of interest the dynamics and observation equations are non-linear2,7. When at least one of the equations describing the system is non-linear, the Kalman Filter cannot be applied. Therefore, suboptimal non-linear filtration algorithms such as an Extended Kalman Filter (EKF), a Linearized Kalman Filter (LKF) and an Unscented Kalman Filter have been developed7.
Both in the EKF and in the LKF, non-linear functions are locally approximated with linear equations obtained from Taylor expansion7. In a highly non-linear systems such solution can introduce significant errors because the linearization uses only the first term of a Taylor series. In such cases, it may be beneficial to use the UKF algorithm that, as an alternative to linearizing non-linear equations, uses non-linear transformations on a set of deterministically chosen sigma points. Such operation is called an unscented transformation (UT) and it takes advantage of the fact that after non-linear transformations, the mean and the covariance matrix of the transformed points are accurate to the second order of the Taylor series expansion7-11. As it was already stated, in algorithms that use linearization of the non-linear equations this accuracy is preserved only to the first order of the Taylor expansion. An additional advantage of the UKF algorithms is its easier implementation, because there is no need to calculate elements of the Jacobian matrix, which is often nontrivial. These operations are replaced with transformations of a set of sigma points7-9.
The developed system is described by a non-linear observation model which represents a relationship between the measured ranges and the object’s position and that is why one of possible non-linear filtration algorithms should be applied. In our case the Unscented Kalman Filter has been chosen.
A PV (Position/Velocity) dynamics model has been adopted in our algorithm. Therefore, the state vector consists of four variables. Two of them are for the object’s position and the other two for the object’s velocity. They describe the object’s movement in the OXY plane2,7:
One of the equations describing the developed system is the dynamics model. It is a linear difference equation which represents a relationship between the object’s successive positions and velocities. Such a model is given as follows7:
where: Φ – transition matrix, w – vector of random process disturbances.
The dynamics model presented as a matrix equation is as follows:
The influence of disturbances on the individual components of the object’s position and velocity during the timestep T is described by the Q matrix which is called the covariance matrix of discrete disturbances of the process w. It is given as follows2,7:
where the quantities 
and 
used in the above matrix express power spectrum densities of disturbing white noises for X and Y axes, respectively.
The relationship between the state vector and the measurement vector is described by the observation model which is given as follows7:
where: z – measurement vector, h – non-linear observation function, v – vector of measurement errors.
The observation model expressed as a matrix equation can be described as follows:
In order to estimate the state of the developed system, which is described by the above relationships, the UKF algorithm composed of the following steps can be used (initialization step is performed once while the other steps are performed recursively)3,7-11:
In the proposed algorithm, the prediction step can be carried out as in the linear Kalman filter with the above equations. It is possible, because only the observation model is non-linear7.
In order to use the unscented transformation, sigma points and their weights are generated. Their number depends on the size of the state vector n and typically equals 2n+1. To calculate these points, predicted values of the state vector and the covariance matrix of prediction errors are used3,7-11. Sigma points can be calculated as follows:
where:
x̂(k + 1|k) – state vector estimated at the moment (k + 1)T,
P(k + 1|k) – covariance matrix of prediction errors at the moment (k + 1)T,
χi (k + 1|k) – i-th sigma point,
λ – scaling parameter,
– i-th row or column of the of the matrix being a square root of (n + λ)P(k + 1|k), which is typically obtained with the use of Cholesky decomposition from the lower triangular matrix.
In order to correct the predicted values, it is necessary to calculate also weights of the generated points, using the following formulas3,7-11:
Calculated quantities Wm and Wc are used to predict the measurement vector and to calculate the covariance matrix in the correction step.
The β parameter, which is used to calculate weights, determines the distribution of the probability density function of the state vector and for the Gaussian distribution it is usually set equal to 2. The symbol λ used in the (11) and (12) relationships is a scaling parameter. It has been described in the literature devoted to the Scaled Unscented Transformation and it determines the extent of the spread of the sigma points around the mean. It is calculated using the following relationship3,12:
Typical range for the κ parameter that appears in the (13) equation is chosen from a range 〈0; ∞), while the range of values for the α parameter is (0; 1〉.
During the phase of the correction, the following elements are calculated: 
– measurement vector estimated at the moment (k + 1)T, the covariance matrices Pxz(k + 1|k) and Pzz(k + 1|k), the innovation covariance matrix S(k + 1), the Kalman gains matrix K(k + 1), the state vector and the covariance matrix of filtration errors3,7-11.
4.
SIMULATIVE STUDIES OF THE ALGORITHM
To evaluate the effectiveness of the proposed Unscented Kalman Filter, simulative studies of the algorithm were conducted. The generated input data reflecting the user’s movement along a given route were processed by the implemented algorithm. The measurements of the ranges between modules were burdened with measurement errors whose values were typical for the developed navigation system.
In the conducted simulations, two different configurations of beacons, optimal and non-optimal from the accuracy point of view, were used. In the optimal case, the beacons were deployed around the area of interest and in the second one all the beacons were in one line. The parameters of the filters had the same values in every simulation. The obtained results for different routes and configurations of the beacons, in the OXY plane, are presented in the figures below.
Figure 2.
Results obtained with the UKF on route 1: a) optimal configurations of the beacons, b) non-optimal configurations of the beacons.
Figure 3.
Results obtained with the UKF on route 2: a) optimal configurations of the beacons, b) non-optimal configurations of the beacons.
The obtained positioning results follow the real travel path with high accuracy, even in case of the non-optimal beacons deployment. With proper selection of the parameters, the proposed algorithm maintains a high accuracy of positioning even when maneuvering.
In order to demonstrate the differences between the proposed Unscented Kalman Filter and the Extended Kalman Filter, some simulations were conducted were both the algorithms were used. The parameters of these simulations were prepared to make the differences between the filters more visible, but they were the same for both filters. The results of the simulations are presented in Fig. 4 and Fig. 5.
Figure 4.
Results obtained for optimal configuration of the beacons on route 1: a) track estimated with the UKF and the EKF, b) zoomed-in fragment of the figure 4a, showing the lower right corner of the estimated route.
Figure 5.
Results obtained for non-optimal configuration of the beacons on route 1: a) track estimated with the UKF and the EKF, b) zoomed-in fragment of the figure 5a, showing the lower right corner of the estimated route.
The obtained results show that the UKF algorithm has better accuracy than the EKF during maneuvering. There are some deviations from the real path but the results from the UKF are more accurate and are corrected faster after the maneuver.
5.
CONCLUSION
The navigation system proposed by the authors allows to achieve accurate range measurements between modules which provides a high accuracy of positioning. The user can be successfully navigated in closed spaces, such as buildings.
Initially, in the developed navigation system, the Extended Kalman Filter was implemented. It allows to obtain accurate information about the user’s position; however, the conducted simulations show that using the Unscented Kalman Filter could bring some additional benefits and improve the positioning process. The tested algorithm reflects the simulated path in a better way, especially during maneuvering.
An improvement of the system could be achieved through an implementation of the new positioning algorithm. The four beacons used in the system allow to determine user’s position in three dimensions, therefore it is possible to develop and implement the UKF algorithm that would estimate also the vertical position component.
