Particle Filter and Finite Impulse Response Filter Fusion and Hector SLAM to Improve the Performance of Robot Positioning

Indoor position estimation is essential for navigation; however, it is a challenging task mainly due to the indoor environments’ (a) high noise to signal ratio and (b) low sampling rate and (c) sudden changes to the environments. This paper uses a hybrid filter algorithm for the indoor positioning system for robot navigation integrating Particle Filter (PF) algorithm and Finite Impulse Response (FIR) filter algorithm to assure the continuity of the positioning solution. Additionally, the Hector Simultaneous Localisation and Mapping (Hector SLAM) algorithm is used to map the environment and improve the accuracy of the navigation. The paper implements the hybrid algorithm that uses the integrated PF, FIR, and Hector SLAM, using an embedded laser scanner sensor. The hybrid algorithm coupled with Hector SLAM is tested in several scenarios to evaluate the performance of the system, in terms of continuity and accuracy of the position estimation, and compares it with similar systems. The scenarios where the system is tested include reducing the laser sensor readings (low sampling rate), dynamic environments (change in the location of the obstacles), and the kidnapped robot situation. The results show that the system provides a significantly better accuracy and continuity of the position estimation in all scenarios, even in comparison with similar hybrid systems, except where there is a high and constant noise, where the performance of the hybrid filter and the simple PF seems almost the same.


Introduction
The positioning technologies have faced several challenges for indoor applications.Reference [1] studied several most widely used positioning technologies, including Wireless Local Area Network (e.g., Wi-Fi) based systems, Bluetooth Low Energy (BLE), Ultra-Wide Band (UWB), Inertial Navigation Systems (INS), Radio Frequency Identification (RFID), and Tactile based systems, from fitness-for-purpose point of view, and concluded that, for almost all of the indoor Location Based Services (LBS) applications, including indoor navigation, none of these stand-alone positioning technologies could yet provide the required level of positional accuracy and continuity.
One of the biggest challenges of indoor localisation is the existence of a relatively high ratio of noise to signal.Also, Non-Line-Of-Sight (NLOS), where the received signal does not traverse the direct path between the receiver and the transmitter, introduces a big challenge for the ranging-based positioning systems.This is a common situation where the positioning technology is being used for indoor applications.To handle some of these issues, up to some degrees, many of the positioning systems use a state estimator for an estimated but accurate position solution in a cluttered and noisy environment [2], such as inside the buildings.The state estimator, which is also called the stochastic filter, is a mathematical algorithm that estimates the state variables of a system from noisy and biased measurements [3].
One of the most widely used state estimators is Kalman filter (KF), which functions particularly well for the linear systems with a Gaussian noise [4].For indoor localisation, the state-space model is typically nonlinear; therefore, the nonlinear filters such as the Extended (or Enhanced) Kalman filter (EKF) and the Particle Filter (PF) could respond better [2,3].The PF, e.g., Monte Carlo Localization (MCL), can provide with a better performance, in comparison with the EKF, in the highly nonlinear environments [5].While the EKF requires the initial position and can only solve the relative positioning solutions [2], e.g., for tracking purposes [6], the ability of the PF to function and provide the positioning solutions with no initial (a priori) data allows many applications to apply and use it.In addition, the PF is relatively easier to implement, particularly in comparison with the EKF [7].However, the PF performance is highly associated with the sample distribution and diversity, which can be an issue in systems that are based on the low rate sampling.The loss of diversity among the samples may result in failing to estimate the state or potentially the large estimation errors [8].This is referred as the sample impoverishment problem and can happen when the measurement noises are small and/or the number of particles is not enough.To handle this, several attempts have been made and some solutions are provided.They include the Regularized Particle Filter (RPF) [9], Markov Chain Monte Carlo (MCMC) move step [10], and combined/integrated Particle and Kalman filters [11].Despite these, the enhanced versions of the PF may still not be able to prevent and/or cop with the sample impoverishment completely.In addition, there may a compromised performance for the PF, and in the extreme cases, the filter can fail to function completely [8].Such extreme cases can include the situations that have got a very small measurement noise or a very low number of particles [12].There are various preventive methods against sample impoverishment and PF failures.However, they concluded that an effective and general remedy to cure a completely failed (or diverging) PF has yet to be proposed [13].
This paper applies a hybrid filter that integrates PF and Finite Impulse Response filtering (FIR), which has been proposed by [13], and couples it with a Hector SLAM algorithm to improve the performance, i.e., accuracy, reliability, and continuity of the localisation under harsh conditions.This system is tested using a laser scanner sensor embedded in a robot to measure the performance of the system in different challenging scenarios and compare the results with the systems that use (a) PF/FIR only [13], and (b) Particle Filter only.While laser scanner can be considered as a standalone positioning technology, the hybrid filter uses a PF as the core main filter, integrated and enhanced by another robust FIR filter, based on the proposed scheme by [13].The PF estimates the state of the robot in the 'normal' condition, i.e., where there is no sample impoverishment or positioning solution failure.Once an enormous estimation error is detected, the FIR filter starts functioning and helps the PF to recover.Note the hybrid system does not enhance the PF and so it can be integrated into currently developed and deployed PF-based systems.The novelty of the paper is to use this integrated PF-FIR system [13] with a Hector SLAM for simultaneously localisation and mapping purposes.Also, this paper uses Manhattan distance for PF failure detection as it seems more compatible with the grid-based environment where the experiments are conducted.So, the system uses a hybrid filter to integrate PF and FIR, which prevent the PF failure, while a laser scanner embedded on a robot is used to apply Hector SLAM and improve the continuity, usability of the system at different scenarios.This will be examined through some experiments where the system is tested with different sampling rates and noises levels imposed to the system and/or the environments.
The used hybrid filter coupled with Hector SLAM is implemented and several experiments and tests are conducted in both simulation and real-world environments in different scenarios, including environments with dynamic/changing obstacles and at different levels of noise.The proposed system also compared against (a) PF only and (b) the hybrid only systems.The results of the experiments show that the hybrid system coupled with Hector SLAM improves the continuity and reliability of the positioning solutions, particularly in harsh scenarios where the number of particles and/or level of measurement noise may be low.Also, it seems to function with better robustness in the case of extreme noise in comparison with FP/FIR, where usually PF fails to function.Also, the tests show that the used hybrid filters (both with and without Hector SLAM) continue functioning and so are able to solve the kidnapped robot situations.
This paper is organised as follows: the next section is about the mathematical background of the hybrid filter, explaining the principles of the PF and FIR separately.Then the hybrid filter is explained in detail.Then the implementations of the used hybrid filter and the experiments at different scenarios are discussed.And finally, there is a conclusion and a forward-looking discussion on future work.

System Design
In order to estimate the state of a system, one of the most widely used approaches is Bayesian filter framework, which uses the observed values and the corresponding confidence coefficients, i.e., the covariance matrix, to estimate the state of the system [14].For many linear variables, the Kalman filter can accurately estimate the state using a Gaussian distribution [2].However, for nonlinear variables, the KF may not be able to provide an accurate estimation and so an enhancement is needed.The EKF algorithm is used for the linear approximation of a nonlinear system using the Gaussian distribution [2].In contrast, the Particle Filter estimates the state of the nonlinear (non-Gaussian) systems using a set of the particles distributed in the state space [14,15].So, the Particle Filter provides a numerical approximation for the nonlinear problem [16].
While the Particle Filter can approximate the nonlinear systems numerically, PF may fail due to a wide range of reasons [3,8] and so resetting the failed PF is important to have a continuous state estimation.Resetting the failed PF also means generating new particles.To generate new particles, this paper uses Finite Impulse Response.
FIR has been applied by several positioning systems, particularly as an alternative to the filters with Infinite Impulse Response structures such as Kalman filter [13,[17][18][19][20][21].FIR can improve the overall performance of such systems [18][19][20] as it provides with a robust response despite the availability of noise and/or model parameter uncertainty [18].While the PF is basically an Infinite Impulse Response (IIR) filter, which can potentially provide a more accurate position estimation than FIR filters generally, it may fail to estimate the state if there is the case of sample impoverishment.To keep the balance hybrid filter (PF and FIR) may provide a good solution.The hybrid filter allows overcoming the issues and limitations of either the stand-alone versions of the PF and the FIR filters [13].This paper uses the hybrid filter, combining the PF and the FIR, to provide a reliable, continuous, and robust position estimation.The hybrid filter acts like a PF as long as it can estimate the state.As soon as an extreme noise or any other circumstances that could fail the functionality of the PF, the FIR filter takes over and assures the continuity of the position estimation.However, this paper couples the hybrid filter with a Hector SLAM for even more continuity and autonomy of the whole system.The hybrid filter uses the PF as its main filter in normal conditions, and as soon as any abnormality occurs it uses FIR to recover the whole system.The hybrid filter, integrating PF and FIR, was initially proposed by [13] and its general process is represented in Figure 1.As the flowchart (Figure 1) illustrates, having been initialised and calibrated, the PF starts functioning.As long as there is no failure diagnosed in the PF, the system relies solely on the PF.This means the relative position of the robot, which can be used for calculation of heading and absolute position, is estimated by the PF.This is mainly because the PF can provide the system, as a whole, with a better performance in comparison with the hybrid filter [13].However, in the case of PF failure, the FIR takes over and estimates the state and error.FIR is practically an auxiliary or a backup filter that only acts as a plan B, i.e., when the PF fails to estimate the state.The estimation of the state and error from the FIR are fed into the 'initialisation' phase and based on these a secondary initial sample set is generated.
In order to identify a failure in the PF, a diagnosis algorithm which uses the Manhattan distance is proposed and used by this paper.This is basically because the maps are available as a raster file, i.e., Pixels and grids with values, where Manhattan distance seems more compatible choice [22,23].

The Failure Diagnosis.
The PF failure can be caused by several reasons; one of the most common reasons behind it is the sample impoverishment, which is also a relatively difficult case to handle [24].This paper used an algorithm that diagnoses the failure if either (a) the majority of the predicted states fall outside the uncertainty ellipse or (b) the distance between the prediction and the actual samples is too big.These two categories of the PF failure symptoms can be associated with the concepts of accuracy and bias, respectively.
The first category uses the concept of uncertainty ellipse; this paper uses Manhattan distance for the outlier detection.While Manhattan distance is relatively simple to implement, it is powerful way to detect the outliers.It is particularly compatible with the grid/pixel-based input data, i.e., raster maps and spatiotemporal intervals for location updates.The points, which fall outside the 99% confidence interval (3 standard deviations from the average of the distance between the predicted points and the actually measured locations) can be identified as outliers.After identification of outlier points and recognising the PF failure, FIR takes over.
One of the important inputs for robot navigation is the map of the environments.In some cases, the maps can be extracted from the building plans while in other cases the map plan is being generated simultaneously while the rover is moving and sensing.In such cases, the autonomous moving object, here the robot, must be able to both estimate the position and also create the map.While the position estimation itself requires a map and for mapping, the localisation is essential too.In this regard, this paper uses the simultaneous approach; i.e., the processes of mapping and the robot position estimation are conducted simultaneously.This is called Simultaneous Localization and Mapping [25].SLAM uses the correlation between the estimated robot position and the 'as-built' or the under-construction map [25,26].The map, generated while navigating the robot, is fed into the system, recursively.The implementation of the whole system is explained in the next section.

Implementations
In this section, the implementation of the system, i.e., the hybrid Particle Filter and the Finite Impulse Response filter, using a mobile robot with a laser sensor for Hector SLAM, is described (see Figure 2).The hardware and then the implementation of the system using the Robot Operating System (ROS), which is the software used for the customization and the development of the hybrid filter algorithm, are described in this section.
As shown in Figure 2, a robot with an embedded LIDAR sensor is used to implement and test the position obtained by the hybrid filter algorithm.The operating system is run on the Raspberry Pi3 and an Arduino Uno board to feed and steer the motors.It also has got two 35rpm motors and one 40Ah drivers.An A1 RPLIDAR scanner is used, which has been the most recent version at the time of development and applied by many new systems.The RPLIDAR A1 scanner provides an angular resolution of 1440 scans per second and a diagnostic length of 6 meters with a measurement range of 360 degrees.The robot controller, Raspberry Pi3, is embedded on an ARM-A7 processor runs on the ROS system under the Ubuntu Linux where all robot software runs.The main purpose of the robot controllers is to calculate the position of the robot and also to send commands to the robot.
The implementation of the used system results in the development of a piece of software, which is in charge of controlling the robot and estimating the position, as shown in Figure 3.
In this paper uses a hybrid filter algorithm for the indoor positioning system for robot navigation using PF and FIR to assure the continuity of the positioning solution, as proposed and implemented by [13].To improve this hybrid filter, however, this paper uses Manhattan distance for PF failure detection and also couples it with Hector SLAM for more autonomy.First, the map of the environment is created, either manually or using SLAM.In this paper, the initial map is generated manually but it is updated by SLAM.This process is explained in more detailed in the next subsection.The data capture is initiated by the laser scanner.Collected data are sent to the ROS system, which includes a particle filtering module.As it mentioned, in this project ROS is the basis of the customization and development.This is mainly because it is very well developed in terms of most the packages and libraries that are essential or useful for positioning and navigation purposes, e.g., locating.This minimizes the software development and programming phase, as some basic functionalities already exist.Also, it allows sending data using a variety of standardized message formats, which could be useful for any further development and test of the used system by other sensors.
The developed piece of software represented in the ROS node with a publisher name, which is responsible for sending all measured data from the robot.Figure 4 shows the sender (node) of all ROS data.
The LIDAR scanner with a laser scan node captures data and sends it to ROS.The RPLIDAR A1 scanner reads data with the angular resolution of 360-degree omnidirectional and the frequency of 5.5 Hz.However, the software is designed to work with lower resolution and/or frequency at other scenarios.The physical position of the sensors in relation to other parts of the robot is very important; TF in ROS allows nodes to communicate with each other in a distributed computing environment.ROS packages usually use the system clock as a time source for synchronization.In this project, the system with synchronized SSH clock has a static and fast structure.

Mapping.
As explained earlier, this paper uses the simultaneous approach, i.e., the processes of mapping and the robot position estimation are conducted simultaneously.Hector SLAM algorithm is used to correlate the estimated robot position and the 'as-built' or the under-construction map [26].
To create the map, Hector SLAM modules, which have been made available by the software package, are used at different instances.Hector SLAM functions based on different sensors samples, along with a metadata specifying the number of parameters such as map frames and sensor data format; see Figures 5 and 6

Results
This subsection explains the implementations of the hybrid filter applying a laser scanner data.Different scenarios that may result in the Particle Filter failure are designed and four experiments were conducted.They include the cases examining the performance of the hybrid filter exposed to (a) low sampling of the laser scanner readings, (b) the kidnapped robot, and (c) in a dynamic environment.The results are then compared with both normal situations where  the Particle Filter solely can estimate the state with no failure.
To have a benchmark, firstly, the hybrid system run on the robot is tested under normal conditions.The robot moves in a counter clockwise direction in a rectangular trace (1m * 0.5m).For each scenario, the same planned path is taken by the robot and the error is calculated based on the same formula.

Robot Position Estimation with Low Samples.
In this scenario, the laser scanner sensor measures low samples, i.e., the number of measured points reduced to 360 samples in 360 ∘ .Note that, in the normal scenarios, the laser scanner sensor measures 1440 samples in 360 ∘ (noise: =0.5).
As shown in Figure 7, the Particle Filter provides poor performance due to the low number of sample points, while the hybrid filter offers a lower location error.The estimated path is shown in Figure 8.

The Kidnapped Robot Problem.
The kidnapped robot problem is one of the most challengeable problems in the robot positioning.To handle this, a position estimation algorithm is needed that is able to recover from a high level of error and noise.The kidnapped robot problem is studied in three phases: (1) The mobile robot starts from the beginning point and moves on the straight line.
(3) Then continuous to move on the straight line in a similar direction.
Since the sudden jump is not feasible for the robot applied here, while possible in other autonomous rovers such as drones, this scenario is tested in a simulated environment.However, the simulation allows the position estimation algorithm to be tested under any other unpredictable behaviours, including the kidnapped robot problem. Figure 9 shows the trajectory of the movements, i.e., actual path, estimated by PF, and estimated by the hybrid filter.Figure 10 illustrates the level of error from the position estimation of the two filters, i.e., PF only and the hybrid (PR/FIR), before and after the kidnapped robot occurrence, i.e., two peaks.As shown in Figure 10, the hybrid PF/FIR filter provides with better accuracy and also continues to track the robot after a shorter transient period.

Dynamic Environments.
The last experiment is to test the performance of the hybrid filter in a dynamic and changing map.Any changes to the robot map would be very difficult to handle for the positioning and tracking system as the most of indoor positioning technologies are based on relative (dead reckoning) localisation and so the change can have a significant impact on the position estimation.In this experiment, the robot moves through the map where an existing map is fed to the system, instead of creating the map on the fly (SLAM).Then a sudden change occurs in the surroundings of the robot.This experiment, i.e., the change in the environment map, is conducted for each of filters, and the position estimation error is measured.As shown in Figure 11 the hybrid filter provides with a better accuracy compared to the Particle Filter.
As discussed above, in the three scenarios of a lower sampling rate, kidnapped robot, and dynamic environment, the position estimation of the robot with an embedded laser scanner sensor has got a lower level of overall error.Table 1 shows the improvement (percentage) of the overall accuracy of the hybrid filters used with respect to the PF only estimation.
Figure 12 compares the average accuracy of the PF only, hybrid filter (without Hector SLAM) and the proposed   system which couples the hybrid (PF and FIR) with Hector SLAM.As it is shown in Figure 12, the proposed system provides an overall higher accuracy and better robustness (particularly with high noise where both PF only and hybrid filter-only systems have got a peak, see the transparent box).

Conclusion
This paper used a hybrid (PF/FIR) algorithm for robot positioning in harsh environments, where there are more noise and sudden changes.The hybrid filter algorithm is implemented in three different scenarios; each could potentially fail the PF, which is the most commonly used filter in nonlinear cases.Those scenarios include the kidnapped robot, changing/dynamic environments, high noise to signal ratio, and lower sampling rate.The results of the implementations using a laser scanner sensor show the hybrid filter provides a more accurate, continuous, and reliable position estimation.The flexibility of the hybrid filter algorithm to be applied by any PF and/or any FIR filter allows taking this research to another level and perhaps overcoming the NLOS situation.

Figure 2 :
Figure 2: Robot used for implementation and test.Equipped with a RPLIDAR A1 scanner.

Figure 3 :Figure 4 :
Figure 3: View of the software structure of robot controller. .

Figure 6 :
Figure 6: Representation of robot position using hybrid Particle Filter and Finite Impulse Response in Rviz.

Figure 7 :Figure 8 :Figure 9 :Figure 10 :
Figure 7: The Particle Filter only versus the hybrid against Particle Filter and Finite Impulse Response (PF/FIR) error with 360 laser reading points.

Figure 11 :
Figure 11: Position errors of PF and hybrid (PF/FIR) filters in a dynamic environment.

Figure 12 :
Figure 12: Average error for PF only (blue), hybrid filter only (grey), and hybrid filter coupled with Hector SLAM (orange).

Table 1 :
Decrease or increase of the hybrid PR/FIR filter in relation to PF only scenarios.