1.

INTRODUCTION

The continuous increase of traffic requires from today’s navigation systems increasing accuracy and certainty of action. Integrated navigation systems fusing data from various navigation devices¹ are created to increase the accuracy of the determined data and to improve the reliability of the system^2,3. The development of such systems was impeded by the invention of a Kalman filtration algorithm, which is a milestone in the history of navigation⁴. This filter is an optimal estimator of the state and it allows to determine quantities directly inaccessible for measurement, based on available measurements and using an assumed mathematical model of the system^5,6.

The classic Kalman filter is the optimal data processing algorithm only for linear systems. For systems whose mathematical description contains non-linear functions, non-linear filters are applied^6,7. One possible solution is a local approximation of the non-linear functions representing the dynamics model and/or observation model of the system. The linearization is realized by expanding these functions into Taylor series and concatenating the result by remaining only its linear term^7-10. This is how the extended version of the Kalman filtering algorithm, i.e. EKF, deals with non-linearities. The EKF is distinguished from the classic linear version of the Kalman filter by a different form of the transition matrix and the observation matrix, which are calculated as Jacobians of non-linear transformations⁶.

In the section 2 of this paper, a model of a simple pedestrian or robot positioning system using range measurements to several base stations, as well as an extended version of the Kalman filter, developed and examined for the purpose of testing such a system, are presented. The main aim of the works described in this paper was to elaborate a computer application facilitating testing of the properties of the EKF filter. The application is described in the section 3. An assumed testing methods and chosen results of performed simulations are included in the section 4 and commented in the section 5.

The computer application presented in this paper was developed to examine the extended Kalman filter in such a way that it allows considering various simulation scenarios and different model’s and filter’s parameters.

2.

DESCRIPTION OF POSITIONING SYSTEM

Various navigation methods and devices are used when elaborating integrated navigation systems. Their selection is often strictly connected with the assumed application of the system. The system developed and presented in this paper is supposed to be used for determination of the position of a land object, which can be a pedestrian or possibly a mobile robot. In such an application, inertial sensors¹¹ are frequently applied as signals from Global Navigation Satellite Systems (GNSS)¹ are usually unavailable in covered spaces. The mentioned systems usually operate only locally, therefore other suitable devices are range measuring devices which use laser or radio signals^12,13. The robot systems are additionally often supported by object detection and recognition systems¹⁴.

The navigation system presented in this paper was elaborated mainly for research and educational purposes to enable testing properties of Kalman filters and to support the process of choosing their parameters. Therefore, it omits modelling many features of real systems and is based on several simplifying assumptions. It is assumed that the system contains several stationary units (base stations) and a mobile unit installed on a moving object and realizing range measurements to the stationary base stations in subsequent time moments. The idea of such a system is presented in Fig. 1.

Figure 1.

Local pedestrian or robot positioning system based on range measurements.

When designing the system, the following assumptions were made:

The measurements with errors are calculated based on the following relationship¹³:

where: d_i – the distance between the i-th base station and the object; X_i, Y_i – the i-th base station coordinates, x, y – position of the object, v_i – the i-th distance measuring error.

Due to a non-linear relationship (1) between the distances d_i and the coordinates of object’s position, the system’s observation model is non-linear. The distances from individual stations to the object form a measurement vector z, which for four stations takes the following form:

The non-linear observation model for this system configuration can be written as follows:

The range measurements included in the vector z are processed by a Kalman filter. Due to the non-linearity of the observation model, an extended version of the Kalman filtration algorithm was used to estimate the position of the object, and thus the observation matrix had to be determined^1,5. This matrix, when four base stations and a PVA motion model are used, takes the following form:

The observation matrix H is composed of partial derivatives of the function h after the elements of the state vector x and is calculated during the filter’s operation. It has different sizes depending on the number of stations used for measurements and depending on the adopted motion model, which can be the PV or the PVA model.

3.

COMPUTER APPLICATION

When developing a computer application which would enable an insight into the Kalman filter’s operation and enable testing of its properties, it was assumed that the data generation and processing will be performed in several separate stages. The first step of the simulations consists in a generation of the object’s motion trajectory. Three variants of the object’s movement were assumed, enabling observation of the algorithm’s operation in various conditions, i.e. movement along the edges of a square, around a circle and along a more complex, predefined curve.

After simulating the object’s motion, range measurements with errors were generated for subsequent object locations. As the object’ s route is known at the simulation stage, it is possible to determine the distances between the base stations and the object, and then generate their noisy measurements. These data are processed in the described application by means of the implemented extended Kalman filter, which performs estimation of the object’s position. Key operations implemented in the computer program are illustrated in Fig. 2.

Figure 2.

Basic operations performed in the designed computer application.

The results of filtration are presented in the graphs in the graphical window of the application. The application enables displaying the assumed trajectory of motion, its estimates from the EKF, as well as the components of estimated position and velocity errors. Additionally, it calculates and presents the root-mean-squared (RMS) and maximal estimation errors. The user, thanks to the GUI (Fig. 3), can easily change selected parameters used both in the system and in the filter itself.

Figure 3.

Main window of the designed computer application.

The application GUI contains controls described as “Object movement” and “Base stations” that allow the user to choose the shape of generated trajectory and configure the base stations distribution and number. The positions of base stations are entered by the user in the provided windows. In addition, the GUI allows to enter the value of a standard deviation of errors of the station-object distance measurements σ. The controls gathered in the “Kalman filter settings” panel make it possible to change the motion model (PV or PVA), initialize the filter with a value of the initial position and enter a value of the power spectral density of the process noise S_a, which is a measure of the intensity of disturbances acting on the object’s movement. The application described in the paper enables imaging of the real trajectory of the object along with the estimated trajectory as well as imaging of the values of the position error and velocity of the object. The application window used for a visualization of these errors is shown in Fig. 4.

Figure 4.

Example graphs showing object position errors and velocity errors.

4.

SIMULATION RESULTS

Simulation tests were performed for observing the operation and verifying the accuracy of position estimation of a moving object depending on the shape of its trajectory, the adopted motion model, the number and location of the base stations, as well as depending on the selected filter’s parameters.

The user can easily test the filter in various simulation variants thanks to the provided GUI. Below, in Fig. 5, selected simulations results are presented, showing the object’s trajectories available in the program and their estimates obtained with the EKF filter. The EKF was configured to work with the assumed PVA motion model, the initial position of the object X₀ = 100 m, Y₀ = 100 m and with the parameters , σ = 1 m.

Figure 5.

The effect of the program on motion trajectories: a) on a square, b) on a circle, c) on a set function.

More detailed results of simulations, presenting effects of filtration for various motion models, for various initialization variants and for various number and locations of the base stations are presented in subsequent subsections 4.1-4.3.

4.1

Influence of motion model on filter’s operation

The test presented below was aimed at determining how the selection of the object’s motion model impacts on the results of position estimation. The simulation results for PV and PVA models assumed during system’s configuration are shown in Fig. 6.

Figure 6.

The result of object’s position estimation for: a) the PV model, b) the PVA model.

The application also allows displaying graphs showing errors of position and velocity of the object. These graphs for the PV and PVA motion models are shown in Fig. 7.

Figure 7.

Object’s position and velocity errors: PV model - left side of the drawing, PVA model - right side of the drawing.

4.2

Influence of initialization on filter’s operation

The next stage of the EKF testing concerned checking the impact of its initialization on the positioning accuracy. The obtained results are shown in Fig. 8.

Figure 8.

Initializing the filter at a point with coordinates: a) X₀ = 100 m, Y₀ = 100 m, b) X₀ = 100 m, Y₀ = 300 m, c) X₀ = 300 m, Y₀ = 200 m.

As can be seen, an initialization with the values equal or close to the actual position of the object results in small filtration errors at the beginning of the filter’s operation. Incorrect initialization, however, resulting e.g. from lack of knowledge about the initial position, causes large errors at the beginning of the filter’s operation, which, however, are relatively quickly reduced to the level typical for the rest of the trajectory.

4.3

Influence of the number and position of active base stations on filter’s operation

The accuracy of object position estimation is also dependent on the location and the number of active base stations. Fig. 9 presents the filter’s operation for two examples of the base stations’ placement, where different geometrical conditions affecting the accuracy of position estimation occur in different parts of the object’s trajectory. In the parts of the trajectory located inside the area surrounded by the base stations, higher filtration accuracy is obtained than outside of such areas.

Figure 9.

Base station locations: a) option 1, b) option 2.

The following figures show the effects of using a different number of base stations. The situation presented in Fig. 10 corresponds to the case with four stations, Fig. 11 shows the results for a configuration with three base stations, and Fig. 12 for the configuration with two stations only. This confirms that the number of base stations has a significant impact on the accuracy of the navigation. Particularly noticeable is a significant deterioration of accuracy in the last presented case.

Figure 10.

Influence of the number of active base stations on positioning accuracy - 4 stations.

Figure 11.

Influence of the number of active base stations on positioning accuracy - 3 stations.

Figure 12.

Influence of the number of active base stations on positioning accuracy - 2 stations.

5.

CONCLUSIONS

The paper described a computer application used to study the Kalman filtration algorithm’s properties. The extended version of algorithm was implemented in the MATLAB® environment and analyzed based on simulation tests carried out for various scenarios, configurations and with different filter’s parameters.

Analyzing the obtained results, one can notice the discrepancies that arise when the object makes a turn, especially when it follows a square-shaped trajectory. On straight-line sections of the trajectory, the values of estimates are by much more accurate. The estimation errors occurring during the maneuvers are different when using the PV and PVA model. For the PV model, the maximum position errors are almost three times higher than the maximum errors recorded for the PVA model. Thus, in case of trajectories containing frequent maneuvers, better results can be obtained by using movement using the PVA model. The filter’s initialization also affects the correct operation of the algorithm. An appropriate selection of the initial values of object’s position plays a significant role in the initial phase of the filter’s operation.

The ability of the application to easily change the number and the location of base stations as allows for observation of the estimation accuracy in various system’s configurations, which can be very useful when choosing positions of the base stations. Based on the presented results, one can see the increase of positioning errors on the trajectory fragments distant from the base stations which is an expected behavior for this kind of system.

It should be stressed that the paper contains only selected samples of the results and the range of possible experiments is much broader. The application turned out to be useful for the EKF testing in various system’s configurations and thanks to its convenient graphical interface, intuitive operation and a wide range of possible tests, it can find its applications in both research and education.