# An Improved EKF-SLAM for Mars Surface Exploration

Authors: Bo Zheng; Zexu Zhang
Journal: International Journal of Aerospace Engineering
 (2019)

---

## Abstract

In the traditional EKF-SLAM algorithm, the computational complexity and uncertainty will grow up rapidly with the increase of the feature points and the enlargement of the map coverage. As we know, the computational complexity is proportional to the quadratic of the number of feature points contained in a single filtering process. The approach represented in the paper combines EKF-SLAM with local submaps, which can improve the computational efficiency and reduce the computational complexity. At first, an independent local submap is established for the observed feature points. When the number of feature points contained in the local submap reaches a certain threshold value, the local submap is integrated into the global map. At last, the submap is initialized again. The simulation results show that the approach can reduce the computational complexity effectively and increase the computation speed greatly in the case of maintaining the computational accuracy of the traditional EKF-SLAM algorithm.


---

## Body

## 1. Introduction

With the development of the manned deep space exploration, Mars as the most suitable planet for human habitation has attracted extensive attention. The traditional flying-around method can no longer satisfy the deeper exploration mission. Therefore, the cruising exploration method of the Mars rover has become an excellent choice [1]. The relative pose information of the Mars rover can be obtained by traditional odometer during the cruising exploration process, which is not accurate on the complex Mars terrain. The navigation accuracy will be improved through the visual aid navigation method in this circumstance, especially SLAM method [2, 3]. The SLAM problem can be described as a problem that you place a robot in an unknown environment with sensors, it can build the incremental maps and achieve its location autonomously during the movement [4].The initial framework for the navigation of the mobile robot was presented by Dissanayake et al. in the 1980s. Meanwhile, he established the SLAM navigation algorithm for the mobile robot based on Kalman filter [5]. The analysis of the consistency of the EKF-SLAM was presented by Bailey and Durrant-Whyte [6]. The robocentric map joining method was presented by Castellanos et al. for improving the consistency of EKF-SLAM [7]. The bounding uncertainty of the robocentric local map sequencing algorithm was analyzed by Martinez-Cantin and Castellanos [8]. A lot of landmarks need to be observed and introduced into the state equation by EKF-SLAM, which increases the computational complexity and decreases the running speed of the system [9, 6]. Therefore, the traditional EKF-SLAM algorithm needs to be improved. The computational cost analysis of EKF-SLAM was done by time measurement on substep motion update and measurement update on EKF by considering the total number of landmarks and numerous setting on range observation distance by Samsuri et al. [10], which showed the number of landmarks and the range observation distances were the factors to cause computational cost. A computationally efficient SLAM (CESLAM) algorithm was presented by Hsu et al. [11], where odometer information was considered for updating the robot’s pose in particles. When a measurement had a maximum likelihood with the known landmark in the particle, the particle state was updated before updating the land mark estimates. The algorithm aimed to improve the computational efficiency and estimation accuracy of conventional SLAM algorithm.An improved EKF-SLAM with local submaps is presented in the paper. Firstly, the feature points observed by binocular camera are grouped; meanwhile, a few submaps are established, which are independent of the global map. And then only the local state variables are updated by the new algorithm in each update process, because the local submap is integrated into the global map only when the area of the submap reaches a certain scale. The global state variables will not be updated until the local submap is integrated into the global map. The main difference between the standard EKF-SLAM algorithm and the algorithm represented in this paper is the introduction of the local submaps, through which the number of feature points used for each filtering update is reduced. Since the computational complexity is proportional to the quadratic of the number of feature points contained in a single filtering process of EKF and the total computational complexity of EKF-SLAM algorithm is proportional to the cube of the final number of feature points in the state variables, the computational complexity is reduced by the improvement. The paper is organized as follows: Section2 builds the motion model equation of the Mars rover. The EKF-SLAM algorithm is improved based on the local submaps in Section 3. Simulation results and the comparison of the standard EKF-SLAM and the SLAM with submaps are given in Section 4. Finally, the conclusion is drawn in the last section.


## 2. Motion Model of the Mars Rover

The set of the coordinate system of the Mars rover’s motion is given in Figure1.Figure 1
Coordinate system of the Mars rover’s motion.As is shown in Figure1, we denote OG−xGyGzG as the inertial coordinates and global coordinates; OR−xRyRzR as the body coordinates of the Mars rover, where xR points to the direction of the rover’s motion; OC−xCyCzC as the binocular camera coordinates, where xC points to the vertical direction of the camera lens; OAi−xAiyAizAi as the coordinates of the rover’s ith wheel, which is used to establish the trajectory of the rover’s centroid based on the terrain of Mars; and OWi−xWiyWizWi as the coordinates of the contact point for the Mars rover’s ith wheel and the planet surface, where xWi points to the tangential direction of the contact point’s surface, which is used to establish the trajectory of the rover’s centroid based on the terrain of Mars.The installing model of the binocular camera is shown in Figure2 (vertical view) and Figure 3 (lateral view).Figure 2
The installing model of the binocular camera (vertical view).Figure 3
The installing model of the binocular camera (lateral view).The Mars rover’s model used in the paper is a four-wheel differential rover with six degrees of freedom. The linear velocity and the angular velocity are controlled input variables, which is decided by angular velocities of differential wheels [12]. The motion equation is as follows:
(1)ẋ=rwθ̇Lt+θ̇Rt2,ωz=rwBθ̇Lt−θ̇Rt,where θ̇Lt is the left wheel’s velocity, θ̇Rt is the right wheel’s velocity, rw is the wheel’s radius, and B is the distance between the left wheel and the right wheel.The derivative process is omitted; therefore, the kinematic model equation of the Mars rover is given as follows [13]:
(2)ẋRẏRżRϕ̇xϕ̇yϕ̇z=CRG00SωGvω,where CRG is the rotation matrix; SωG is the transformation matrix of the angular velocity; xR,yR,zR,ϕx,ϕy,ϕzT denotes pose vectors of the rover, where ϕz, ϕy, and ϕx stand for the yaw angle, pitch angle, and roll angle; v is the linear velocity of the Mars rover; and ω is the angular velocity of the Mars rover.The coordinates of the contact point can be obtained from the coordinates of the Mars rover’s wheel [14].
(3)XWiAiYWiAiZWiAi1=CWiAiXWiYWiZWi1,i=1,2,3,4,where CWiAi is the transformation matrix from the contact point coordinates to the Mars rover’s coordinates and XWi,YWi,ZWi is the contact point’s coordinate.The binocular camera is fixed on an oblique stick with two degrees of freedom at a certain angle.uL=ωδ,ωϕ are the control inputs of the binocular camera, through which a wide range of 3D terrain can be scanned. ωδ is the rotation angle of the control stick around the platform, and ωϕ is the rotation angle of the control stick around the connecting bolt. ϕL is the angle between the control stick and the vertical direction of the platform. The derivative process is omitted; therefore, the pose of the feature point in global coordinates can be described as follows [15]:
(4)miG=pR+CRGCHRCCHmiC+d,i=1,…,N,where pR=xR,yR,zRT is the position of the rover in global coordinates, d=a,b,c is the position of binocular camera platform in the Mars rover’s coordinates, miC is the position of the feature points in the camera coordinates, miG is the position of the feature points in the global coordinates, CRG is the transformation matrix from the body coordinates to the global coordinates, CHR is the transformation matrix from the platform coordinates to the body coordinates, and CCH is the transformation matrix from the camera coordinates to the platform coordinates.


## 3. An Improved EKF-SLAM with Local Submaps

### 3.1. System State Variables

The system state variables of the local submap algorithm include the Mars rover and the feature points in the local coordinates and the feature points in the global coordinates and the coordinate of the local coordinates in the global coordinates, which can be described as follows:(5)Xlsk=XSGkX1GkX2Gk⋮XRSkX1Sk⋮,where XSGk is the pose vector of the local submap in the global map, XiGk is the position of the feature point in the global map, XRSk is the pose of the Mars rover in the local submap, XiSk is the position of the same feature point in the local submap vector XSGk, and k is the discrete time coefficient. The state variables defined above can be simplified as follows:
(6)Xlsk=XSGkXmGkXRSkXmSk,where XmGk is the feature point in the global map and XmSk is the feature point in the local submap. The integration process from the submap to the global map is shown in Figure 4, where Uk is the control input, Xk is the state variable, and Zk is the observed variable.Figure 4
The flow chart of the integration process from submap to global map.


### 3.2. State Estimation Process

The state covariance matrixes include the covariance matrixes of the global state variables and local state variables and the cross covariance between the global state variables and the local state variables. In this paper, the independence of the global state variables and local state variables is assumed for short intervals, and the short interval independence will be used. The cross covariance between the global state and the local state is zero because of the independence between the global state variable estimation and the local state variables, which is given as follows:(7)Plsk=PSSGkPmSGk00PSmGkPmmGk0000PRRSkPRmSk00PmRSkPmmSk,where PSSGk is the covariance matrix of the local submap pose estimation in the global state and PRRSk is the covariance matrix of the Mars rover pose estimation in the local state.The state predicted valueXlsk∣k−1 is given as follows:
(8)Xlskk−1=XGk−1fXRSk−1,ukXmSk−1.The global estimation is not changed in the local update due to the independence between the local state and the global state, which reduces the computational complexity of the prediction process.(9)Plskk−1=PGk−1000∇fXkPRRSk−1∇fXTk+Qk∇fXkPRmSk−10∇fXkPRmSk−1TPmmSk−1,where Plsk∣k−1 is the predicted value of the covariance matrix of the state variables.The state predicted valuez^k∣k−1 is obtained by the observation model.
(10)z^kk−1=hX^RSk−1,X^iSk−1.hX^RSk−1,X^iSk−1 includes the measure distance, azimuth angle, and rotation angle.The observation residual and residual covariance matrix ofX^ can be described as follows:
(11)rk=zk−z^kk−1,Sk=∇hXkPSkk−1∇hXTk+Qνk.Finally, the state update process is given as follows:(12)X^lsk=X^lskk−1+Wkrk,Plsk=Plskk−1−WkPkWTk,Wk=Plskk−1∇XhTkS−1k.The global state is not updated in the process, because the global state is independent of the local state. The feature points contained in the local state are much less than those contained in the global state. Therefore, the computational complexity is reduced greatly in the update process.(13)Plsk=PGkk−100PSkk−1−WkSkWTk,Wk=PSkk−1∇hXTkS−1k.


### 3.3. The Independence of Local State Estimation

The local submap established above is composed of the Mars rover’s pose state in the local submap and the local submap state. The local submap achieves its initialization by initializing the location of the rover. We hypothesize the initial state matrix is given as follows:(14)X^llk−1=X^RGk−1X^mGk−1,where X^RG is the estimated value of Mars rover’s pose in the global map and X^mG is the estimated value of the feature point position in the global map.The state covariance matrix is shown as follows:(15)Pllk=PRRGkPRmGkPmRGkPmmGk,where PRRGk is the covariance matrix of the Mars rover’s pose estimation in the global state, PmmGk is the covariance matrix of the feature point position estimation in the global state, and PRmGk is the covariance matrix of the Mars rover and the feature point in the global state.A local submap is established at timek−1, which takes the Mars rover’s position as the center of the submap. Therefore, the Mars rover is located at the origin in the submap whose uncertainty is zero. We hypothesis that the initial state matrix X^llk−1 is shown as follows:
(16)X^llk−1=X^SGk−1X^mGk−1X^vSk−1,Pllk=PSSGkPSmGk0PmSGkPmmGk0000,where PSSGk is the covariance matrix of the local submap pose estimation in the global state and PmmGk is the covariance matrix of the feature point position estimation in the global state.The noise variance is generated by the local Mars rover’s state variables under the modeling error and the observation error during the update process in the local submap. However, this process is independent of global state, which will not affect the global state and covariance matrix. The feature points observed in the new local submap are independent of the global map. Meanwhile, if an independent Mars rover’s state estimation is initialized in the local submap, the estimation will maintain its independence to the global map. Therefore, the local state estimation and the global state estimation remain independence.


### 3.4. The Integration between the Submap and the Global Map

The submap is needed for integration into the global map when it reaches a certain scale. Then, the submap is rebuilt and initialized again. We hypothesize thatTk is the transformation matrix, which can convert the local state to a global state.
(17)X^lsGk=TSGkX^lsk,PclGk=∇TSGkPclk∇TSGkT,X^lsGk=TkX^SGkX^mGkX^RSkX^mSk=X^SGkX^mGkX^SGk⊕X^RSkX^SGk⊕X^mSk,where ⊕ denotes the state summation operation in different coordinates including the angular rotation process between different coordinates. We hypothesize that X^iSk is the position of a feature point in the local submap.
(18)X^iSk=xiSkyiSkziSk.Likewise, we hypothesize thatX^SGk is the coordinate of the local coordinates in the global coordinates.
(19)X^SGk=xSGkySGkzSGkϕSxGϕSyGϕSzG.The global position of the feature point can be obtained as follows:(20)X^lsGk=X^SGkX^iGk=TSGkX^SGkX^iSk,where TSGk can be described as follows:
(21)where CSGk=CRGϕSxG,ϕSyG,ϕSzG which is the transformation matrix from the local coordinates to the global coordinates. The covariance matrix of the global state variables can be expressed as follows:
(22)PlsGk=∇TSGkPSSGkPiiSk∇TSGkT,where ∇TSGk is the Jacobian matrix of the transformation matrix, which is shown as follows:
(23)∇TSGk=∂TSGk∂X^SGk,X^iSk.We hypothesize that the local pose of the rover isxRSk,yRSk,zRSk,ϕxSϕySϕzST and the global pose of the rover is given as follows:
(24)where SSGk=SωGϕSxG,ϕSyG,ϕSzG is the pose transformation matrix. Therefore, the covariance matrix of the Mars rover in the global coordinates can be described as follows:
(25)PRRGk=∇RSGkPSSGkPRRSk∇RSGkT,where ∇RSGk is
(26)∇RSGk=∂RSG∂X^SGk,X^RSk.


### 3.5. The Constraints for Local Feature Point Estimation

The local pose estimation of the Mars rover is independent of the global coordinates in the local submap algorithm. The estimation of the feature points in the global coordinates cannot be applied to the update process for the local submap without the connection between the coordinates. The feature points in the global coordinates and the local coordinates are independent, which results in repeated calculations when the feature points in the local coordinates are integrated into the global coordinates. The estimation of the feature points must be able to apply in the global estimation process.In the general process, the consistent estimation of the feature points state can be obtained by the known constraints between the common state. The independent estimation of the feature points can be integrated into the global coordinate; meanwhile, a simple and consistent global estimation is generated by using the known connection between the state variables.The projection is defined as follows:(27)CX^k=b.The constraint can be approximated estimation by EKF. The result can be described as follows:(28)X^ck=X^k+Wckb−CkX^k,Pck=Pk+WckSckWcTk,where
(29)Wck=Pk∇CkSc−1k,Sck=∇CkPk∇CTk.The system state variables are shown above, then the constraints must satisfy the condition, which is the constrains for local feature points integrating into the global map:(30)XiGk−XSGk⊕XiSk=0.The constraints can be expressed as follows:(31)CkXSGkXiGkXRSkXiSk=0,where
(32)Ck=−II0−I,b=0, and the constraint filtering can be applied to the conversion process to the global map for the local feature points.


## 3.1. System State Variables

The system state variables of the local submap algorithm include the Mars rover and the feature points in the local coordinates and the feature points in the global coordinates and the coordinate of the local coordinates in the global coordinates, which can be described as follows:(5)Xlsk=XSGkX1GkX2Gk⋮XRSkX1Sk⋮,where XSGk is the pose vector of the local submap in the global map, XiGk is the position of the feature point in the global map, XRSk is the pose of the Mars rover in the local submap, XiSk is the position of the same feature point in the local submap vector XSGk, and k is the discrete time coefficient. The state variables defined above can be simplified as follows:
(6)Xlsk=XSGkXmGkXRSkXmSk,where XmGk is the feature point in the global map and XmSk is the feature point in the local submap. The integration process from the submap to the global map is shown in Figure 4, where Uk is the control input, Xk is the state variable, and Zk is the observed variable.Figure 4
The flow chart of the integration process from submap to global map.


## 3.2. State Estimation Process

The state covariance matrixes include the covariance matrixes of the global state variables and local state variables and the cross covariance between the global state variables and the local state variables. In this paper, the independence of the global state variables and local state variables is assumed for short intervals, and the short interval independence will be used. The cross covariance between the global state and the local state is zero because of the independence between the global state variable estimation and the local state variables, which is given as follows:(7)Plsk=PSSGkPmSGk00PSmGkPmmGk0000PRRSkPRmSk00PmRSkPmmSk,where PSSGk is the covariance matrix of the local submap pose estimation in the global state and PRRSk is the covariance matrix of the Mars rover pose estimation in the local state.The state predicted valueXlsk∣k−1 is given as follows:
(8)Xlskk−1=XGk−1fXRSk−1,ukXmSk−1.The global estimation is not changed in the local update due to the independence between the local state and the global state, which reduces the computational complexity of the prediction process.(9)Plskk−1=PGk−1000∇fXkPRRSk−1∇fXTk+Qk∇fXkPRmSk−10∇fXkPRmSk−1TPmmSk−1,where Plsk∣k−1 is the predicted value of the covariance matrix of the state variables.The state predicted valuez^k∣k−1 is obtained by the observation model.
(10)z^kk−1=hX^RSk−1,X^iSk−1.hX^RSk−1,X^iSk−1 includes the measure distance, azimuth angle, and rotation angle.The observation residual and residual covariance matrix ofX^ can be described as follows:
(11)rk=zk−z^kk−1,Sk=∇hXkPSkk−1∇hXTk+Qνk.Finally, the state update process is given as follows:(12)X^lsk=X^lskk−1+Wkrk,Plsk=Plskk−1−WkPkWTk,Wk=Plskk−1∇XhTkS−1k.The global state is not updated in the process, because the global state is independent of the local state. The feature points contained in the local state are much less than those contained in the global state. Therefore, the computational complexity is reduced greatly in the update process.(13)Plsk=PGkk−100PSkk−1−WkSkWTk,Wk=PSkk−1∇hXTkS−1k.


## 3.3. The Independence of Local State Estimation

The local submap established above is composed of the Mars rover’s pose state in the local submap and the local submap state. The local submap achieves its initialization by initializing the location of the rover. We hypothesize the initial state matrix is given as follows:(14)X^llk−1=X^RGk−1X^mGk−1,where X^RG is the estimated value of Mars rover’s pose in the global map and X^mG is the estimated value of the feature point position in the global map.The state covariance matrix is shown as follows:(15)Pllk=PRRGkPRmGkPmRGkPmmGk,where PRRGk is the covariance matrix of the Mars rover’s pose estimation in the global state, PmmGk is the covariance matrix of the feature point position estimation in the global state, and PRmGk is the covariance matrix of the Mars rover and the feature point in the global state.A local submap is established at timek−1, which takes the Mars rover’s position as the center of the submap. Therefore, the Mars rover is located at the origin in the submap whose uncertainty is zero. We hypothesis that the initial state matrix X^llk−1 is shown as follows:
(16)X^llk−1=X^SGk−1X^mGk−1X^vSk−1,Pllk=PSSGkPSmGk0PmSGkPmmGk0000,where PSSGk is the covariance matrix of the local submap pose estimation in the global state and PmmGk is the covariance matrix of the feature point position estimation in the global state.The noise variance is generated by the local Mars rover’s state variables under the modeling error and the observation error during the update process in the local submap. However, this process is independent of global state, which will not affect the global state and covariance matrix. The feature points observed in the new local submap are independent of the global map. Meanwhile, if an independent Mars rover’s state estimation is initialized in the local submap, the estimation will maintain its independence to the global map. Therefore, the local state estimation and the global state estimation remain independence.


## 3.4. The Integration between the Submap and the Global Map

The submap is needed for integration into the global map when it reaches a certain scale. Then, the submap is rebuilt and initialized again. We hypothesize thatTk is the transformation matrix, which can convert the local state to a global state.
(17)X^lsGk=TSGkX^lsk,PclGk=∇TSGkPclk∇TSGkT,X^lsGk=TkX^SGkX^mGkX^RSkX^mSk=X^SGkX^mGkX^SGk⊕X^RSkX^SGk⊕X^mSk,where ⊕ denotes the state summation operation in different coordinates including the angular rotation process between different coordinates. We hypothesize that X^iSk is the position of a feature point in the local submap.
(18)X^iSk=xiSkyiSkziSk.Likewise, we hypothesize thatX^SGk is the coordinate of the local coordinates in the global coordinates.
(19)X^SGk=xSGkySGkzSGkϕSxGϕSyGϕSzG.The global position of the feature point can be obtained as follows:(20)X^lsGk=X^SGkX^iGk=TSGkX^SGkX^iSk,where TSGk can be described as follows:
(21)where CSGk=CRGϕSxG,ϕSyG,ϕSzG which is the transformation matrix from the local coordinates to the global coordinates. The covariance matrix of the global state variables can be expressed as follows:
(22)PlsGk=∇TSGkPSSGkPiiSk∇TSGkT,where ∇TSGk is the Jacobian matrix of the transformation matrix, which is shown as follows:
(23)∇TSGk=∂TSGk∂X^SGk,X^iSk.We hypothesize that the local pose of the rover isxRSk,yRSk,zRSk,ϕxSϕySϕzST and the global pose of the rover is given as follows:
(24)where SSGk=SωGϕSxG,ϕSyG,ϕSzG is the pose transformation matrix. Therefore, the covariance matrix of the Mars rover in the global coordinates can be described as follows:
(25)PRRGk=∇RSGkPSSGkPRRSk∇RSGkT,where ∇RSGk is
(26)∇RSGk=∂RSG∂X^SGk,X^RSk.


## 3.5. The Constraints for Local Feature Point Estimation

The local pose estimation of the Mars rover is independent of the global coordinates in the local submap algorithm. The estimation of the feature points in the global coordinates cannot be applied to the update process for the local submap without the connection between the coordinates. The feature points in the global coordinates and the local coordinates are independent, which results in repeated calculations when the feature points in the local coordinates are integrated into the global coordinates. The estimation of the feature points must be able to apply in the global estimation process.In the general process, the consistent estimation of the feature points state can be obtained by the known constraints between the common state. The independent estimation of the feature points can be integrated into the global coordinate; meanwhile, a simple and consistent global estimation is generated by using the known connection between the state variables.The projection is defined as follows:(27)CX^k=b.The constraint can be approximated estimation by EKF. The result can be described as follows:(28)X^ck=X^k+Wckb−CkX^k,Pck=Pk+WckSckWcTk,where
(29)Wck=Pk∇CkSc−1k,Sck=∇CkPk∇CTk.The system state variables are shown above, then the constraints must satisfy the condition, which is the constrains for local feature points integrating into the global map:(30)XiGk−XSGk⊕XiSk=0.The constraints can be expressed as follows:(31)CkXSGkXiGkXRSkXiSk=0,where
(32)Ck=−II0−I,b=0, and the constraint filtering can be applied to the conversion process to the global map for the local feature points.


## 4. Simulation Results

The simulation results are analyzed in this section. We can obtain the Mars terrain as the method mentioned in the paper [16]. We hypothesize that the real trajectory of the Mars rover is a circle. The 3D terrain and Mars rover’s trajectory are shown in Figure 5. The following simulation results are mainly divided into two parts. The results shown from Figures 6–8 are simulated by standard EKF-SLAM algorithm; meanwhile, the results shown from Figures 9–12 are simulated by the new algorithm which is presented in this paper. The total number of the feature points considered in the state variables of EKF-SLAM and the new algorithm are compared in Figure 13. Since the computational complexity is proportional to the quadratic of the number of the feature points contained in a single filtering process, the main difference between the two algorithms can be shown in Figure 13. Obviously, the number of the feature points used in the new algorithm is much less than which are used in standard EKF-SLAM algorithm. The lower computational complexity is obtained by the SLAM algorithm with local submaps (Table 1).Figure 5
3D terrain and Mars rover’s trajectory 200 landmarks (asterisks) are distributed in the map. The simulation parameters are shown in Table1.Figure 6
The true trajectory and estimated trajectory of the Mars rover by standard EKF-SLAM.Figure 7
The position error curve of the Mars rover.Figure 8
The attitude error of the Mars rover.Figure 9
The true trajectory and estimated trajectory of the Mars rover by EKF-SLAM with submaps.Figure 10
The pose error curve of the Mars rover.Figure 11
The attitude error of the Mars rover.Figure 12
The standard deviation curve of the local position and azimuth deviation of the Mars rover.Figure 13
The total number of the feature points considered in the state variables of EKF-SLAM and EKF-SLAM with local submaps.Table 1
The simulation parameters.
Name
Value
Sampling period
0.2 s
The number of feature points in the map
200
The radius of the wheel
0.3 m
The initial position of the rover
100,20,−0.3174
The SD of RN of the accelerometer
2.0×10−3m/s
The SD of RN of the gyroscope
0.02°/s
The SD of RN of the measuring length
1.0 m
The SD of RN of the measuring azimuth
3°
The SD of RN of the control stick
3°
Sensor frequency of the IMU
20 Hz
Sensor frequency of the camera
0.5 Hz
Simulation time
400 s
Note: SD denotes standard deviation; RN denotes random noise.The true trajectory and estimated trajectory of the Mars rover by standard EKF-SLAM are shown in Figure6. The pink solid line represents the true trajectory and the blue dashed line represents the estimated trajectory of the Mars rover. Meanwhile, the green asterisks represent land marks. The red asterisks represent the observed feature points. The position error is less than 1 m, and the attitude error is less than 0.5°, which can be seen from Figures 7 and 8.In the following section, an improved EKF-SLAM approach with submaps is analyzed. The true trajectory and estimated trajectory of the Mars rover by improved EKF-SLAM algorithm with submaps are shown in Figure9. The position error and attitude error can be seen in Figures 10 and 11 (1 m, 0.5°), which keep the same accuracy and error as the previous standard EKF-SLAM algorithm. However, the total number of the feature points considered in the state variables of the improved algorithm with submaps is much smaller than that of the standard EKF-SLAM algorithm. As we know, the computational complexity is proportional to the quadratic of the number of the feature points contained in a single filtering process. Therefore, the computational complexity of the improved EKF-SLAM algorithm with submaps has decreased significantly compared with the standard EKF-SLAM algorithm. The standard deviation curve of the local position and azimuth deviation of the Mars rover is given in Figure 12. The total number of the feature points considered in the state variables of the standard EKF-SLAM and SLAM with submaps are shown in Figure 13. The computational complexity is proportional to the square of the number of the feature points contained in the state variables, and the total computational complexity of EKF-SLAM algorithm is proportional to the cube of the final number of feature points in the state variables. Therefore, the computational complexity is reduced sharply in SLAM with submaps. Some comparison results of two algorithms are listed in Table 2, from which we can see the difference of the two algorithms clearly.Table 2
Some comparison results of the two algorithms.
Standard EKF-SLAM
EKF-SLAM with submaps
The total number of the feature points considered in the state variables
700
440
Computational complexity
∝7003
∝4403
The triaxial position error of Mars rover
≤1 m
≤0.5 m
The triaxial angle error of Mars rover
≤0.5°
≤0.5°


## 5. Conclusion

In the paper, we proposed an improved EKF-SLAM algorithm with submaps for Mars surface exploration. The presented method could reduce the computational complexity to a great extent. The error of the position and attitude could be controlled in an acceptable ranges (1 m, 0.5°) for both two algorithms, which can be seen in the simulation results. The position error of the EKF-SLAM with submaps is smaller than that of the standard EKF-SLAM. Meanwhile, the computational complexity is lower by the improved algorithm, which is more suitable for the complex terrain of Mars.



---

*Source: 7637469-2019-02-04.xml*