# A PHD-SLAM Method for Mixed Birth Map Information Based on Amplitude Information

Authors: Fei Zhang; Zijing Zhang; Luxi Yang; Xinyu Zhang
Journal: Mathematical Problems in Engineering
 (2021)

---

## Abstract

The Simultaneous Localization and Mapping (SLAM) method of mobile robots has the problem of low accuracy in complex environments with dense clutter and various map features, such as complex indoor environments and underwater environments. This problem is mainly embodied in estimating the location and number of feature points on the map and the position of the robot itself. In order to solve this problem, a new method based on the probability hypothesis density (PHD) SLAM is proposed in this paper, a PHD-SLAM Method for Mixed Birth Map Information Based on Amplitude Information (AI-MBMI-PHD-SLAM). Firstly, this paper proposes a PHD-SLAM method based on amplitude information (AI-PHD-SLAM). The method uses the amplitude information of map features to obtain more precise map features. Then, the clutter likelihood function is used to improve the estimation accuracy of the feature map in the SLAM process. Meanwhile, this paper studies the performance of the PHD-SLAM method with the amplitude information under the condition of the known signal-to-noise ratio or the unknown signal-to-noise ratio. Secondly, aiming at the problem that PHD-SLAM lacks a priori information in the prediction stage, an AI-PHD-SLAM-based mixed birth map information method is added. In this method, map information that has been detected before the previous moment is added to the observation information in the map prediction phase as a new map information set in the prediction phase. This can increase the prior information and improve the problem of insufficient prior information in the prediction stage. The results of the experiments show that the proposed method and the improved method outperform the RB-PHD-SLAM method in estimating the number and location accuracy of map features and have higher computational efficiency.


---

## Body

## 1. Introduction

With the development of integration technology, mobile robots have been widely used in various fields, such as indoor fire rescue and underwater exploration. Therefore, navigation technology is particularly important for mobile robots. In the environment described above, most navigation algorithms that require external navigation data will suffer performance degradation. But Simultaneous Localization and Mapping (SLAM) is better suited to the situation above [1–6]. The SLAM method can be described as that the mobile robot starts from a certain location in an unknown environment, acquires the position of the map feature near the environment relative to itself by the sensor carried by the robot itself, and incrementally constructs the map to determine the map information; and correct the robot's own posture based on the determined map information [6–9].In recent years, the probability hypothesis density SLAM (PHD-SLAM) method based on random finite sets (RFS) theory has gained wide attention [10–12]. The PHD-SLAM method is a new SLAM method that avoids complex data association and brings the missing probability into the map feature filtering when estimating the map features. It is suitable for the case where the data association is ambiguous and the target feature is substantial [12–15]. However, because of the complexity of the complex environment and the sensors with natural characteristics, the observed information of the robot during its movement may contain a large number of false alarms and clutter information, resulting in the real map feature point information being drowned in the clutter [16, 17]. Due to the above problems, the accuracy of SLAM is reduced. As a result, the robot must use more auxiliary information to distinguish clutter and true features from observation information and obtain more accurate map features and clutter likelihood functions, so as to improve the accuracy of SLAM method. The amplitude information for filter was first proposed by Daniel Clark and others. They provided the amplitude likelihood function under the known and unknown target signal-to-noise ratio and put forward an amplitude-aided PHD filtering (amplitude information PHD, AI-PHD) method to improve the accuracy of multitarget state estimation [18–22]. However, this method has not been used for SLAM. What is more, considering the characteristics of the PHD method, it is necessary to provide certain a priori information for recursive filtering operations; however, the SLAM process moves from an unknown location in an unknown environment and cannot provide a priori information. Therefore, a new feature strategy is proposed in “SLAM Gets a PHD: New concepts in map estimation” [23], taking the observation set of the mobile robot at the previous time as the prior information set of the current time. But the new target set only uses the observation set at the last moment, which may result in low accuracy of the estimation of the map feature position with insufficient prior information [24].In summary, this paper proposes a new method, A PHD-SLAM Method for Mixed Birth Map Information Based on Amplitude Information (AI-MBMI-PHD-SLAM). Firstly, based on the above-mentioned AI-PHD method, this paper proposes an amplitude information-assisted PHD-SLAM method (AI-PHD-SLAM), which can improve the accuracy of PHD-SLAM estimation of map features and robot pose in the same clutter environment and alleviate the computational amount. Then, in order to solve the problem of inaccurate map estimation caused by insufficient prior information, the method of mixing new map information is added on AI-PHD-SLAM method in this paper. This method takes the map features near the robot position in the detected map information into the last moment observation set as the new map feature set in the PHD-SLAM prediction process, which overcomes low accuracy of the map feature position and number estimation for lack of prior information.The structure of this paper is as follows. In Section2, AI-PHD-SLAM and AI-MBMI-PHD-SLAM methods and their modeling are introduced. In Section 3, the processes of AI-PHD-SLAM and ai-mbmi-PHD-SLAM methods implementation are presented. In Section 4, the results of the stimulation experiments and the experiments in real environment are discussed. Finally, a conclusion is given in Section 5.


## 2. The Description of AI-PHD-SLAM and AI-MBMI-PHD-SLAM

Firstly, the signal amplitude modeling for known SNR and unknown SNR ratio is presented in Section2.1. Then, map information modeling based on RFS is introduced in Section 2.2, including map feature modeling and map observation modeling. In Section 2.3, the amplitude information is applied to PHD-SLAM, and the prediction and updating formula of single particle in the process of AI-PHD-SLAM is deduced. Finally, the AI-MBMI-PHD-SLAM method and modeling are introduced in detail in Section 2.4.


### 2.1. Modeling of Signal Amplitude

#### 2.1.1. Known SNR

The observed information set is denoted byZk=zk1,zk2,…,zkm at time k, and each observation in the set contains the map feature distance and the amplitude intensity of the map feature, that is, Zk1=pT,aT, where the amplitude intensity is obtained by the sensor. Since the observation set includes false alarms and clutter besides the true map features and they are difficult to be distinguished from the real measurement of map features, the signal processor can be used to obtain the signal intensity, which means that the measured amplitude information can be used to distinguish the clutter from the real measurement. Generally, by comparing the amplitude information of the measurement with threshold parameter, we can determine the validity of the measurement, which can improve the accuracy of the SLAM method to estimate the robot’s coordinates and map information under low SNR and high false-alarm rate.We denoted the probability density function (PDF) of the envelope detector outputa when the signal is due to noise only by p0a and the corresponding PDF when the signal originated from map features measurements by p1a. If the SNR is d, the density function of noise only and map features measurements can be written, respectively, as [16](1)p0a=a⋅exp−a22,a≥0,p1da=a1+dexp−a221+d,a≥0.After choosing the appropriate detection threshold, we define the probability of false alarmpFA and the probability of detection pD as(2)pFAτ=∫τ∞p0ada=exp−τ22,pDτd=∫τ∞p1dada=exp−τ221+d.Therefore,(3)p0τa=a⋅expτ2−a22,a≥τ,p1τ,da=a1+dexpτ2−a221+d,a≥τ.According to the above equation, detection thresholdτ and signal-to-noise ratio SNR d are the key to determine the detection probability of map feature. To increase the detection probability of map feature, it is necessary to reduce the detection threshold, which will cause the increase of false-alarm probability. The false alarm and clutter amplitude likelihood function caτa and the map feature likelihood function gaτa|d are expressed as(4)caτa=p0τa,gaτa|d=p1τ,da.


#### 2.1.2. Unknown SNR

In SLAM, we need to get a robot navigation starting in an unknown location and without previous knowledge of the environment; then it can build a map via its onboard sensors and use the same map to compute the robot’s location. Therefore, the robot cannot directly obtain the true SNR of the map feature in the sonic field of view. In order to solve this problem, the straight way is to obtain the true SNR of the map feature by acquiring a large number of measurements. But this was inconsistent with the actual application of SLAM. In this paper, the range of signal-to-noise ratio is estimated in advance by using the method in [17], and SNR probability of the target is integrated in the range. Besides, this paper adopts map features detection probability and the signal amplitude likelihood function, which are independent of the true SNR distribution.Firstly, the signal-to-noise ratiopd of the map features in the robot’s sight is predicted to be within d1,d2. Therefore, the signal amplitude likelihood function and detection probability of the map feature can be, respectively, expressed as [17](5)ga=∫d1d2pδa1+δexp−a221+δdδ,(6)pDτ=∫d1d2pδexp−τ221+δdδ.Then, letpd∝1/1+d; then the likelihood function analytic solution of map features gaa can be described as(7)gaa=2exp−a2/21+d2−exp−a2/21+d1aln1+d2−ln1+d1.


### 2.2. The RFS Model of Map Information

#### 2.2.1. The RFS Model of Map Feature

Compared with FastSLAM, the observation information and the map feature in RFS-SLAM are not expressed by the form of vector sequence but represented by the form of random finite set. The number of map features varies arbitrarily from zero to any size. When the robot continues to move, the map features are monotonically increasing. All map features are independent of each other [20].(8)Mk=Mk−1∪FoVXk∩M¯k−1.Note thatFoVXk represents the location of the features that have been covered by sensor’s field of view in case of robot pose vector Xk at time k. Mk−1 represents the set on the space of map features, which intersects with the union of individual FoVXk, over the robot trajectory up to and including time k−1 . M¯k−1 represents a set of map features that are not within the Mk−1 map. The above describes the monotonous increase of the map feature when the robot constantly explores the unknown domain.


#### 2.2.2. The RFS Model of Map Observation

In the real environment, the presence of real map features is also disordered due to sensors leaks and spurious measurements produced by sensors themselves. There is no intrinsic correlation between the features of the different maps, and the number of map feature observations is also uncertain. Therefore, the form of random finite set can be used to represent map features more accurately. The feature observation set obtained by the robot with the positionXk at time k is represented as Zk . Its RFS model is(9)Zk=∪m∈MkDkm,Xk∪CkXk,where Dkm,Xk denotes the measurement RFS generated by the real map feature at m. CkXk represents the measurement RFS generated by the clutter. The number of measurements in Zk=zk1,zk2,…,zkm varies arbitrarily, and each measurement is disordered. Dkm,Xk is modeled as a Bernoulli-type RFS. The probability of Dkm,Xk=φ is 1−PDm|Xk [18], which indicates the missing probability of the map feature due to the sensor uncertainty. The probability density of Dkm,Xk=z is PDm|Xkgkz|m,Xk, which indicates the probability of detecting the map feature, where PDm|Xk is the probability of detecting the map feature at m when the pose of the robot is Xk and gkz|m,Xk represents the sensor model that the robot carries.


### 2.3. PHD-SLAM with Amplitude Information

The Rao-Blackwellised particle filter is used in the posterior estimation of the robot in the process of PHD-SLAM. The map is estimated according to the possible path of robot’s motion described by each particle. Instead of using the vector sequence in FastSLAM, PHD filtering is used to describe the state of the map features and update the map features according to the observations at each moment. When the amplitude information is introduced, the PHD-SLAM filter not only considers the observation information of the map coordinate but also uses the amplitude information of the observation. The prediction and update formula of AI-PHD-SLAM process are deduced from the signal amplitude likelihood function of the previous two sections:(1)
Single-particle prediction phase:(10)V˜k|k−1m|Xk=∫PS˜,kςfk|k−1m|ςv˜k−1ςδdvk−1dςdv+bm|Xk=pdVk|k−1m|Xk−1,wherefk|k−1m|ς represents the map feature state transfer function; bm|Xk is the new map feature PHD; it provides a priori information of the map feature information filter to determine where new map features may appear in the robot's field of view. Ps˜,kς represents the probability that the map feature still exists in the robot’s view.(2)
Single-particle update phase:(11)Vk|km|Xk=Vk|k−1m|Xk1−pDm|Xk+Vk|k−1m|Xk∑z∈ZkΛm|Xkckz+∫Λξ|Xkvk|k−1ξ|Xkdξ.After the amplitude information is introduced, the updating formula of AI-PHD-SLAM will vary. Compared with equation (11), the updating formula of AI-PHD-SLAM becomes(12)V˜km|Xk=1−p˜Dτm|XkV˜k|k−1m|Xk+∑z∈Zkp˜Dτm|Xkg˜kz|m,XkV˜k|k−1m|Xkλkc˜kz+∫p˜Dτξg˜kz|ξ,XkV˜k|k−1ξ|Xkdξ,whereg˜k denotes the likelihood function of the map feature observation, c˜k denotes the probability distribution of the clutter in the observation information set, and λk denotes the number of all the maps’ observation information exceeding the threshold at time k. When a threshold is determined, the probability that sensors can detect is merely dependent on the signal-to-noise ratio when the map feature comes into the robot's view as the robot moves.(13)1−p˜Dτm|XkV˜k|k−1m|Xk=1−pDτdpdVk|k−1m|Xkp˜Dτm|Xkg˜kz|m,XkV˜k|k−1m|Xk=pDτdpdgaτa|dgkz|m,XkVk|k−1m|Xkc˜kz=ckzcaτa.Formulas (10) and (12) form the recursive formula of AI-PHD-SLAM. When the SNR of the map feature information is unknown, substitute equations (6) and (7) as the detection probability pDτ and map feature likelihood function gaτa|d into the update equation (12).


### 2.4. Model of Birth Feature-Based Map

The prediction phase plays an important role in estimating the number of map feature targets and map feature accuracy. The method of mixing new target information used in this paper increases the PHD of new map features during the prediction phase, which is expressed asbm|Xk in equation (10). When the robot departs from the initial time, due to the lack of prior information, the observation set at time k-1 is used as bm|Xk of the new map feature information at time k. After the robot updates the map information of detected area through PHD-SLAM, the map features near the robot position in the detected map information are taken as prior information, which is added to the previous time observation set, and they are taken as the current time bm|Xk and used in the current PHD prediction stage.(14)bm|Xk=Zk−1+Mf,where(15)Mf=∪m∈MγM|Xk−1<T=R+ν.In the above two formulas,Mf represents the PHD of the RFS from the map feature near the robot pose in the detected map, ν represents the speed of the robot, R represents the scanning radius of the sensor of the robot, and M represents map feature information that has been obtained by the PHD-SLAM method. A threshold T is determined by the robot speed ν and the sensor scanning radius R. When the distance between the map feature position in the detected map feature set and the robot position is less than this threshold, the map feature is taken as a new map feature that may appear in the robot's sight at the current moment. γm|Xk−1 represents the PHD that formed by the map features that are smaller than the threshold among the detected map features. The implemented pseudocode of the above method is shown in Algorithm 1.Algorithm 1:Mixing newborn map information.
ifM=φbm|Xk=Zk−1else ifM=m1,m2,…,mnfori = 1 to nT=R+νifmi−Xk−1<TMf=Mf+miend forbm|Xk=Zk−1+Mfendbm|Xk of the mixed birth map information strategy includes not only the observation set at time k−1 but also the map feature set near the robot pose in the map information of detected area. When the map features that have been detected in the robot's vision field reappear, they will be added to bm|Xk as the a priori information. After PHD-SLAM map filtering, the weight of reappearing map features will increase; meanwhile the accuracy of estimating the position and number of map feature points is improved.


## 2.1. Modeling of Signal Amplitude

### 2.1.1. Known SNR

The observed information set is denoted byZk=zk1,zk2,…,zkm at time k, and each observation in the set contains the map feature distance and the amplitude intensity of the map feature, that is, Zk1=pT,aT, where the amplitude intensity is obtained by the sensor. Since the observation set includes false alarms and clutter besides the true map features and they are difficult to be distinguished from the real measurement of map features, the signal processor can be used to obtain the signal intensity, which means that the measured amplitude information can be used to distinguish the clutter from the real measurement. Generally, by comparing the amplitude information of the measurement with threshold parameter, we can determine the validity of the measurement, which can improve the accuracy of the SLAM method to estimate the robot’s coordinates and map information under low SNR and high false-alarm rate.We denoted the probability density function (PDF) of the envelope detector outputa when the signal is due to noise only by p0a and the corresponding PDF when the signal originated from map features measurements by p1a. If the SNR is d, the density function of noise only and map features measurements can be written, respectively, as [16](1)p0a=a⋅exp−a22,a≥0,p1da=a1+dexp−a221+d,a≥0.After choosing the appropriate detection threshold, we define the probability of false alarmpFA and the probability of detection pD as(2)pFAτ=∫τ∞p0ada=exp−τ22,pDτd=∫τ∞p1dada=exp−τ221+d.Therefore,(3)p0τa=a⋅expτ2−a22,a≥τ,p1τ,da=a1+dexpτ2−a221+d,a≥τ.According to the above equation, detection thresholdτ and signal-to-noise ratio SNR d are the key to determine the detection probability of map feature. To increase the detection probability of map feature, it is necessary to reduce the detection threshold, which will cause the increase of false-alarm probability. The false alarm and clutter amplitude likelihood function caτa and the map feature likelihood function gaτa|d are expressed as(4)caτa=p0τa,gaτa|d=p1τ,da.


### 2.1.2. Unknown SNR

In SLAM, we need to get a robot navigation starting in an unknown location and without previous knowledge of the environment; then it can build a map via its onboard sensors and use the same map to compute the robot’s location. Therefore, the robot cannot directly obtain the true SNR of the map feature in the sonic field of view. In order to solve this problem, the straight way is to obtain the true SNR of the map feature by acquiring a large number of measurements. But this was inconsistent with the actual application of SLAM. In this paper, the range of signal-to-noise ratio is estimated in advance by using the method in [17], and SNR probability of the target is integrated in the range. Besides, this paper adopts map features detection probability and the signal amplitude likelihood function, which are independent of the true SNR distribution.Firstly, the signal-to-noise ratiopd of the map features in the robot’s sight is predicted to be within d1,d2. Therefore, the signal amplitude likelihood function and detection probability of the map feature can be, respectively, expressed as [17](5)ga=∫d1d2pδa1+δexp−a221+δdδ,(6)pDτ=∫d1d2pδexp−τ221+δdδ.Then, letpd∝1/1+d; then the likelihood function analytic solution of map features gaa can be described as(7)gaa=2exp−a2/21+d2−exp−a2/21+d1aln1+d2−ln1+d1.


## 2.1.1. Known SNR

The observed information set is denoted byZk=zk1,zk2,…,zkm at time k, and each observation in the set contains the map feature distance and the amplitude intensity of the map feature, that is, Zk1=pT,aT, where the amplitude intensity is obtained by the sensor. Since the observation set includes false alarms and clutter besides the true map features and they are difficult to be distinguished from the real measurement of map features, the signal processor can be used to obtain the signal intensity, which means that the measured amplitude information can be used to distinguish the clutter from the real measurement. Generally, by comparing the amplitude information of the measurement with threshold parameter, we can determine the validity of the measurement, which can improve the accuracy of the SLAM method to estimate the robot’s coordinates and map information under low SNR and high false-alarm rate.We denoted the probability density function (PDF) of the envelope detector outputa when the signal is due to noise only by p0a and the corresponding PDF when the signal originated from map features measurements by p1a. If the SNR is d, the density function of noise only and map features measurements can be written, respectively, as [16](1)p0a=a⋅exp−a22,a≥0,p1da=a1+dexp−a221+d,a≥0.After choosing the appropriate detection threshold, we define the probability of false alarmpFA and the probability of detection pD as(2)pFAτ=∫τ∞p0ada=exp−τ22,pDτd=∫τ∞p1dada=exp−τ221+d.Therefore,(3)p0τa=a⋅expτ2−a22,a≥τ,p1τ,da=a1+dexpτ2−a221+d,a≥τ.According to the above equation, detection thresholdτ and signal-to-noise ratio SNR d are the key to determine the detection probability of map feature. To increase the detection probability of map feature, it is necessary to reduce the detection threshold, which will cause the increase of false-alarm probability. The false alarm and clutter amplitude likelihood function caτa and the map feature likelihood function gaτa|d are expressed as(4)caτa=p0τa,gaτa|d=p1τ,da.


## 2.1.2. Unknown SNR

In SLAM, we need to get a robot navigation starting in an unknown location and without previous knowledge of the environment; then it can build a map via its onboard sensors and use the same map to compute the robot’s location. Therefore, the robot cannot directly obtain the true SNR of the map feature in the sonic field of view. In order to solve this problem, the straight way is to obtain the true SNR of the map feature by acquiring a large number of measurements. But this was inconsistent with the actual application of SLAM. In this paper, the range of signal-to-noise ratio is estimated in advance by using the method in [17], and SNR probability of the target is integrated in the range. Besides, this paper adopts map features detection probability and the signal amplitude likelihood function, which are independent of the true SNR distribution.Firstly, the signal-to-noise ratiopd of the map features in the robot’s sight is predicted to be within d1,d2. Therefore, the signal amplitude likelihood function and detection probability of the map feature can be, respectively, expressed as [17](5)ga=∫d1d2pδa1+δexp−a221+δdδ,(6)pDτ=∫d1d2pδexp−τ221+δdδ.Then, letpd∝1/1+d; then the likelihood function analytic solution of map features gaa can be described as(7)gaa=2exp−a2/21+d2−exp−a2/21+d1aln1+d2−ln1+d1.


## 2.2. The RFS Model of Map Information

### 2.2.1. The RFS Model of Map Feature

Compared with FastSLAM, the observation information and the map feature in RFS-SLAM are not expressed by the form of vector sequence but represented by the form of random finite set. The number of map features varies arbitrarily from zero to any size. When the robot continues to move, the map features are monotonically increasing. All map features are independent of each other [20].(8)Mk=Mk−1∪FoVXk∩M¯k−1.Note thatFoVXk represents the location of the features that have been covered by sensor’s field of view in case of robot pose vector Xk at time k. Mk−1 represents the set on the space of map features, which intersects with the union of individual FoVXk, over the robot trajectory up to and including time k−1 . M¯k−1 represents a set of map features that are not within the Mk−1 map. The above describes the monotonous increase of the map feature when the robot constantly explores the unknown domain.


### 2.2.2. The RFS Model of Map Observation

In the real environment, the presence of real map features is also disordered due to sensors leaks and spurious measurements produced by sensors themselves. There is no intrinsic correlation between the features of the different maps, and the number of map feature observations is also uncertain. Therefore, the form of random finite set can be used to represent map features more accurately. The feature observation set obtained by the robot with the positionXk at time k is represented as Zk . Its RFS model is(9)Zk=∪m∈MkDkm,Xk∪CkXk,where Dkm,Xk denotes the measurement RFS generated by the real map feature at m. CkXk represents the measurement RFS generated by the clutter. The number of measurements in Zk=zk1,zk2,…,zkm varies arbitrarily, and each measurement is disordered. Dkm,Xk is modeled as a Bernoulli-type RFS. The probability of Dkm,Xk=φ is 1−PDm|Xk [18], which indicates the missing probability of the map feature due to the sensor uncertainty. The probability density of Dkm,Xk=z is PDm|Xkgkz|m,Xk, which indicates the probability of detecting the map feature, where PDm|Xk is the probability of detecting the map feature at m when the pose of the robot is Xk and gkz|m,Xk represents the sensor model that the robot carries.


## 2.2.1. The RFS Model of Map Feature

Compared with FastSLAM, the observation information and the map feature in RFS-SLAM are not expressed by the form of vector sequence but represented by the form of random finite set. The number of map features varies arbitrarily from zero to any size. When the robot continues to move, the map features are monotonically increasing. All map features are independent of each other [20].(8)Mk=Mk−1∪FoVXk∩M¯k−1.Note thatFoVXk represents the location of the features that have been covered by sensor’s field of view in case of robot pose vector Xk at time k. Mk−1 represents the set on the space of map features, which intersects with the union of individual FoVXk, over the robot trajectory up to and including time k−1 . M¯k−1 represents a set of map features that are not within the Mk−1 map. The above describes the monotonous increase of the map feature when the robot constantly explores the unknown domain.


## 2.2.2. The RFS Model of Map Observation

In the real environment, the presence of real map features is also disordered due to sensors leaks and spurious measurements produced by sensors themselves. There is no intrinsic correlation between the features of the different maps, and the number of map feature observations is also uncertain. Therefore, the form of random finite set can be used to represent map features more accurately. The feature observation set obtained by the robot with the positionXk at time k is represented as Zk . Its RFS model is(9)Zk=∪m∈MkDkm,Xk∪CkXk,where Dkm,Xk denotes the measurement RFS generated by the real map feature at m. CkXk represents the measurement RFS generated by the clutter. The number of measurements in Zk=zk1,zk2,…,zkm varies arbitrarily, and each measurement is disordered. Dkm,Xk is modeled as a Bernoulli-type RFS. The probability of Dkm,Xk=φ is 1−PDm|Xk [18], which indicates the missing probability of the map feature due to the sensor uncertainty. The probability density of Dkm,Xk=z is PDm|Xkgkz|m,Xk, which indicates the probability of detecting the map feature, where PDm|Xk is the probability of detecting the map feature at m when the pose of the robot is Xk and gkz|m,Xk represents the sensor model that the robot carries.


## 2.3. PHD-SLAM with Amplitude Information

The Rao-Blackwellised particle filter is used in the posterior estimation of the robot in the process of PHD-SLAM. The map is estimated according to the possible path of robot’s motion described by each particle. Instead of using the vector sequence in FastSLAM, PHD filtering is used to describe the state of the map features and update the map features according to the observations at each moment. When the amplitude information is introduced, the PHD-SLAM filter not only considers the observation information of the map coordinate but also uses the amplitude information of the observation. The prediction and update formula of AI-PHD-SLAM process are deduced from the signal amplitude likelihood function of the previous two sections:(1)
Single-particle prediction phase:(10)V˜k|k−1m|Xk=∫PS˜,kςfk|k−1m|ςv˜k−1ςδdvk−1dςdv+bm|Xk=pdVk|k−1m|Xk−1,wherefk|k−1m|ς represents the map feature state transfer function; bm|Xk is the new map feature PHD; it provides a priori information of the map feature information filter to determine where new map features may appear in the robot's field of view. Ps˜,kς represents the probability that the map feature still exists in the robot’s view.(2)
Single-particle update phase:(11)Vk|km|Xk=Vk|k−1m|Xk1−pDm|Xk+Vk|k−1m|Xk∑z∈ZkΛm|Xkckz+∫Λξ|Xkvk|k−1ξ|Xkdξ.After the amplitude information is introduced, the updating formula of AI-PHD-SLAM will vary. Compared with equation (11), the updating formula of AI-PHD-SLAM becomes(12)V˜km|Xk=1−p˜Dτm|XkV˜k|k−1m|Xk+∑z∈Zkp˜Dτm|Xkg˜kz|m,XkV˜k|k−1m|Xkλkc˜kz+∫p˜Dτξg˜kz|ξ,XkV˜k|k−1ξ|Xkdξ,whereg˜k denotes the likelihood function of the map feature observation, c˜k denotes the probability distribution of the clutter in the observation information set, and λk denotes the number of all the maps’ observation information exceeding the threshold at time k. When a threshold is determined, the probability that sensors can detect is merely dependent on the signal-to-noise ratio when the map feature comes into the robot's view as the robot moves.(13)1−p˜Dτm|XkV˜k|k−1m|Xk=1−pDτdpdVk|k−1m|Xkp˜Dτm|Xkg˜kz|m,XkV˜k|k−1m|Xk=pDτdpdgaτa|dgkz|m,XkVk|k−1m|Xkc˜kz=ckzcaτa.Formulas (10) and (12) form the recursive formula of AI-PHD-SLAM. When the SNR of the map feature information is unknown, substitute equations (6) and (7) as the detection probability pDτ and map feature likelihood function gaτa|d into the update equation (12).


## 2.4. Model of Birth Feature-Based Map

The prediction phase plays an important role in estimating the number of map feature targets and map feature accuracy. The method of mixing new target information used in this paper increases the PHD of new map features during the prediction phase, which is expressed asbm|Xk in equation (10). When the robot departs from the initial time, due to the lack of prior information, the observation set at time k-1 is used as bm|Xk of the new map feature information at time k. After the robot updates the map information of detected area through PHD-SLAM, the map features near the robot position in the detected map information are taken as prior information, which is added to the previous time observation set, and they are taken as the current time bm|Xk and used in the current PHD prediction stage.(14)bm|Xk=Zk−1+Mf,where(15)Mf=∪m∈MγM|Xk−1<T=R+ν.In the above two formulas,Mf represents the PHD of the RFS from the map feature near the robot pose in the detected map, ν represents the speed of the robot, R represents the scanning radius of the sensor of the robot, and M represents map feature information that has been obtained by the PHD-SLAM method. A threshold T is determined by the robot speed ν and the sensor scanning radius R. When the distance between the map feature position in the detected map feature set and the robot position is less than this threshold, the map feature is taken as a new map feature that may appear in the robot's sight at the current moment. γm|Xk−1 represents the PHD that formed by the map features that are smaller than the threshold among the detected map features. The implemented pseudocode of the above method is shown in Algorithm 1.Algorithm 1:Mixing newborn map information.
ifM=φbm|Xk=Zk−1else ifM=m1,m2,…,mnfori = 1 to nT=R+νifmi−Xk−1<TMf=Mf+miend forbm|Xk=Zk−1+Mfendbm|Xk of the mixed birth map information strategy includes not only the observation set at time k−1 but also the map feature set near the robot pose in the map information of detected area. When the map features that have been detected in the robot's vision field reappear, they will be added to bm|Xk as the a priori information. After PHD-SLAM map filtering, the weight of reappearing map features will increase; meanwhile the accuracy of estimating the position and number of map feature points is improved.


## 3. Implementation

In this section, the processes of AI-PHD-SLAM and AI-MBMI-PHD-SLAM methods implementation are presented.


### 3.1. AI-PHD-SLAM Implementation

The AI-PHD-SLAM method uses the Rao-Blackwellised particle filter to estimate the pose of the robot. But the position of each particle only represents the possible position of the robot and the weight of the particle represents the possibility [25]. With the known robot position, the PHD filter is used to describe the probability distributions of the map features and to update them using the observation set with amplitude information. The intensity of the PHD-SLAM of the particle set at time k−1 can be expressed as ωk−1i,Xk−1i,νk−1im|X0:k−1ii=1N.N denotes the number of particles, ωk−1idenotes the weight of the i-th particle, X0:k−1i=X0,X1i,X2i,…,Xk−1i denotes the robot pose estimated at the i-th particle at different times, and νk−1im|X0:k−1i denotes the map probability hypothesis density of the i-th particle under its corresponding pose. The posterior distribution of the map corresponding to each particle is implemented by Gaussian mixture terms, where the distribution of PHD is represented by a series of weighted Gaussian terms. The peak of each Gaussian term represents the possible location of the landmark, and the size of the possibility is represented by the weight.


### 3.2. GM-PHD Map Estimation of Single Particle

#### 3.2.1. Prediction of PHD

According to the sampling position of each particle, we can obtain a Gaussian mixture of the form with new features at timek: Jb,ki.(16)bm|Zk−1,Xk−1i=∑j=1Jb,kiηb,ki,jNm;μki,j,Pki,j,where Jb,ki defines the number of Gaussians in the new feature intensity at time k. Meanwhile ηb,ki,j, μki,j, and Pki,j denote the weights, means, and covariances.The prior map PHD for thei-th particle can be expressed as vk−1im|Xk−1i, which is comprised of Gaussian mixtures of the form in (15):(17)νk−1i⋅|Xk−1i=∑j=1Jk−1iηk−1i,jNm;μk−1i,j,Pk−1i,j,where Jk−1i defines the number of Gaussians after PHD filter at time k−1 and ηk−1i,j, μk−1i,j, and Pk−1i,j are the corresponding predicted weights, means, and covariances, respectively, for the j-th Gaussian component of the map PHD of the i-th trajectory.According to equation (11), the Gaussian mixture PHD map prediction is expressed as(18)νk|k−1im|Xki=∑j=1Jk|k−1iηk|k−1i,jNm;μk|k−1i,j,P|k−1i,j,where(19)Jk|k−1i=Jk−1i+Jb,ki,The number of Gaussians representing predicted map features consists of the number of Gaussians in the new feature intensity at timek and the feature intensity after PHD filter at time k−1.


#### 3.2.2. Update of PHD with Amplitude Information

The measurement likelihood is also Gaussian form:(20)νkm|Xki=νk|k−1m|Xki1−PDτd+∑z∈Zk∑j=1Jk|k−1iνG,ki,jz,m|Xki,νG,ki,jz,m|Xki=ηki,jz|XkiNm;μk|ki,j,Pk|ki,j,ηki,jz|Xki=gaa|d⋅ηk|k−1i,j⋅qi,jz,Xkicaτa+gaa|d∑l=1Jk|k−1ηk|k−1i,lqi,lz,Xki,qi,jz,Xki=Nz;Hkμk|k−1i,j,Ski,j.When the map feature SNR is unknown,pDτ and gaa are, respectively, used to replace pDτd and gaa|d in the above formula. In addition, μk|k,Pk|k, and Sk can be obtained by updating PHD filter based on Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF). As presented in this paper, a simple EKF-PHD approach is used to update the posterior map.


#### 3.2.3. Pruning, Merging, and Map Feature Extraction

On one hand, after amplitude information filtering and PHD filtering, low-weight Gaussian components caused by false alarm and clutter are still generated, and the Gaussian components whose weights are below the threshold value need to be removed. On the other hand, for each particle, due to the error of its own position, the estimation for each real map feature is biased. Therefore, before the state extraction, if the distance between Gaussian components is less than the thresholdTm, these Gaussian components could be merged. After pruning and merging, if the weight of the Gaussian component is greater than the map feature extraction threshold Tf, these Gaussian components can be extracted. The mean of Gaussian components represents the position of the map features. The number of the Gaussian components bigger than the threshold Tf is known as the number of the map features.


### 3.3. Robot Pose Estimation

Rao-Blackwellised particle filter is used to estimate the position of the robot. At timek, the estimated state of the robot is the weighted average of all the particles position. The estimation process of robot’s pose consists of two parts: particle importance sampling and particle resampling [26].(1)
Particle Importance SamplingThe posterior distribution of the robot’s pose is represented by a set of particles with different weights, asw˜ki,X˜kii=1N. w˜ki denotes the weight of the i-th sampled particle at time k, and Xki denotes its corresponding pose. The recurrence formula of weight is(21)w˜ki=gkZk|Z0:k−1,X˜0:kiwk−1i.From the above equation,gkZk|Z0:k−1,X˜0:ki is the key of the weight recursion. The calculation of gkZk|Z0:k−1,X˜0:ki is no longer defined in Euclidean space but is defined in the random finite set space. In order to improve the computational efficiency, this paper chooses an empty map strategy with simple structure and less computation. gkZk|Z0:k−1,X˜0:ki is calculated as(22)gkZk|Z0:k−1,X˜0:ki≈CkZk×expm^k−m^k|k−1−λc.In the formula,CkZk indicates that the k-time clutter forms the PHD, m^k is the number of updated map features, m^k|k−1 is the number of predicted map features, and λc is the number of the clutters.(2)
Particle ResamplingIn order to solve the problem of particle degeneration, particle resampling is needed. After particle resampling, a new particle set,wki,Xkii=1N, is obtained.


### 3.4. AI-MBMI-PHD-SLAM Implementation

As can be seen from the second chapter, the difference between AI-MBMI-PHD-SLAM and AI-PHD-SLAM lies inbm|Xk in equation (10), which is the PHD of the new map feature information. bm|Xk provides map feature information filter prior information to predict where new map features may appear in the robot's field of view. However, the SLAM problem starts from an unknown location in an unknown environment, and it lacks prior information. In Section 3.1, the AI-PHD-SLAM method only utilizes the observation set at time k−1 as bm|Xk at time K, so there is a lag in estimation of the map features in the robot’s sight.When the robot returns to the detected environment or the detected map features reappear in the robot's field of view, the method cannot use the detected map information to improve the estimation accuracy of the map features at the current time. According to Section2.4, use the mixed birth map information method to replace bm|Zk−1,Xk−1i in the prediction stage in formula (16). In this way, BB includes not only the Gaussian term formed by the observation set at the previous moment but also the Gaussian term of the map feature whose distance between the map feature position and the robot position in the detected map information is less than the threshold T. In addition, AI-MBMI-PHD-SLAM and AI-PHD-SLAM have the same steps in map estimation and robot pose estimation.


## 3.1. AI-PHD-SLAM Implementation

The AI-PHD-SLAM method uses the Rao-Blackwellised particle filter to estimate the pose of the robot. But the position of each particle only represents the possible position of the robot and the weight of the particle represents the possibility [25]. With the known robot position, the PHD filter is used to describe the probability distributions of the map features and to update them using the observation set with amplitude information. The intensity of the PHD-SLAM of the particle set at time k−1 can be expressed as ωk−1i,Xk−1i,νk−1im|X0:k−1ii=1N.N denotes the number of particles, ωk−1idenotes the weight of the i-th particle, X0:k−1i=X0,X1i,X2i,…,Xk−1i denotes the robot pose estimated at the i-th particle at different times, and νk−1im|X0:k−1i denotes the map probability hypothesis density of the i-th particle under its corresponding pose. The posterior distribution of the map corresponding to each particle is implemented by Gaussian mixture terms, where the distribution of PHD is represented by a series of weighted Gaussian terms. The peak of each Gaussian term represents the possible location of the landmark, and the size of the possibility is represented by the weight.


## 3.2. GM-PHD Map Estimation of Single Particle

### 3.2.1. Prediction of PHD

According to the sampling position of each particle, we can obtain a Gaussian mixture of the form with new features at timek: Jb,ki.(16)bm|Zk−1,Xk−1i=∑j=1Jb,kiηb,ki,jNm;μki,j,Pki,j,where Jb,ki defines the number of Gaussians in the new feature intensity at time k. Meanwhile ηb,ki,j, μki,j, and Pki,j denote the weights, means, and covariances.The prior map PHD for thei-th particle can be expressed as vk−1im|Xk−1i, which is comprised of Gaussian mixtures of the form in (15):(17)νk−1i⋅|Xk−1i=∑j=1Jk−1iηk−1i,jNm;μk−1i,j,Pk−1i,j,where Jk−1i defines the number of Gaussians after PHD filter at time k−1 and ηk−1i,j, μk−1i,j, and Pk−1i,j are the corresponding predicted weights, means, and covariances, respectively, for the j-th Gaussian component of the map PHD of the i-th trajectory.According to equation (11), the Gaussian mixture PHD map prediction is expressed as(18)νk|k−1im|Xki=∑j=1Jk|k−1iηk|k−1i,jNm;μk|k−1i,j,P|k−1i,j,where(19)Jk|k−1i=Jk−1i+Jb,ki,The number of Gaussians representing predicted map features consists of the number of Gaussians in the new feature intensity at timek and the feature intensity after PHD filter at time k−1.


### 3.2.2. Update of PHD with Amplitude Information

The measurement likelihood is also Gaussian form:(20)νkm|Xki=νk|k−1m|Xki1−PDτd+∑z∈Zk∑j=1Jk|k−1iνG,ki,jz,m|Xki,νG,ki,jz,m|Xki=ηki,jz|XkiNm;μk|ki,j,Pk|ki,j,ηki,jz|Xki=gaa|d⋅ηk|k−1i,j⋅qi,jz,Xkicaτa+gaa|d∑l=1Jk|k−1ηk|k−1i,lqi,lz,Xki,qi,jz,Xki=Nz;Hkμk|k−1i,j,Ski,j.When the map feature SNR is unknown,pDτ and gaa are, respectively, used to replace pDτd and gaa|d in the above formula. In addition, μk|k,Pk|k, and Sk can be obtained by updating PHD filter based on Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF). As presented in this paper, a simple EKF-PHD approach is used to update the posterior map.


### 3.2.3. Pruning, Merging, and Map Feature Extraction

On one hand, after amplitude information filtering and PHD filtering, low-weight Gaussian components caused by false alarm and clutter are still generated, and the Gaussian components whose weights are below the threshold value need to be removed. On the other hand, for each particle, due to the error of its own position, the estimation for each real map feature is biased. Therefore, before the state extraction, if the distance between Gaussian components is less than the thresholdTm, these Gaussian components could be merged. After pruning and merging, if the weight of the Gaussian component is greater than the map feature extraction threshold Tf, these Gaussian components can be extracted. The mean of Gaussian components represents the position of the map features. The number of the Gaussian components bigger than the threshold Tf is known as the number of the map features.


## 3.2.1. Prediction of PHD

According to the sampling position of each particle, we can obtain a Gaussian mixture of the form with new features at timek: Jb,ki.(16)bm|Zk−1,Xk−1i=∑j=1Jb,kiηb,ki,jNm;μki,j,Pki,j,where Jb,ki defines the number of Gaussians in the new feature intensity at time k. Meanwhile ηb,ki,j, μki,j, and Pki,j denote the weights, means, and covariances.The prior map PHD for thei-th particle can be expressed as vk−1im|Xk−1i, which is comprised of Gaussian mixtures of the form in (15):(17)νk−1i⋅|Xk−1i=∑j=1Jk−1iηk−1i,jNm;μk−1i,j,Pk−1i,j,where Jk−1i defines the number of Gaussians after PHD filter at time k−1 and ηk−1i,j, μk−1i,j, and Pk−1i,j are the corresponding predicted weights, means, and covariances, respectively, for the j-th Gaussian component of the map PHD of the i-th trajectory.According to equation (11), the Gaussian mixture PHD map prediction is expressed as(18)νk|k−1im|Xki=∑j=1Jk|k−1iηk|k−1i,jNm;μk|k−1i,j,P|k−1i,j,where(19)Jk|k−1i=Jk−1i+Jb,ki,The number of Gaussians representing predicted map features consists of the number of Gaussians in the new feature intensity at timek and the feature intensity after PHD filter at time k−1.


## 3.2.2. Update of PHD with Amplitude Information

The measurement likelihood is also Gaussian form:(20)νkm|Xki=νk|k−1m|Xki1−PDτd+∑z∈Zk∑j=1Jk|k−1iνG,ki,jz,m|Xki,νG,ki,jz,m|Xki=ηki,jz|XkiNm;μk|ki,j,Pk|ki,j,ηki,jz|Xki=gaa|d⋅ηk|k−1i,j⋅qi,jz,Xkicaτa+gaa|d∑l=1Jk|k−1ηk|k−1i,lqi,lz,Xki,qi,jz,Xki=Nz;Hkμk|k−1i,j,Ski,j.When the map feature SNR is unknown,pDτ and gaa are, respectively, used to replace pDτd and gaa|d in the above formula. In addition, μk|k,Pk|k, and Sk can be obtained by updating PHD filter based on Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF). As presented in this paper, a simple EKF-PHD approach is used to update the posterior map.


## 3.2.3. Pruning, Merging, and Map Feature Extraction

On one hand, after amplitude information filtering and PHD filtering, low-weight Gaussian components caused by false alarm and clutter are still generated, and the Gaussian components whose weights are below the threshold value need to be removed. On the other hand, for each particle, due to the error of its own position, the estimation for each real map feature is biased. Therefore, before the state extraction, if the distance between Gaussian components is less than the thresholdTm, these Gaussian components could be merged. After pruning and merging, if the weight of the Gaussian component is greater than the map feature extraction threshold Tf, these Gaussian components can be extracted. The mean of Gaussian components represents the position of the map features. The number of the Gaussian components bigger than the threshold Tf is known as the number of the map features.


## 3.3. Robot Pose Estimation

Rao-Blackwellised particle filter is used to estimate the position of the robot. At timek, the estimated state of the robot is the weighted average of all the particles position. The estimation process of robot’s pose consists of two parts: particle importance sampling and particle resampling [26].(1)
Particle Importance SamplingThe posterior distribution of the robot’s pose is represented by a set of particles with different weights, asw˜ki,X˜kii=1N. w˜ki denotes the weight of the i-th sampled particle at time k, and Xki denotes its corresponding pose. The recurrence formula of weight is(21)w˜ki=gkZk|Z0:k−1,X˜0:kiwk−1i.From the above equation,gkZk|Z0:k−1,X˜0:ki is the key of the weight recursion. The calculation of gkZk|Z0:k−1,X˜0:ki is no longer defined in Euclidean space but is defined in the random finite set space. In order to improve the computational efficiency, this paper chooses an empty map strategy with simple structure and less computation. gkZk|Z0:k−1,X˜0:ki is calculated as(22)gkZk|Z0:k−1,X˜0:ki≈CkZk×expm^k−m^k|k−1−λc.In the formula,CkZk indicates that the k-time clutter forms the PHD, m^k is the number of updated map features, m^k|k−1 is the number of predicted map features, and λc is the number of the clutters.(2)
Particle ResamplingIn order to solve the problem of particle degeneration, particle resampling is needed. After particle resampling, a new particle set,wki,Xkii=1N, is obtained.


## 3.4. AI-MBMI-PHD-SLAM Implementation

As can be seen from the second chapter, the difference between AI-MBMI-PHD-SLAM and AI-PHD-SLAM lies inbm|Xk in equation (10), which is the PHD of the new map feature information. bm|Xk provides map feature information filter prior information to predict where new map features may appear in the robot's field of view. However, the SLAM problem starts from an unknown location in an unknown environment, and it lacks prior information. In Section 3.1, the AI-PHD-SLAM method only utilizes the observation set at time k−1 as bm|Xk at time K, so there is a lag in estimation of the map features in the robot’s sight.When the robot returns to the detected environment or the detected map features reappear in the robot's field of view, the method cannot use the detected map information to improve the estimation accuracy of the map features at the current time. According to Section2.4, use the mixed birth map information method to replace bm|Zk−1,Xk−1i in the prediction stage in formula (16). In this way, BB includes not only the Gaussian term formed by the observation set at the previous moment but also the Gaussian term of the map feature whose distance between the map feature position and the robot position in the detected map information is less than the threshold T. In addition, AI-MBMI-PHD-SLAM and AI-PHD-SLAM have the same steps in map estimation and robot pose estimation.


## 4. Results and Analysis

This section introduces the experimental results and data analysis of RB-PHD-SLAM, AI-PHD-SLAM, and AI-MBMI-PHD-SLAM methods. The comparative analysis results of the above three methods are given for the underwater simulation environment and the real indoor environment.


### 4.1. Simulation Environment and Simulation Results

In the Matlab 2016b platform simulation environment, the map size is 50 m ∗ 60 m with a total of 28 map coordinates. The sensor of the underwater robot is the sonar. The noise in the simulation experiment adopts the Gaussian noise. The specific model parameter of the robot movement is shown in Table1.Table 1
Kinematic model parameters of the robot.
ParameterValueSpeed of underwater robot (m/s)1.5Control speed noise (m)0.3Sensor scanning angle (°)360Sensor scanning radius (m)10Observation angle noise (°)1Observation distance noise (m)0.1Under the same environment parameters, the map simulation results of RB-PHD-SLAM, AI-PHD-SLAM, and AI-MBMI-PHD-SLAM are shown in Figures1–3.Figure 1
RB-PHD-SLAM simulation results.Figure 2
AI-PHD-SLAM simulation results.Figure 3
AI-MBMI-PHD-SLAM simulation results.In order to better compare and analyze the effectiveness of the two improved methods proposed in this paper, the analysis of experimental simulation results is divided into two parts. The first part compares and analyzes the performance of RB-PHD-SLAM and AI-PHD-SLAM methods to prove the effectiveness of adding amplitude information. The second part compares and analyzes the performance of AI-MBMI-PHD-SLAM and AI-PHD-SLAM to prove that adding mixed birth map information method on the basis of amplitude auxiliary information can make the method more effective. The simulation results are analyzed from three aspects: map feature estimation accuracy, robot positioning error, and method calculation time.


### 4.2. Comparative Analysis of RB-PHD-SLAM, AI-MBMI-PHD-SLAM, and AI-PHD-SLAM Simulation Results

#### 4.2.1. Analysis of Robot Positioning Performance

The root-mean-square error (RMSE) of the robot position is used as an index to evaluate the position error of the robot. The simulation experiment compared the RMSE of robot position at each moment and evaluated the performance estimation of the two methods. In this paper, the number of sampled particles of the robot’s pose is 20 with 50 times simulation experiments. The comparison results are shown in Figure4.Figure 4
The robot pose estimation error comparison.It can be seen from Figure4 that the difference between the three methods is not obvious within the initial 15 steps. However, as the running time increases, the position error is accumulated. Firstly, the results show that AI-PHD-SLAM method is significantly lower than RB-PHD-SLAM method, and its accuracy is significantly improved. Secondly, the attitude estimation error of AI-MBMI-PHD-SLAM method is significantly smaller than that of AI-PHD-SLAM method. This indicates that AI-MBMI-PHD-SLAM method has better accuracy in robot attitude estimation than AI-PHD-SLAM method. The comparison of the data is shown in Table 2.Table 2
Performance comparison of RB-PHD-SLAM, AI-PHD-SLAM, and AI-MBMI-PHD-SLAM
AI-MBMI-PHD-SLAMAI-PHD-SLAMRB-PHD-SLAMRoot-mean-square error of robot position (m)0.170.240.35The final estimated number of map features29.1029.9536.85The miss distance of map feature (OSPA) (m)0.330.560.81The time of the method computation (s)0.520.371.28


#### 4.2.2. Map Feature Estimation Results and Performance Analysis

The estimation result of the map feature is evaluated from two aspects: one is the estimate of the number of map features at each time, and the other is the estimation error of the map feature position at each moment. As is shown in Figure5, the number of map features is compared with the actual number of map features in the same circumstances.Figure 5
Estimation of map features.It can be seen from the comparison that there is no significant difference in the estimation of map features between the three methods at the beginning. However, with the expansion of scanning range, the estimation of map feature number by A and B methods is closer to the actual map feature number. However, in the final estimation of the number of map features, AI-MBMI-PHD-SLAM method is closer to the real number of map features than AI-PHD-SLAM method.PHD-SLAM uses the form of random finite set to express map feature information. Each map feature information is disordered. Therefore, the traditional map feature error calculation method cannot be applied to PHD-SLAM. In this paper, the optimal subpattern assignment (OSPA) algorithm is used to calculate the error distance of the difference between sets. In this experiment, the truncation distance is 3 and the order is 2. The results show that AI-MBMI-PHD-SLAM is superior to AI-PHD-SLAM and RB-PHD-SLAM in the estimation of the number of map features.The estimation errors of the three methods for map features are shown in Figure6.Figure 6
Map features OSPA error contrast.It can be seen from the comparison results that the performance of AI-PHD-SLAM is better than that of RB-PHD-SLAM from the 45th step, and, with the increase of running time, the estimation error of this method for map features is significantly smaller than that of RB-PHD-SLAM. The accuracy of AI-PHD-SLAM method is improved obviously. The OSPA error of AI-MBMI-PHD-SLAM is always smaller than that of AI-PHD-SLAM, which indicates that the accuracy of AI-MBMI-PHD-SLAM is improved compared with that of AI-PHD-SLAM.The comparison of the data is shown in Table2.


#### 4.2.3. Computational Time Comparison

According to Section2.1, it is known that, after setting the amplitude threshold τ, removing the observation information whose magnitude is less than τ will reduce the number of Gaussian terms and reduce the computation time of the whole method. In this simulation experiment, the number of clutters is 15, and the two methods run 50 times to get their average computing time. The experimental results are shown in Figure 7.Figure 7
Run time comparison.As shown in Figure7, we can see that AI-PHD-SLAM method and AI-MBMI-PHD-SLAM method can effectively shorten the overall simulation time in each step in the same clutter environment of the same simulation platform, which reduce the calculation amount and improve the operation efficiency of PHD-SLAM. The time taken by the AI-MBMI-PHD-SLAM method is slightly longer than that taken by the AI-PHD-SLAM method. This is because the AI-MBMI-PHD-SLAM method has more prior information in the prediction stage than the AI-PHD-SLAM method, but the time difference between the two methods is not large. It can be seen from Figure 7 that the simulation time used by the AI-MBMI-PHD-SLAM method is still less than that used by the RB-PHD-SLAM method. Although the calculation of the AI-MBMI-PHD-SLAM method has increased a bit, this method has improved the accuracy of map feature estimation and robot pose estimation a lot.


### 4.3. Real Environment and Real Result

The sensor used in this section is TI company’s AWR1642 radar plate, which is loaded on the four-wheel trolley for real data collection. Firstly, through the robot test, get the real measurement data. Then the method of real data is processed on the computer to get the final test junction of SLAM. Finally, the results are analyzed and compared. The experimental robot is shown in Figure8. The real experimental environment is a loop-shaped corridor, as shown in Figure 8.Figure 8
Real environment. (a) Robot turning diagram. (b) Robot moving forward.
(a)(b)As shown in Figures9–11, “○,” “+,” and “×,” respectively, represent the real position of the map feature, estimated feature, and noise of the map. The black-dotted line represents the truth trajectory of the robot. The red solid line, blue solid line, and light blue solid line represent the robot trajectory estimation.Figure 9
AI-MBMI-PHD-SLAM real results.Figure 10
AI -PHD-SLAM real results.Figure 11
RB-PHD-SLAM real results.


#### 4.3.1. Analysis of Robot Positioning Performance

This section mainly introduces the comparison and analysis of the error results of robot pose estimation by AI-MBMI-PHD-SLAM method, AI- PHD-SLAM method, and RB-PHD-SLAM method in the real map environment. The RMSE comparison results of AI-MBMI-PHD-SLAM method, AI-PHD-SLAM method, and RB-PHD-SLAM method are shown in Figure12.Figure 12
Comparison of robot position error.


#### 4.3.2. Map Feature Estimation Results and Performance Analysis

From Figures13 and 14, we can see that, in the first 20 steps, the number of map features estimated by both methods is close to the true number of features, but, after 20 steps, the number of map features estimated by the RB-PHD-SLAM method is more than the number of real map features. The number of map features estimated by AI-MBMI-PHD-SLAM method is almost the same as the actual number of map features. It shows that AI-MBMI-PHD-SLAM method is superior to RB-PHD-SLAM method in the estimation of map features in real environment.Figure 13
Comparison of the number of features in the map.Figure 14
Comparison of the estimated number error of features.As can be seen from the results in Figure15, the OSPA of AI-MBMI-PHD-SLAM method is always superior to the RB-PHD-SLAM method and AI-PHD-SLAM method in general. Therefore, AI-MBMI-PHD-SLAM method has higher accuracy in the estimation of map features in real environment, and the overall performance is more stable.Figure 15
Comparison of OSPA error.As can be seen from Table3, AI-PHD-SLAM method is superior to RB-PHD-SLAM method and AI-MBMI-PHD-SLAM method is superior to AI-PHD-SLAM method in major comparison points. Firstly, the comparison on the position error of robot shows that AI-PHD-SLAM method is 33.8% better than RB-PHD-SLAM method, and AI-MBMI-PHD-SLAM method is 24.7% better than AI-PHD-SLAM method. Secondly, in the estimation of map features, AI-PHD-SLAM method and AI-MBMI-PHD-SLAM method are generally superior to RB-PHD-SLAM method. Thirdly, in the aspect of OSPA, it can be seen that AI-PHD-SLAM method is increased by 34.0% compared to RB-PHD-SLAM method and AI-MBMI-PHD-SLAM method is increased by 29.7% compared to AI-PHD-SLAM method.Table 3
Comparison of data in real environment.
AI-MBMI-PHD-SLAMAI-PHD-SLAMRB-PHD-SLAMRobot position error0.280.370.56The number of features in the map333340The estimated number error of features007OSPA error0.630.9031.37From the above analysis, the following conclusions can be drawn. AI-MBMI-PHD-SLAM method and AI-PHD-SLAM method are more accurately than RB-PHD-SLAM method in the respect of the estimated accuracy of the number of map features. In terms of robot position and attitude estimation and OPSA estimation, the performance of AI-MBMI-PHD-SLAM method is better than that of AI-PHD-SLAM method and is significantly better than that of RB-PHD-SLAM method.


## 4.1. Simulation Environment and Simulation Results

In the Matlab 2016b platform simulation environment, the map size is 50 m ∗ 60 m with a total of 28 map coordinates. The sensor of the underwater robot is the sonar. The noise in the simulation experiment adopts the Gaussian noise. The specific model parameter of the robot movement is shown in Table1.Table 1
Kinematic model parameters of the robot.
ParameterValueSpeed of underwater robot (m/s)1.5Control speed noise (m)0.3Sensor scanning angle (°)360Sensor scanning radius (m)10Observation angle noise (°)1Observation distance noise (m)0.1Under the same environment parameters, the map simulation results of RB-PHD-SLAM, AI-PHD-SLAM, and AI-MBMI-PHD-SLAM are shown in Figures1–3.Figure 1
RB-PHD-SLAM simulation results.Figure 2
AI-PHD-SLAM simulation results.Figure 3
AI-MBMI-PHD-SLAM simulation results.In order to better compare and analyze the effectiveness of the two improved methods proposed in this paper, the analysis of experimental simulation results is divided into two parts. The first part compares and analyzes the performance of RB-PHD-SLAM and AI-PHD-SLAM methods to prove the effectiveness of adding amplitude information. The second part compares and analyzes the performance of AI-MBMI-PHD-SLAM and AI-PHD-SLAM to prove that adding mixed birth map information method on the basis of amplitude auxiliary information can make the method more effective. The simulation results are analyzed from three aspects: map feature estimation accuracy, robot positioning error, and method calculation time.


## 4.2. Comparative Analysis of RB-PHD-SLAM, AI-MBMI-PHD-SLAM, and AI-PHD-SLAM Simulation Results

### 4.2.1. Analysis of Robot Positioning Performance

The root-mean-square error (RMSE) of the robot position is used as an index to evaluate the position error of the robot. The simulation experiment compared the RMSE of robot position at each moment and evaluated the performance estimation of the two methods. In this paper, the number of sampled particles of the robot’s pose is 20 with 50 times simulation experiments. The comparison results are shown in Figure4.Figure 4
The robot pose estimation error comparison.It can be seen from Figure4 that the difference between the three methods is not obvious within the initial 15 steps. However, as the running time increases, the position error is accumulated. Firstly, the results show that AI-PHD-SLAM method is significantly lower than RB-PHD-SLAM method, and its accuracy is significantly improved. Secondly, the attitude estimation error of AI-MBMI-PHD-SLAM method is significantly smaller than that of AI-PHD-SLAM method. This indicates that AI-MBMI-PHD-SLAM method has better accuracy in robot attitude estimation than AI-PHD-SLAM method. The comparison of the data is shown in Table 2.Table 2
Performance comparison of RB-PHD-SLAM, AI-PHD-SLAM, and AI-MBMI-PHD-SLAM
AI-MBMI-PHD-SLAMAI-PHD-SLAMRB-PHD-SLAMRoot-mean-square error of robot position (m)0.170.240.35The final estimated number of map features29.1029.9536.85The miss distance of map feature (OSPA) (m)0.330.560.81The time of the method computation (s)0.520.371.28


### 4.2.2. Map Feature Estimation Results and Performance Analysis

The estimation result of the map feature is evaluated from two aspects: one is the estimate of the number of map features at each time, and the other is the estimation error of the map feature position at each moment. As is shown in Figure5, the number of map features is compared with the actual number of map features in the same circumstances.Figure 5
Estimation of map features.It can be seen from the comparison that there is no significant difference in the estimation of map features between the three methods at the beginning. However, with the expansion of scanning range, the estimation of map feature number by A and B methods is closer to the actual map feature number. However, in the final estimation of the number of map features, AI-MBMI-PHD-SLAM method is closer to the real number of map features than AI-PHD-SLAM method.PHD-SLAM uses the form of random finite set to express map feature information. Each map feature information is disordered. Therefore, the traditional map feature error calculation method cannot be applied to PHD-SLAM. In this paper, the optimal subpattern assignment (OSPA) algorithm is used to calculate the error distance of the difference between sets. In this experiment, the truncation distance is 3 and the order is 2. The results show that AI-MBMI-PHD-SLAM is superior to AI-PHD-SLAM and RB-PHD-SLAM in the estimation of the number of map features.The estimation errors of the three methods for map features are shown in Figure6.Figure 6
Map features OSPA error contrast.It can be seen from the comparison results that the performance of AI-PHD-SLAM is better than that of RB-PHD-SLAM from the 45th step, and, with the increase of running time, the estimation error of this method for map features is significantly smaller than that of RB-PHD-SLAM. The accuracy of AI-PHD-SLAM method is improved obviously. The OSPA error of AI-MBMI-PHD-SLAM is always smaller than that of AI-PHD-SLAM, which indicates that the accuracy of AI-MBMI-PHD-SLAM is improved compared with that of AI-PHD-SLAM.The comparison of the data is shown in Table2.


### 4.2.3. Computational Time Comparison

According to Section2.1, it is known that, after setting the amplitude threshold τ, removing the observation information whose magnitude is less than τ will reduce the number of Gaussian terms and reduce the computation time of the whole method. In this simulation experiment, the number of clutters is 15, and the two methods run 50 times to get their average computing time. The experimental results are shown in Figure 7.Figure 7
Run time comparison.As shown in Figure7, we can see that AI-PHD-SLAM method and AI-MBMI-PHD-SLAM method can effectively shorten the overall simulation time in each step in the same clutter environment of the same simulation platform, which reduce the calculation amount and improve the operation efficiency of PHD-SLAM. The time taken by the AI-MBMI-PHD-SLAM method is slightly longer than that taken by the AI-PHD-SLAM method. This is because the AI-MBMI-PHD-SLAM method has more prior information in the prediction stage than the AI-PHD-SLAM method, but the time difference between the two methods is not large. It can be seen from Figure 7 that the simulation time used by the AI-MBMI-PHD-SLAM method is still less than that used by the RB-PHD-SLAM method. Although the calculation of the AI-MBMI-PHD-SLAM method has increased a bit, this method has improved the accuracy of map feature estimation and robot pose estimation a lot.


## 4.2.1. Analysis of Robot Positioning Performance

The root-mean-square error (RMSE) of the robot position is used as an index to evaluate the position error of the robot. The simulation experiment compared the RMSE of robot position at each moment and evaluated the performance estimation of the two methods. In this paper, the number of sampled particles of the robot’s pose is 20 with 50 times simulation experiments. The comparison results are shown in Figure4.Figure 4
The robot pose estimation error comparison.It can be seen from Figure4 that the difference between the three methods is not obvious within the initial 15 steps. However, as the running time increases, the position error is accumulated. Firstly, the results show that AI-PHD-SLAM method is significantly lower than RB-PHD-SLAM method, and its accuracy is significantly improved. Secondly, the attitude estimation error of AI-MBMI-PHD-SLAM method is significantly smaller than that of AI-PHD-SLAM method. This indicates that AI-MBMI-PHD-SLAM method has better accuracy in robot attitude estimation than AI-PHD-SLAM method. The comparison of the data is shown in Table 2.Table 2
Performance comparison of RB-PHD-SLAM, AI-PHD-SLAM, and AI-MBMI-PHD-SLAM
AI-MBMI-PHD-SLAMAI-PHD-SLAMRB-PHD-SLAMRoot-mean-square error of robot position (m)0.170.240.35The final estimated number of map features29.1029.9536.85The miss distance of map feature (OSPA) (m)0.330.560.81The time of the method computation (s)0.520.371.28


## 4.2.2. Map Feature Estimation Results and Performance Analysis

The estimation result of the map feature is evaluated from two aspects: one is the estimate of the number of map features at each time, and the other is the estimation error of the map feature position at each moment. As is shown in Figure5, the number of map features is compared with the actual number of map features in the same circumstances.Figure 5
Estimation of map features.It can be seen from the comparison that there is no significant difference in the estimation of map features between the three methods at the beginning. However, with the expansion of scanning range, the estimation of map feature number by A and B methods is closer to the actual map feature number. However, in the final estimation of the number of map features, AI-MBMI-PHD-SLAM method is closer to the real number of map features than AI-PHD-SLAM method.PHD-SLAM uses the form of random finite set to express map feature information. Each map feature information is disordered. Therefore, the traditional map feature error calculation method cannot be applied to PHD-SLAM. In this paper, the optimal subpattern assignment (OSPA) algorithm is used to calculate the error distance of the difference between sets. In this experiment, the truncation distance is 3 and the order is 2. The results show that AI-MBMI-PHD-SLAM is superior to AI-PHD-SLAM and RB-PHD-SLAM in the estimation of the number of map features.The estimation errors of the three methods for map features are shown in Figure6.Figure 6
Map features OSPA error contrast.It can be seen from the comparison results that the performance of AI-PHD-SLAM is better than that of RB-PHD-SLAM from the 45th step, and, with the increase of running time, the estimation error of this method for map features is significantly smaller than that of RB-PHD-SLAM. The accuracy of AI-PHD-SLAM method is improved obviously. The OSPA error of AI-MBMI-PHD-SLAM is always smaller than that of AI-PHD-SLAM, which indicates that the accuracy of AI-MBMI-PHD-SLAM is improved compared with that of AI-PHD-SLAM.The comparison of the data is shown in Table2.


## 4.2.3. Computational Time Comparison

According to Section2.1, it is known that, after setting the amplitude threshold τ, removing the observation information whose magnitude is less than τ will reduce the number of Gaussian terms and reduce the computation time of the whole method. In this simulation experiment, the number of clutters is 15, and the two methods run 50 times to get their average computing time. The experimental results are shown in Figure 7.Figure 7
Run time comparison.As shown in Figure7, we can see that AI-PHD-SLAM method and AI-MBMI-PHD-SLAM method can effectively shorten the overall simulation time in each step in the same clutter environment of the same simulation platform, which reduce the calculation amount and improve the operation efficiency of PHD-SLAM. The time taken by the AI-MBMI-PHD-SLAM method is slightly longer than that taken by the AI-PHD-SLAM method. This is because the AI-MBMI-PHD-SLAM method has more prior information in the prediction stage than the AI-PHD-SLAM method, but the time difference between the two methods is not large. It can be seen from Figure 7 that the simulation time used by the AI-MBMI-PHD-SLAM method is still less than that used by the RB-PHD-SLAM method. Although the calculation of the AI-MBMI-PHD-SLAM method has increased a bit, this method has improved the accuracy of map feature estimation and robot pose estimation a lot.


## 4.3. Real Environment and Real Result

The sensor used in this section is TI company’s AWR1642 radar plate, which is loaded on the four-wheel trolley for real data collection. Firstly, through the robot test, get the real measurement data. Then the method of real data is processed on the computer to get the final test junction of SLAM. Finally, the results are analyzed and compared. The experimental robot is shown in Figure8. The real experimental environment is a loop-shaped corridor, as shown in Figure 8.Figure 8
Real environment. (a) Robot turning diagram. (b) Robot moving forward.
(a)(b)As shown in Figures9–11, “○,” “+,” and “×,” respectively, represent the real position of the map feature, estimated feature, and noise of the map. The black-dotted line represents the truth trajectory of the robot. The red solid line, blue solid line, and light blue solid line represent the robot trajectory estimation.Figure 9
AI-MBMI-PHD-SLAM real results.Figure 10
AI -PHD-SLAM real results.Figure 11
RB-PHD-SLAM real results.


### 4.3.1. Analysis of Robot Positioning Performance

This section mainly introduces the comparison and analysis of the error results of robot pose estimation by AI-MBMI-PHD-SLAM method, AI- PHD-SLAM method, and RB-PHD-SLAM method in the real map environment. The RMSE comparison results of AI-MBMI-PHD-SLAM method, AI-PHD-SLAM method, and RB-PHD-SLAM method are shown in Figure12.Figure 12
Comparison of robot position error.


### 4.3.2. Map Feature Estimation Results and Performance Analysis

From Figures13 and 14, we can see that, in the first 20 steps, the number of map features estimated by both methods is close to the true number of features, but, after 20 steps, the number of map features estimated by the RB-PHD-SLAM method is more than the number of real map features. The number of map features estimated by AI-MBMI-PHD-SLAM method is almost the same as the actual number of map features. It shows that AI-MBMI-PHD-SLAM method is superior to RB-PHD-SLAM method in the estimation of map features in real environment.Figure 13
Comparison of the number of features in the map.Figure 14
Comparison of the estimated number error of features.As can be seen from the results in Figure15, the OSPA of AI-MBMI-PHD-SLAM method is always superior to the RB-PHD-SLAM method and AI-PHD-SLAM method in general. Therefore, AI-MBMI-PHD-SLAM method has higher accuracy in the estimation of map features in real environment, and the overall performance is more stable.Figure 15
Comparison of OSPA error.As can be seen from Table3, AI-PHD-SLAM method is superior to RB-PHD-SLAM method and AI-MBMI-PHD-SLAM method is superior to AI-PHD-SLAM method in major comparison points. Firstly, the comparison on the position error of robot shows that AI-PHD-SLAM method is 33.8% better than RB-PHD-SLAM method, and AI-MBMI-PHD-SLAM method is 24.7% better than AI-PHD-SLAM method. Secondly, in the estimation of map features, AI-PHD-SLAM method and AI-MBMI-PHD-SLAM method are generally superior to RB-PHD-SLAM method. Thirdly, in the aspect of OSPA, it can be seen that AI-PHD-SLAM method is increased by 34.0% compared to RB-PHD-SLAM method and AI-MBMI-PHD-SLAM method is increased by 29.7% compared to AI-PHD-SLAM method.Table 3
Comparison of data in real environment.
AI-MBMI-PHD-SLAMAI-PHD-SLAMRB-PHD-SLAMRobot position error0.280.370.56The number of features in the map333340The estimated number error of features007OSPA error0.630.9031.37From the above analysis, the following conclusions can be drawn. AI-MBMI-PHD-SLAM method and AI-PHD-SLAM method are more accurately than RB-PHD-SLAM method in the respect of the estimated accuracy of the number of map features. In terms of robot position and attitude estimation and OPSA estimation, the performance of AI-MBMI-PHD-SLAM method is better than that of AI-PHD-SLAM method and is significantly better than that of RB-PHD-SLAM method.


## 4.3.1. Analysis of Robot Positioning Performance

This section mainly introduces the comparison and analysis of the error results of robot pose estimation by AI-MBMI-PHD-SLAM method, AI- PHD-SLAM method, and RB-PHD-SLAM method in the real map environment. The RMSE comparison results of AI-MBMI-PHD-SLAM method, AI-PHD-SLAM method, and RB-PHD-SLAM method are shown in Figure12.Figure 12
Comparison of robot position error.


## 4.3.2. Map Feature Estimation Results and Performance Analysis

From Figures13 and 14, we can see that, in the first 20 steps, the number of map features estimated by both methods is close to the true number of features, but, after 20 steps, the number of map features estimated by the RB-PHD-SLAM method is more than the number of real map features. The number of map features estimated by AI-MBMI-PHD-SLAM method is almost the same as the actual number of map features. It shows that AI-MBMI-PHD-SLAM method is superior to RB-PHD-SLAM method in the estimation of map features in real environment.Figure 13
Comparison of the number of features in the map.Figure 14
Comparison of the estimated number error of features.As can be seen from the results in Figure15, the OSPA of AI-MBMI-PHD-SLAM method is always superior to the RB-PHD-SLAM method and AI-PHD-SLAM method in general. Therefore, AI-MBMI-PHD-SLAM method has higher accuracy in the estimation of map features in real environment, and the overall performance is more stable.Figure 15
Comparison of OSPA error.As can be seen from Table3, AI-PHD-SLAM method is superior to RB-PHD-SLAM method and AI-MBMI-PHD-SLAM method is superior to AI-PHD-SLAM method in major comparison points. Firstly, the comparison on the position error of robot shows that AI-PHD-SLAM method is 33.8% better than RB-PHD-SLAM method, and AI-MBMI-PHD-SLAM method is 24.7% better than AI-PHD-SLAM method. Secondly, in the estimation of map features, AI-PHD-SLAM method and AI-MBMI-PHD-SLAM method are generally superior to RB-PHD-SLAM method. Thirdly, in the aspect of OSPA, it can be seen that AI-PHD-SLAM method is increased by 34.0% compared to RB-PHD-SLAM method and AI-MBMI-PHD-SLAM method is increased by 29.7% compared to AI-PHD-SLAM method.Table 3
Comparison of data in real environment.
AI-MBMI-PHD-SLAMAI-PHD-SLAMRB-PHD-SLAMRobot position error0.280.370.56The number of features in the map333340The estimated number error of features007OSPA error0.630.9031.37From the above analysis, the following conclusions can be drawn. AI-MBMI-PHD-SLAM method and AI-PHD-SLAM method are more accurately than RB-PHD-SLAM method in the respect of the estimated accuracy of the number of map features. In terms of robot position and attitude estimation and OPSA estimation, the performance of AI-MBMI-PHD-SLAM method is better than that of AI-PHD-SLAM method and is significantly better than that of RB-PHD-SLAM method.


## 5. Conclusion

Taking into account the observations obtained by the robot during motion in the complex environment, in addition to the real map features, the observed information obtained by the robot in motion may contain a large number of false alarms and clutters, leading to the real map features being submerged in clutters. In this paper, firstly, the AI-PHD-SLAM method is proposed, which uses the random finite set method to describe the real environment factors such as the map feature information, the map feature observation information, and the missing probability caused by the sensor uncertainty. Meanwhile, the method uses the amplitude information of map features to estimate the map feature set and to obtain more accurate map features and clutter likelihood function, which improve the estimation accuracy of the feature map in the SLAM process. Secondly, based on AI-PHD-SLAM, a method of adding mixed birth map information is proposed, and an AI-MBMI-PHD-SLAM algorithm is proposed. This method can use the map information that has been detected to increase the PHD of the a priori information, thereby improving the estimation accuracy of the map at the current time and further improving the method accuracy. Finally, simulation experiments and experiments on real-world data are performed on the two methods, and the experimental results are compared and analyzed. It is verified that the method proposed in this paper can effectively improve the estimation accuracy of the map and can greatly reduce the computational complexity and improve the efficiency of the method.



---

*Source: 8420814-2021-03-11.xml*