# FastSLAM Using Compressed Occupancy Grids

Authors: Christopher Cain; Alexander Leonessa
Journal: Journal of Sensors
 (2016)

---

## Abstract

Robotic vehicles working in unknown environments require the ability to determine their location while learning about obstacles located around them. In this paper a method of solving the SLAM problem that makes use of compressed occupancy grids is presented. The presented approach is an extension of the FastSLAM algorithm which stores a compressed form of the occupancy grid to reduce the amount of memory required to store the set of occupancy grids maintained by the particle filter. The performance of the algorithm is presented using experimental results obtained using a small inexpensive ground vehicle equipped with LiDAR, compass, and downward facing camera that provides the vehicle with visual odometry measurements. The presented results demonstrate that although with our approach the occupancy grid maintained by each particle uses only40
% of the data needed to store the uncompressed occupancy grid, we can still achieve almost identical results to the approach where each particle filter stores the full occupancy grid.


---

## Body

## 1. Introduction

One of the most important problems in the development of an unmanned vehicle (UMV) is giving it the ability to be placed in a completely unknown environment and allowing it to determine (1) its surroundings, based on what it “sees” using available sensors, and (2) its position, based on how far it has moved and what it sees. In the field of robotics this problem is referred to as Simultaneous Localization and Mapping (SLAM) or Concurrent Mapping and Localization (CML). In this paper we develop a method of solving the SLAM problem which implements a representation of the world using a compressed occupancy grid [1].The approach we develop is an extension of the FastSLAM algorithm [2] which is one of the first solutions to the SLAM problem that makes use of a particle filter. More specifically the FastSLAM algorithm uses a Rao-Blackwellized [3, 4] particle filter to generate a position estimate of the UMV while each particle in filter maintains its copy of the occupancy grid. Based on the expected size of the environment in which a UMV operates and the level of detail required, the size of each occupancy grid can become extremely large (matrices holding hundreds of thousands of entries). Furthermore, as discussed further in Section 3.1.2, the probability that the estimate produced by the FastSLAM algorithm is accurate increases as the number of particles in the particle filter increases. For these reasons the amount of computer resources required to perform the algorithm can grow quickly. This problem is exacerbated in small form factor and low cost UMVs as the computing resources available to vehicles of this type are much less than the resources available to larger and more expensive vehicles. The approach that we develop extends the FastSLAM algorithm [2] to use compressed occupancy grids to overcome the memory restrictions that can occur when storing full occupancy grids without making drastic changes to the original algorithm.The remainder of this paper is organized as follows. The mathematical notations used in this paper along with some mathematical preliminaries are presented in Section2. The updated FastSLAM algorithm that makes use of compressed occupancy grids is introduced in Section 3. In Section 4 four separate approaches for reconstructing the compressed occupancy grid are examined. Section 5 presents a set of performance improvements to some components of the algorithm. In Section 6 experimental results are provided and concluding remarks are presented in Section 7.


## 2. Mathematical Preliminaries

In much of the existing literature, the SLAM problem is addressed in a probabilistic sense. In many cases we would like to estimate the probability, also referred to as the distribution, of some random variablex
∈
R and we denote the distribution of x as p
(
x
). In many cases we use some additional information y
∈
R to tell us something about the random variable x. In this situation p
(
x
) is referred to as theprior probability and it is all that we know about the probability of x without the inclusion of y, in many cases this is shortened toprior. The distribution of x with the inclusion of the data y is denoted as p
(
x
∣
y
) and referred to as theposterior probability and in many cases is just referred to as theposterior.In SLAM we are attempting to estimate the poseξ
k of a UMV and a map M
k of the environment that surrounds the UMV at some time step k
>
0. In this paper we treat the world in which the UMV operates as a two-dimensional plane; thus ξ
k
≜
x
k
y
k
θ
k
T, where x
k
y
k
∈
R
2 are the horizontal and vertical positions of the vehicle in some frame of reference and θ
k
∈
(
-
π
,
π
] is the heading of the UMV with respect to the positive horizontal axis in the frame of reference. From our use of occupancy grids, the estimated map is represented by a matrix M
k
∈
R
r
×
c, where r
∈
N are the number of rows in the grid and c
∈
N are the number of columns. We assume that the environment that the UMV operates is static; therefore during the time in which the UMV is performing the SLAM algorithm the environment does not change. Based on this assumption, to simplify the notation, the occupancy grid is denoted with M. The estimate produced by SLAM, in many cases, makes use of sensor measurements and control inputs. The set of sensor measurements z
1
:
k denotes the full set of sensor measurements for k
>
0; thus z
1
:
k
≜
[
z
1
,
…
,
z
k
], where z
i
∈
R
m, i
=
1,2
,
…
,
k, and m
∈
N is the number of measurements in z
i. In the same way the full set of control inputs are defined as u
1
:
k
≜
[
u
1
,
…
,
u
k
], where u
i
∈
R
c, i
=
1,2
,
…
,
k, and c
∈
N is the size of the control vector u
i. With the notation defined and the required mathematical preliminaries provided, the FastSLAM algorithm that makes use of compressed occupancy grids is presented in the following section.


## 3. FastSLAM with Compressed Occupancy Grids

In general the goal of SLAM is to estimate the poseξ
k of a UMV and the map, M, of the unknown environment in which the UMV is operating at some time step k
>
0. We present an extension of the FastSLAM algorithm that uses compressed occupancy grids to store the map. Before presenting our approach, an overview of the FastSLAM algorithm that uses standard occupancy grids (FastSLAM OG) is provided.


### 3.1. FastSLAM with Occupancy Grids

As opposed to many SLAM solutions that attempt to solve the online SLAM problem that is estimating the posterior that represents the pose of a UMV and the map of the environment based on a set of control inputs and sensor measurements, the FastSLAM approach attempts to estimate the full SLAM posterior. The full SLAM posterior is given as(1)
p
ξ
1
:
k
,
M
∣
u
1
:
k
,
z
1
:
k
,which is the distribution that represents the trajectory of UMV ξ
1
:
k and the map of the environment M based on the set of control inputs u
1
:
k and observations z
1
:
k. By estimating the full SLAM posterior, (1) can be factored, using the property of conditional independence, into a pair of simpler terms which is given by(2)
p
ξ
1
:
k
,
M
∣
u
1
:
k
,
z
1
:
k
=
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
×
p
M
∣
ξ
1
:
k
,
u
1
:
k
,
z
1
:
k
,where p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k is the distribution that represents the UMV trajectory and p
M
∣
ξ
1
:
k
,
u
1
:
k
,
z
1
:
k is the distribution that represents the map of the environment. The FastSLAM approach estimates the factored SLAM posterior (2) using a Rao-Blackwellized particle filter. The Rao-Blackwellized particle filter is a version of a sampling importance resampling (SIR) [5] particle filter and it is the SIR particle filter that provides the format of the FastSLAM solution. The Rao-Blackwellized particle filter estimates the distribution representing a portion of the state, which in FastSLAM is the UMV trajectory ξ
1
:
k, using a particle filter, and each particle in the particle filter maintains its own estimate of the remainder of the state, which is the map M. As a result, each particle in the particle filter is composed of a UMV pose and a map, which the FastSLAM OG algorithm represents using an occupancy grid; thus the pth particle at time k is defined as(3)
X
k
p
≜
ξ
k
p
M
k
p
,where ξ
k
[
p
] is the pose estimate of the pth particle at time k, M
k
[
p
] is the occupancy grid of the environment maintained by the particle as it has been generated through time step k, and p
∈
1
,
n is the index corresponding to the pth particle where n is the total number of particles used in the particle filter. The FastSLAM approach is implemented in a four-step procedure based on the use of a SIR particle filter to estimate (1) and an overview of the algorithm is presented in Algorithm 1 for time k.Algorithm 1:FastSLAM OG algorithm.
(
1) procedure  Fa
stSLAMOG(X
k
-
1
,
u
k
,
z
k)
(2)  X
-
k
←
(3)  for  p
←
1, n  do
(4)    ξ
k
[
p
]
~
p
(
ξ
k
∣
ξ
k
-
1
p
,
u
k
)
(5)    w
[
p
]
←
η
p
(
z
k
∣
ξ
k
p
,
M
k
-
1
p
)
(6)    M
k
[
p
] = OGUpdate
(
ξ
k
p
,
M
k
-
1
p
,
z
k
)
(7)    X
-
k
[
p
]
←
ξ
k
p
M
k
p
w
[
p
]
(8)  end for
(9)  X
k
←
(10)  for  p
=
1, n  do
(11)    select j with probability proportional to w
[
p
]
(12)    X
k
[
p
]
←
X
-
k
[
j
]
(13)  end for
(14) end procedure


#### 3.1.1. Motion Model Update

In the first step of the algorithm (Line (4) in Algorithm 1) a new particle pose is generated and added to a set of temporary particles, X
¯
k. The new pose for the pth particle is generated by sampling from the probabilistic motion model of the UMV:(4)
ξ
k
p
~
p
ξ
k
∣
ξ
k
-
1
p
,
u
k
.The probabilistic motion model of the UMV generates a new pose based on the state transition model of the UMV, the pose of the pth particle at time step k
-
1, the current control input u
k, and the characteristics of the noise on u
k. The result is that the particles in X
¯
k are distributed according to(5)
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
-
1
,which is referred to as theproposal distribution. The actual distribution which we are attempting to estimate is referred to as thetarget distribution and given as(6)
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
.The proposal distribution is transformed to the target distribution in the last stage of the algorithm, the resampling stage (Lines (10)–(13) in in Algorithm 1). Before the resampling process can occur, z
k must be incorporated into the estimate, which is achieved in the calculation of the particle’s importance weight.


#### 3.1.2. Importance Weight Calculation

In the second step of the algorithm an importance weight for each particle is generated. The need for an importance weight for each particle comes from the use of a SIR particle filter and, as seen in the previous section, we are sampling particles from a distribution different from the one that we are attempting to estimate. According to [6], if we are unable to directly sample from the distribution that we are attempting to estimate, then the importance weight for each particle is given as(7)
w
k
p
=
t
a
r
g
e
t
d
i
s
t
r
i
b
u
t
i
o
n
p
r
o
p
o
s
a
l
d
i
s
t
r
i
b
u
t
i
o
n
, and particles are drawn from X
¯
k with replacement and added to X
k with a probability proportional to w
k
[
p
]. When drawing particles from X
¯
k with replacement, each particle that is added to X
k remains in X
¯
k so that a given particle that is selected has the potential of being added to X
k multiple times. After the resampling process X
k will approximate the target distribution and the quality of the approximation will improve, the number of particles increases.Using (7) along with the previously discussed distributions (5) and (6), the importance weight of each particle simplifies to [7](8)
w
k
p
=
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
-
1
=
η
p
z
k
∣
ξ
k
p
,
M
k
-
1
p
,where p
z
k
∣
ξ
k
p
,
M
k
-
1
[
p
] is the probabilistic measurement model of the sensor being used by the UMV and η
>
0 is a normalizing factor. The probabilistic measurement model is dependent on the perceptual sensor being used by the UMV and it calculates the probability that the sensor measurement would make based on the current pose estimate and map maintained by the particle. This form of importance weight incorporates the sensor observations into the estimate which were not included in the pose sampling procedure. Once the importance weight has been calculated for each particle then the occupancy grid maintained by each particle is updated using the new particle pose and current sensor measurement.


#### 3.1.3. Occupancy Grid Update

Once the importance weight for each particle has been calculated, the third step of the algorithm updates the occupancy grid of each particle using the current sensor measurement and pose estimate. Occupancy grids decompose an unknown environment into a finite set of cells where each cell holds the probability that the location in the world enclosed by the cell is occupied by an object. From [8], the occupancy grid for a given particle at time k is obtained in a probabilistic sense by calculating the posterior over maps based on the history of sensor measurements and trajectory of a particle:(9)
p
M
k
p
∣
ξ
1
:
k
p
,
z
1
:
k
.By decomposing the environment into cells, the posterior calculated by the occupancy grid can be restated as a product of cellular posteriors:(10)
p
M
k
p
∣
ξ
1
:
k
p
,
z
1
:
k
=
∏
i
,
j
p
M
k
p
i
,
j
∣
ξ
1
:
k
p
,
z
1
:
k
,where p
M
k
[
p
]
i
,
j
∣
ξ
1
:
k
[
p
]
,
z
1
:
k contains the probability that the location in the environment represented by the cell at i
,
j is occupied based on the measurement history and particle trajectory. A solution to a problem of this form is the binary Bayes filter [8]. The binary Bayes filter stores the probability of occupancy for each cell in log odds form to prevent truncation errors that can occur during the update process. The log odds form of the variable x
∈
R is given as(11)
l
x
≜
log
⁡
x
1
-
x
,and the value of x can be recovered from the log odds form as(12)
x
≜
1
-
1
1
+
exp
⁡
l
x
.The binary Bayes filter based occupancy grid update modifies the probability of occupancy of each cell in the grid when an observation provides information about that location. As a consequence, for locations that have been observed, the probability of occupancy quickly approaches 0 for free cells and 1 for occupied cells. While the addition of new information to the estimate makes the probability quickly approach the limits, the probability of occupancy of each cell should never equal 0 or 1 as this would assume perfect knowledge about the occupancy of the cell. Depending on the data type selected to store the probability, the precision required to store the value can quickly be exceeded resulting in the probability being rounded to either 0 or 1. Once this rounding has occurred any new information learned about the occupancy of a cell is lost. To overcome this problem the binary Bayes filter stores the probability of occupancy in log odds form which takes the potential probability values in 0,1 and spreads them over the range -
∞
,
∞. By spreading the small range of probability values over this much larger range, the data type being used to store the probability has more precision to store the probability thus preventing the truncation/rounding from occurring. The occupancy grid update algorithm based on the binary Bayes filter is provided in Algorithm 2.Algorithm 2:Standard occupancy grid update algorithm.
(
1) procedure OGUp
date(ξ
k
,
M
k
-
1
,
z
k)
(2)  for  i
=
1, h  do
(3)    for  j
=
1, w  do
(4)      for  l
=
1, m  do
(5)        if  M
k
-
1
(
i
,
j
) lies in the perceptual range of z
k
(
l
)  then
(6)          M
k
i
,
j
⟵
M
k
-
1
i
,
j
+
log
⁡
p
M
i
,
j
∣
ξ
k
,
z
k
l
1
-
p
M
i
,
j
∣
ξ
k
,
z
k
l
-
l
0
(7)        end if
(8)      end for
(9)    end for
(10)  end for
(11) end procedureIn Algorithm2, p
M
i
,
j
∣
ξ
k
,
z
k
(
l
) is the inverse sensor model of the sensor being used to generate the map and it returns the probability that the cell at i
,
j is occupied based on the lth sensor measurement and the pose estimate of the particle. The value of l
0 in Algorithm 2 is referred to as the prior of occupancy and is defined as(13)
l
0
≜
l
o
g
p
M
i
,
j
1
-
p
M
i
,
j and gives the probability that a cell is occupied before any sensor measurements are integrated into the map. Typically p
M
i
,
j in (13) is selected to be 0.5.


#### 3.1.4. Resampling

After the occupancy grid for each particle inX
¯
k has been updated, the final step of the algorithm is the resampling process. During the resampling process particles are drawn with replacement from X
¯
k with a probability proportional to w
[
p
]. This resampling step takes the set of temporary particles that are distributed according to (5) and ensures that there is a high probability that X
k is distributed according to (6).One of the problems with this approach is the large amount of memory required to run the algorithm. The estimate generated by the algorithm improves as the number of particles increases; thus we like to have as many particles as possible. However, each particle must keep a copy of the occupancy grid and each occupancy grid can be quite large. It is easy to see that while we would like to run the particle filter with as many particles as possible we can be limited by the fact that each particle must maintain a full copy of the occupancy grid. To address this problem we propose an updated version of the algorithm that decreases the amount of memory required for each particle by storing a compressed version of the occupancy grid as opposed to the full uncompressed occupancy grid.


### 3.2. FastSLAM with Compressed Occupancy Grids

Before discussing the updated SLAM algorithm, we will first address the compressed form of the occupancy grid. To compressM
k
[
p
] we first reshape the occupancy grid so that r
×
c matrix becomes a column vector containing n
=
r
c elements and we refer to the vector form of the occupancy grid as m
k
[
p
]
≜
v
e
c
(
M
k
[
p
]
). The reshaping process is performed in such a way that the cell in the original occupancy grid located at i
,
j can be accessed in the vector form at l, where l
=
i
c
+
j. In order to compress the occupancy grid without major degradation, we assume that the occupancy grid can be represented in an alternate basis in which it is sparse. The relationship between the two different representations is given as(14)
m
k
p
=
Φ
c
k
p
,where Φ is a transformation matrix that defines the relationship and c
k
[
p
] is the set of coefficients that represent the occupancy grid in the alternate basis. In order for the occupancy grid to be compressible, c
k
[
p
] must be sparse [9]; that is #
{
s
u
p
p
c
k
[
p
]
}
<
n, where s
u
p
p
(
·
) is the support operator and # denotes the cardinality of the set. This means that, in order for us to be able to compress the occupancy grid, we must find a basis in which the coefficient vector is composed of a large number of zeros. If this condition is met then the occupancy grid for a given particle can be compressed using(15)
y
k
p
=
H
c
k
p
,where H is a matrix that performs the compression by selecting a subset of coefficient data that is smaller than the amount of data in the complete signal.The FastSLAM COG algorithm is a modification of Algorithm1. Since we are now maintaining the compressed form of the occupancy grid, ith particle in the particle set is defined as(16)
X
k
p
≜
ξ
k
p
y
k
p
.As with FastSLAM OG, FastSLAM COG is performed in several steps and an overview of the updated algorithm is presented in Algorithm 3. As a result of using compressed occupancy grids the only changes that must be addressed are the steps of the algorithm that require the occupancy grid, the importance weight calculation, and occupancy grid update.Algorithm 3:FastSLAM algorithm with compressed occupancy grids.
(1) procedure Fa
stSLAMCOG(X
k
-
1
,
u
k
,
z
k)
(2)   X
-
k
←
(3)   for  p
←
1, n  do
(4)     ξ
k
[
p
]
~
p
(
ξ
k
∣
ξ
k
-
1
p
,
u
k
)
(5)     c
^
k
[
p
] = uncompress(y
k
-
1
p)
(6)     w
[
p
]
←
η
p
(
z
k
∣
ξ
k
p
,
Φ
c
^
k
[
p
]
)
(7)     α
k
[
p
] = AlphaConstruction(m
k
-
1
[
p
]
,
z
k)
(8)     y
k
[
p
]
=
H
[
c
^
k
p
+
Φ
T
[
α
k
p
-
β
]
]
(9)     X
-
k
[
p
]
←
ξ
k
[
p
]
y
k
[
p
]
w
[
p
]
(10)   end for
(11)   X
k
←
(12)   for  j
=
1, n  do
(13)     sample with replacement from select i with probability proportional to w
[
p
]
(14)     X
k
[
p
]
←
X
-
k
[
p
]
(15)   end for
(16) end procedure


#### 3.2.1. Importance Weight Calculation

As discussed in Section3.1.2, the importance weight for each particle is the normalized probabilistic measurement model for each particle. The probabilistic measurement model is a measure of how well the current set of sensor measurements matches up with what we expect based on the current pose and map of the particle. In calculating the probabilistic measurement model, the algorithm must have access to the raw occupancy grid values so that an expected sensor measurement can be generated. Based on this requirement, the compressed occupancy grid coefficients are first uncompressed. For now a general reconstruction operator denoted as H
r is used; however the details of several reconstruction methods are explored in detail in Section 4. Once the compressed coefficients have been reconstructed, the occupancy grid can be extracted using (14). The importance weight calculation can be found in Algorithm 3 on Line (6).


#### 3.2.2. Compressed Occupancy Grid Update

As previously seen in Algorithm2, the standard approach to updating the occupancy grid is to examine every cell in the occupancy grid and if a given cell falls within the perceptual range of the sensor attached to the UMV then the corresponding cell of the occupancy grid is updated according to(17)
M
k
p
i
,
j
=
M
k
-
1
p
i
,
j
+
l
o
g
p
M
i
,
j
∣
ξ
k
p
,
z
k
l
1
-
p
M
i
,
j
∣
ξ
k
p
,
z
k
l
-
l
0
. Using Algorithm 2 as our basis, we rewrite the occupancy grid update in vector form. If we assume that the full occupancy grid is stored in vector form, then the occupancy grid update can be performed using(18)
m
k
p
=
m
k
-
1
p
+
α
k
p
-
β
,where α
k
[
p
] is constructed using Algorithm 4 and β is vector with every element containing l
0.Algorithm 4:Construction of α
[
p
].
(
1) procedure Al
phaConstruction(m
,
z
k)
(2)  for  i
=
1, n  do
(3)    α
(
i
)
=
l
0
(4)  end for
(5)  for  i
=
1, n  do
(6)    for  j
=
1, m  do
(7)      if  m
(
i
) in the perceptual range of z
k
(
j
)  then
(8)        α
i
=
log
⁡
m
i
∣
ξ
1
:
k
[
p
]
,
z
k
j
1
-
p
m
i
∣
ξ
1
:
k
[
p
]
,
z
k
j
(9)      end if
(10)    end for
(11)  end for
(12) end procedureAlgorithm4 first initializes α
k
[
p
] using l
0. This ensures that cells that do not fall within the perceptual range of the sensor remain unchanged by the algorithm. For cells that fall into the perceptual range of the sensor, their α value is set as the log odds form of the inverse sensor model and it is this value that either increases or decreases the likelihood that the cell is occupied. Equation (18) updates the occupancy grid in vector form; however we need to modify it so that it updates the occupancy grid which is stored using an alternative basis. Incorporating (14), the vector form of the occupancy grid update in the alternate basis becomes(19)
c
k
p
=
c
k
-
1
p
+
Φ
T
α
k
p
-
Φ
T
β
,where Φ
T is used to convert α
k
[
p
] and β to the alternate basis. In (19), Φ
T is used to change basis as opposed to Φ
-
1 because we assume that our choice of basis is orthonormal so Φ
-
1
=
Φ
T; however if the alternative basis that is selected is not orthonormal then Φ
-
1 should replace Φ
T in (19). We can now simplify (19) by grouping the two components that are multiplied by Φ
T to reduce the number of matrix-vector multiplications; this yields(20)
c
k
p
=
c
k
-
1
p
+
Φ
T
α
k
p
-
β
.Equation (20) updates the occupancy grid in the alternate basis; hence the final step is to incorporate compression into the update. The occupancy grid maintained by each particle contains the compressed set of basis of coefficients; thus (20) can be updated using (15) to yield(21)
c
k
p
=
H
r
y
k
-
1
p
+
Φ
T
α
k
p
-
β
.Finally, the basis coefficients used to represent the occupancy grid must be recompressed in order to be stored by the particle. Including the final compression the compressed occupancy grid update can be performed according to(22)
y
k
p
=
H
H
r
y
k
-
1
p
+
Φ
T
α
k
p
-
β
,which is seen in Algorithm 3 Line (8).


## 3.1. FastSLAM with Occupancy Grids

As opposed to many SLAM solutions that attempt to solve the online SLAM problem that is estimating the posterior that represents the pose of a UMV and the map of the environment based on a set of control inputs and sensor measurements, the FastSLAM approach attempts to estimate the full SLAM posterior. The full SLAM posterior is given as(1)
p
ξ
1
:
k
,
M
∣
u
1
:
k
,
z
1
:
k
,which is the distribution that represents the trajectory of UMV ξ
1
:
k and the map of the environment M based on the set of control inputs u
1
:
k and observations z
1
:
k. By estimating the full SLAM posterior, (1) can be factored, using the property of conditional independence, into a pair of simpler terms which is given by(2)
p
ξ
1
:
k
,
M
∣
u
1
:
k
,
z
1
:
k
=
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
×
p
M
∣
ξ
1
:
k
,
u
1
:
k
,
z
1
:
k
,where p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k is the distribution that represents the UMV trajectory and p
M
∣
ξ
1
:
k
,
u
1
:
k
,
z
1
:
k is the distribution that represents the map of the environment. The FastSLAM approach estimates the factored SLAM posterior (2) using a Rao-Blackwellized particle filter. The Rao-Blackwellized particle filter is a version of a sampling importance resampling (SIR) [5] particle filter and it is the SIR particle filter that provides the format of the FastSLAM solution. The Rao-Blackwellized particle filter estimates the distribution representing a portion of the state, which in FastSLAM is the UMV trajectory ξ
1
:
k, using a particle filter, and each particle in the particle filter maintains its own estimate of the remainder of the state, which is the map M. As a result, each particle in the particle filter is composed of a UMV pose and a map, which the FastSLAM OG algorithm represents using an occupancy grid; thus the pth particle at time k is defined as(3)
X
k
p
≜
ξ
k
p
M
k
p
,where ξ
k
[
p
] is the pose estimate of the pth particle at time k, M
k
[
p
] is the occupancy grid of the environment maintained by the particle as it has been generated through time step k, and p
∈
1
,
n is the index corresponding to the pth particle where n is the total number of particles used in the particle filter. The FastSLAM approach is implemented in a four-step procedure based on the use of a SIR particle filter to estimate (1) and an overview of the algorithm is presented in Algorithm 1 for time k.Algorithm 1:FastSLAM OG algorithm.
(
1) procedure  Fa
stSLAMOG(X
k
-
1
,
u
k
,
z
k)
(2)  X
-
k
←
(3)  for  p
←
1, n  do
(4)    ξ
k
[
p
]
~
p
(
ξ
k
∣
ξ
k
-
1
p
,
u
k
)
(5)    w
[
p
]
←
η
p
(
z
k
∣
ξ
k
p
,
M
k
-
1
p
)
(6)    M
k
[
p
] = OGUpdate
(
ξ
k
p
,
M
k
-
1
p
,
z
k
)
(7)    X
-
k
[
p
]
←
ξ
k
p
M
k
p
w
[
p
]
(8)  end for
(9)  X
k
←
(10)  for  p
=
1, n  do
(11)    select j with probability proportional to w
[
p
]
(12)    X
k
[
p
]
←
X
-
k
[
j
]
(13)  end for
(14) end procedure


### 3.1.1. Motion Model Update

In the first step of the algorithm (Line (4) in Algorithm 1) a new particle pose is generated and added to a set of temporary particles, X
¯
k. The new pose for the pth particle is generated by sampling from the probabilistic motion model of the UMV:(4)
ξ
k
p
~
p
ξ
k
∣
ξ
k
-
1
p
,
u
k
.The probabilistic motion model of the UMV generates a new pose based on the state transition model of the UMV, the pose of the pth particle at time step k
-
1, the current control input u
k, and the characteristics of the noise on u
k. The result is that the particles in X
¯
k are distributed according to(5)
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
-
1
,which is referred to as theproposal distribution. The actual distribution which we are attempting to estimate is referred to as thetarget distribution and given as(6)
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
.The proposal distribution is transformed to the target distribution in the last stage of the algorithm, the resampling stage (Lines (10)–(13) in in Algorithm 1). Before the resampling process can occur, z
k must be incorporated into the estimate, which is achieved in the calculation of the particle’s importance weight.


### 3.1.2. Importance Weight Calculation

In the second step of the algorithm an importance weight for each particle is generated. The need for an importance weight for each particle comes from the use of a SIR particle filter and, as seen in the previous section, we are sampling particles from a distribution different from the one that we are attempting to estimate. According to [6], if we are unable to directly sample from the distribution that we are attempting to estimate, then the importance weight for each particle is given as(7)
w
k
p
=
t
a
r
g
e
t
d
i
s
t
r
i
b
u
t
i
o
n
p
r
o
p
o
s
a
l
d
i
s
t
r
i
b
u
t
i
o
n
, and particles are drawn from X
¯
k with replacement and added to X
k with a probability proportional to w
k
[
p
]. When drawing particles from X
¯
k with replacement, each particle that is added to X
k remains in X
¯
k so that a given particle that is selected has the potential of being added to X
k multiple times. After the resampling process X
k will approximate the target distribution and the quality of the approximation will improve, the number of particles increases.Using (7) along with the previously discussed distributions (5) and (6), the importance weight of each particle simplifies to [7](8)
w
k
p
=
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
-
1
=
η
p
z
k
∣
ξ
k
p
,
M
k
-
1
p
,where p
z
k
∣
ξ
k
p
,
M
k
-
1
[
p
] is the probabilistic measurement model of the sensor being used by the UMV and η
>
0 is a normalizing factor. The probabilistic measurement model is dependent on the perceptual sensor being used by the UMV and it calculates the probability that the sensor measurement would make based on the current pose estimate and map maintained by the particle. This form of importance weight incorporates the sensor observations into the estimate which were not included in the pose sampling procedure. Once the importance weight has been calculated for each particle then the occupancy grid maintained by each particle is updated using the new particle pose and current sensor measurement.


### 3.1.3. Occupancy Grid Update

Once the importance weight for each particle has been calculated, the third step of the algorithm updates the occupancy grid of each particle using the current sensor measurement and pose estimate. Occupancy grids decompose an unknown environment into a finite set of cells where each cell holds the probability that the location in the world enclosed by the cell is occupied by an object. From [8], the occupancy grid for a given particle at time k is obtained in a probabilistic sense by calculating the posterior over maps based on the history of sensor measurements and trajectory of a particle:(9)
p
M
k
p
∣
ξ
1
:
k
p
,
z
1
:
k
.By decomposing the environment into cells, the posterior calculated by the occupancy grid can be restated as a product of cellular posteriors:(10)
p
M
k
p
∣
ξ
1
:
k
p
,
z
1
:
k
=
∏
i
,
j
p
M
k
p
i
,
j
∣
ξ
1
:
k
p
,
z
1
:
k
,where p
M
k
[
p
]
i
,
j
∣
ξ
1
:
k
[
p
]
,
z
1
:
k contains the probability that the location in the environment represented by the cell at i
,
j is occupied based on the measurement history and particle trajectory. A solution to a problem of this form is the binary Bayes filter [8]. The binary Bayes filter stores the probability of occupancy for each cell in log odds form to prevent truncation errors that can occur during the update process. The log odds form of the variable x
∈
R is given as(11)
l
x
≜
log
⁡
x
1
-
x
,and the value of x can be recovered from the log odds form as(12)
x
≜
1
-
1
1
+
exp
⁡
l
x
.The binary Bayes filter based occupancy grid update modifies the probability of occupancy of each cell in the grid when an observation provides information about that location. As a consequence, for locations that have been observed, the probability of occupancy quickly approaches 0 for free cells and 1 for occupied cells. While the addition of new information to the estimate makes the probability quickly approach the limits, the probability of occupancy of each cell should never equal 0 or 1 as this would assume perfect knowledge about the occupancy of the cell. Depending on the data type selected to store the probability, the precision required to store the value can quickly be exceeded resulting in the probability being rounded to either 0 or 1. Once this rounding has occurred any new information learned about the occupancy of a cell is lost. To overcome this problem the binary Bayes filter stores the probability of occupancy in log odds form which takes the potential probability values in 0,1 and spreads them over the range -
∞
,
∞. By spreading the small range of probability values over this much larger range, the data type being used to store the probability has more precision to store the probability thus preventing the truncation/rounding from occurring. The occupancy grid update algorithm based on the binary Bayes filter is provided in Algorithm 2.Algorithm 2:Standard occupancy grid update algorithm.
(
1) procedure OGUp
date(ξ
k
,
M
k
-
1
,
z
k)
(2)  for  i
=
1, h  do
(3)    for  j
=
1, w  do
(4)      for  l
=
1, m  do
(5)        if  M
k
-
1
(
i
,
j
) lies in the perceptual range of z
k
(
l
)  then
(6)          M
k
i
,
j
⟵
M
k
-
1
i
,
j
+
log
⁡
p
M
i
,
j
∣
ξ
k
,
z
k
l
1
-
p
M
i
,
j
∣
ξ
k
,
z
k
l
-
l
0
(7)        end if
(8)      end for
(9)    end for
(10)  end for
(11) end procedureIn Algorithm2, p
M
i
,
j
∣
ξ
k
,
z
k
(
l
) is the inverse sensor model of the sensor being used to generate the map and it returns the probability that the cell at i
,
j is occupied based on the lth sensor measurement and the pose estimate of the particle. The value of l
0 in Algorithm 2 is referred to as the prior of occupancy and is defined as(13)
l
0
≜
l
o
g
p
M
i
,
j
1
-
p
M
i
,
j and gives the probability that a cell is occupied before any sensor measurements are integrated into the map. Typically p
M
i
,
j in (13) is selected to be 0.5.


### 3.1.4. Resampling

After the occupancy grid for each particle inX
¯
k has been updated, the final step of the algorithm is the resampling process. During the resampling process particles are drawn with replacement from X
¯
k with a probability proportional to w
[
p
]. This resampling step takes the set of temporary particles that are distributed according to (5) and ensures that there is a high probability that X
k is distributed according to (6).One of the problems with this approach is the large amount of memory required to run the algorithm. The estimate generated by the algorithm improves as the number of particles increases; thus we like to have as many particles as possible. However, each particle must keep a copy of the occupancy grid and each occupancy grid can be quite large. It is easy to see that while we would like to run the particle filter with as many particles as possible we can be limited by the fact that each particle must maintain a full copy of the occupancy grid. To address this problem we propose an updated version of the algorithm that decreases the amount of memory required for each particle by storing a compressed version of the occupancy grid as opposed to the full uncompressed occupancy grid.


## 3.1.1. Motion Model Update

In the first step of the algorithm (Line (4) in Algorithm 1) a new particle pose is generated and added to a set of temporary particles, X
¯
k. The new pose for the pth particle is generated by sampling from the probabilistic motion model of the UMV:(4)
ξ
k
p
~
p
ξ
k
∣
ξ
k
-
1
p
,
u
k
.The probabilistic motion model of the UMV generates a new pose based on the state transition model of the UMV, the pose of the pth particle at time step k
-
1, the current control input u
k, and the characteristics of the noise on u
k. The result is that the particles in X
¯
k are distributed according to(5)
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
-
1
,which is referred to as theproposal distribution. The actual distribution which we are attempting to estimate is referred to as thetarget distribution and given as(6)
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
.The proposal distribution is transformed to the target distribution in the last stage of the algorithm, the resampling stage (Lines (10)–(13) in in Algorithm 1). Before the resampling process can occur, z
k must be incorporated into the estimate, which is achieved in the calculation of the particle’s importance weight.


## 3.1.2. Importance Weight Calculation

In the second step of the algorithm an importance weight for each particle is generated. The need for an importance weight for each particle comes from the use of a SIR particle filter and, as seen in the previous section, we are sampling particles from a distribution different from the one that we are attempting to estimate. According to [6], if we are unable to directly sample from the distribution that we are attempting to estimate, then the importance weight for each particle is given as(7)
w
k
p
=
t
a
r
g
e
t
d
i
s
t
r
i
b
u
t
i
o
n
p
r
o
p
o
s
a
l
d
i
s
t
r
i
b
u
t
i
o
n
, and particles are drawn from X
¯
k with replacement and added to X
k with a probability proportional to w
k
[
p
]. When drawing particles from X
¯
k with replacement, each particle that is added to X
k remains in X
¯
k so that a given particle that is selected has the potential of being added to X
k multiple times. After the resampling process X
k will approximate the target distribution and the quality of the approximation will improve, the number of particles increases.Using (7) along with the previously discussed distributions (5) and (6), the importance weight of each particle simplifies to [7](8)
w
k
p
=
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
p
ξ
1
:
k
∣
u
1
:
k
,
z
1
:
k
-
1
=
η
p
z
k
∣
ξ
k
p
,
M
k
-
1
p
,where p
z
k
∣
ξ
k
p
,
M
k
-
1
[
p
] is the probabilistic measurement model of the sensor being used by the UMV and η
>
0 is a normalizing factor. The probabilistic measurement model is dependent on the perceptual sensor being used by the UMV and it calculates the probability that the sensor measurement would make based on the current pose estimate and map maintained by the particle. This form of importance weight incorporates the sensor observations into the estimate which were not included in the pose sampling procedure. Once the importance weight has been calculated for each particle then the occupancy grid maintained by each particle is updated using the new particle pose and current sensor measurement.


## 3.1.3. Occupancy Grid Update

Once the importance weight for each particle has been calculated, the third step of the algorithm updates the occupancy grid of each particle using the current sensor measurement and pose estimate. Occupancy grids decompose an unknown environment into a finite set of cells where each cell holds the probability that the location in the world enclosed by the cell is occupied by an object. From [8], the occupancy grid for a given particle at time k is obtained in a probabilistic sense by calculating the posterior over maps based on the history of sensor measurements and trajectory of a particle:(9)
p
M
k
p
∣
ξ
1
:
k
p
,
z
1
:
k
.By decomposing the environment into cells, the posterior calculated by the occupancy grid can be restated as a product of cellular posteriors:(10)
p
M
k
p
∣
ξ
1
:
k
p
,
z
1
:
k
=
∏
i
,
j
p
M
k
p
i
,
j
∣
ξ
1
:
k
p
,
z
1
:
k
,where p
M
k
[
p
]
i
,
j
∣
ξ
1
:
k
[
p
]
,
z
1
:
k contains the probability that the location in the environment represented by the cell at i
,
j is occupied based on the measurement history and particle trajectory. A solution to a problem of this form is the binary Bayes filter [8]. The binary Bayes filter stores the probability of occupancy for each cell in log odds form to prevent truncation errors that can occur during the update process. The log odds form of the variable x
∈
R is given as(11)
l
x
≜
log
⁡
x
1
-
x
,and the value of x can be recovered from the log odds form as(12)
x
≜
1
-
1
1
+
exp
⁡
l
x
.The binary Bayes filter based occupancy grid update modifies the probability of occupancy of each cell in the grid when an observation provides information about that location. As a consequence, for locations that have been observed, the probability of occupancy quickly approaches 0 for free cells and 1 for occupied cells. While the addition of new information to the estimate makes the probability quickly approach the limits, the probability of occupancy of each cell should never equal 0 or 1 as this would assume perfect knowledge about the occupancy of the cell. Depending on the data type selected to store the probability, the precision required to store the value can quickly be exceeded resulting in the probability being rounded to either 0 or 1. Once this rounding has occurred any new information learned about the occupancy of a cell is lost. To overcome this problem the binary Bayes filter stores the probability of occupancy in log odds form which takes the potential probability values in 0,1 and spreads them over the range -
∞
,
∞. By spreading the small range of probability values over this much larger range, the data type being used to store the probability has more precision to store the probability thus preventing the truncation/rounding from occurring. The occupancy grid update algorithm based on the binary Bayes filter is provided in Algorithm 2.Algorithm 2:Standard occupancy grid update algorithm.
(
1) procedure OGUp
date(ξ
k
,
M
k
-
1
,
z
k)
(2)  for  i
=
1, h  do
(3)    for  j
=
1, w  do
(4)      for  l
=
1, m  do
(5)        if  M
k
-
1
(
i
,
j
) lies in the perceptual range of z
k
(
l
)  then
(6)          M
k
i
,
j
⟵
M
k
-
1
i
,
j
+
log
⁡
p
M
i
,
j
∣
ξ
k
,
z
k
l
1
-
p
M
i
,
j
∣
ξ
k
,
z
k
l
-
l
0
(7)        end if
(8)      end for
(9)    end for
(10)  end for
(11) end procedureIn Algorithm2, p
M
i
,
j
∣
ξ
k
,
z
k
(
l
) is the inverse sensor model of the sensor being used to generate the map and it returns the probability that the cell at i
,
j is occupied based on the lth sensor measurement and the pose estimate of the particle. The value of l
0 in Algorithm 2 is referred to as the prior of occupancy and is defined as(13)
l
0
≜
l
o
g
p
M
i
,
j
1
-
p
M
i
,
j and gives the probability that a cell is occupied before any sensor measurements are integrated into the map. Typically p
M
i
,
j in (13) is selected to be 0.5.


## 3.1.4. Resampling

After the occupancy grid for each particle inX
¯
k has been updated, the final step of the algorithm is the resampling process. During the resampling process particles are drawn with replacement from X
¯
k with a probability proportional to w
[
p
]. This resampling step takes the set of temporary particles that are distributed according to (5) and ensures that there is a high probability that X
k is distributed according to (6).One of the problems with this approach is the large amount of memory required to run the algorithm. The estimate generated by the algorithm improves as the number of particles increases; thus we like to have as many particles as possible. However, each particle must keep a copy of the occupancy grid and each occupancy grid can be quite large. It is easy to see that while we would like to run the particle filter with as many particles as possible we can be limited by the fact that each particle must maintain a full copy of the occupancy grid. To address this problem we propose an updated version of the algorithm that decreases the amount of memory required for each particle by storing a compressed version of the occupancy grid as opposed to the full uncompressed occupancy grid.


## 3.2. FastSLAM with Compressed Occupancy Grids

Before discussing the updated SLAM algorithm, we will first address the compressed form of the occupancy grid. To compressM
k
[
p
] we first reshape the occupancy grid so that r
×
c matrix becomes a column vector containing n
=
r
c elements and we refer to the vector form of the occupancy grid as m
k
[
p
]
≜
v
e
c
(
M
k
[
p
]
). The reshaping process is performed in such a way that the cell in the original occupancy grid located at i
,
j can be accessed in the vector form at l, where l
=
i
c
+
j. In order to compress the occupancy grid without major degradation, we assume that the occupancy grid can be represented in an alternate basis in which it is sparse. The relationship between the two different representations is given as(14)
m
k
p
=
Φ
c
k
p
,where Φ is a transformation matrix that defines the relationship and c
k
[
p
] is the set of coefficients that represent the occupancy grid in the alternate basis. In order for the occupancy grid to be compressible, c
k
[
p
] must be sparse [9]; that is #
{
s
u
p
p
c
k
[
p
]
}
<
n, where s
u
p
p
(
·
) is the support operator and # denotes the cardinality of the set. This means that, in order for us to be able to compress the occupancy grid, we must find a basis in which the coefficient vector is composed of a large number of zeros. If this condition is met then the occupancy grid for a given particle can be compressed using(15)
y
k
p
=
H
c
k
p
,where H is a matrix that performs the compression by selecting a subset of coefficient data that is smaller than the amount of data in the complete signal.The FastSLAM COG algorithm is a modification of Algorithm1. Since we are now maintaining the compressed form of the occupancy grid, ith particle in the particle set is defined as(16)
X
k
p
≜
ξ
k
p
y
k
p
.As with FastSLAM OG, FastSLAM COG is performed in several steps and an overview of the updated algorithm is presented in Algorithm 3. As a result of using compressed occupancy grids the only changes that must be addressed are the steps of the algorithm that require the occupancy grid, the importance weight calculation, and occupancy grid update.Algorithm 3:FastSLAM algorithm with compressed occupancy grids.
(1) procedure Fa
stSLAMCOG(X
k
-
1
,
u
k
,
z
k)
(2)   X
-
k
←
(3)   for  p
←
1, n  do
(4)     ξ
k
[
p
]
~
p
(
ξ
k
∣
ξ
k
-
1
p
,
u
k
)
(5)     c
^
k
[
p
] = uncompress(y
k
-
1
p)
(6)     w
[
p
]
←
η
p
(
z
k
∣
ξ
k
p
,
Φ
c
^
k
[
p
]
)
(7)     α
k
[
p
] = AlphaConstruction(m
k
-
1
[
p
]
,
z
k)
(8)     y
k
[
p
]
=
H
[
c
^
k
p
+
Φ
T
[
α
k
p
-
β
]
]
(9)     X
-
k
[
p
]
←
ξ
k
[
p
]
y
k
[
p
]
w
[
p
]
(10)   end for
(11)   X
k
←
(12)   for  j
=
1, n  do
(13)     sample with replacement from select i with probability proportional to w
[
p
]
(14)     X
k
[
p
]
←
X
-
k
[
p
]
(15)   end for
(16) end procedure


### 3.2.1. Importance Weight Calculation

As discussed in Section3.1.2, the importance weight for each particle is the normalized probabilistic measurement model for each particle. The probabilistic measurement model is a measure of how well the current set of sensor measurements matches up with what we expect based on the current pose and map of the particle. In calculating the probabilistic measurement model, the algorithm must have access to the raw occupancy grid values so that an expected sensor measurement can be generated. Based on this requirement, the compressed occupancy grid coefficients are first uncompressed. For now a general reconstruction operator denoted as H
r is used; however the details of several reconstruction methods are explored in detail in Section 4. Once the compressed coefficients have been reconstructed, the occupancy grid can be extracted using (14). The importance weight calculation can be found in Algorithm 3 on Line (6).


### 3.2.2. Compressed Occupancy Grid Update

As previously seen in Algorithm2, the standard approach to updating the occupancy grid is to examine every cell in the occupancy grid and if a given cell falls within the perceptual range of the sensor attached to the UMV then the corresponding cell of the occupancy grid is updated according to(17)
M
k
p
i
,
j
=
M
k
-
1
p
i
,
j
+
l
o
g
p
M
i
,
j
∣
ξ
k
p
,
z
k
l
1
-
p
M
i
,
j
∣
ξ
k
p
,
z
k
l
-
l
0
. Using Algorithm 2 as our basis, we rewrite the occupancy grid update in vector form. If we assume that the full occupancy grid is stored in vector form, then the occupancy grid update can be performed using(18)
m
k
p
=
m
k
-
1
p
+
α
k
p
-
β
,where α
k
[
p
] is constructed using Algorithm 4 and β is vector with every element containing l
0.Algorithm 4:Construction of α
[
p
].
(
1) procedure Al
phaConstruction(m
,
z
k)
(2)  for  i
=
1, n  do
(3)    α
(
i
)
=
l
0
(4)  end for
(5)  for  i
=
1, n  do
(6)    for  j
=
1, m  do
(7)      if  m
(
i
) in the perceptual range of z
k
(
j
)  then
(8)        α
i
=
log
⁡
m
i
∣
ξ
1
:
k
[
p
]
,
z
k
j
1
-
p
m
i
∣
ξ
1
:
k
[
p
]
,
z
k
j
(9)      end if
(10)    end for
(11)  end for
(12) end procedureAlgorithm4 first initializes α
k
[
p
] using l
0. This ensures that cells that do not fall within the perceptual range of the sensor remain unchanged by the algorithm. For cells that fall into the perceptual range of the sensor, their α value is set as the log odds form of the inverse sensor model and it is this value that either increases or decreases the likelihood that the cell is occupied. Equation (18) updates the occupancy grid in vector form; however we need to modify it so that it updates the occupancy grid which is stored using an alternative basis. Incorporating (14), the vector form of the occupancy grid update in the alternate basis becomes(19)
c
k
p
=
c
k
-
1
p
+
Φ
T
α
k
p
-
Φ
T
β
,where Φ
T is used to convert α
k
[
p
] and β to the alternate basis. In (19), Φ
T is used to change basis as opposed to Φ
-
1 because we assume that our choice of basis is orthonormal so Φ
-
1
=
Φ
T; however if the alternative basis that is selected is not orthonormal then Φ
-
1 should replace Φ
T in (19). We can now simplify (19) by grouping the two components that are multiplied by Φ
T to reduce the number of matrix-vector multiplications; this yields(20)
c
k
p
=
c
k
-
1
p
+
Φ
T
α
k
p
-
β
.Equation (20) updates the occupancy grid in the alternate basis; hence the final step is to incorporate compression into the update. The occupancy grid maintained by each particle contains the compressed set of basis of coefficients; thus (20) can be updated using (15) to yield(21)
c
k
p
=
H
r
y
k
-
1
p
+
Φ
T
α
k
p
-
β
.Finally, the basis coefficients used to represent the occupancy grid must be recompressed in order to be stored by the particle. Including the final compression the compressed occupancy grid update can be performed according to(22)
y
k
p
=
H
H
r
y
k
-
1
p
+
Φ
T
α
k
p
-
β
,which is seen in Algorithm 3 Line (8).


## 3.2.1. Importance Weight Calculation

As discussed in Section3.1.2, the importance weight for each particle is the normalized probabilistic measurement model for each particle. The probabilistic measurement model is a measure of how well the current set of sensor measurements matches up with what we expect based on the current pose and map of the particle. In calculating the probabilistic measurement model, the algorithm must have access to the raw occupancy grid values so that an expected sensor measurement can be generated. Based on this requirement, the compressed occupancy grid coefficients are first uncompressed. For now a general reconstruction operator denoted as H
r is used; however the details of several reconstruction methods are explored in detail in Section 4. Once the compressed coefficients have been reconstructed, the occupancy grid can be extracted using (14). The importance weight calculation can be found in Algorithm 3 on Line (6).


## 3.2.2. Compressed Occupancy Grid Update

As previously seen in Algorithm2, the standard approach to updating the occupancy grid is to examine every cell in the occupancy grid and if a given cell falls within the perceptual range of the sensor attached to the UMV then the corresponding cell of the occupancy grid is updated according to(17)
M
k
p
i
,
j
=
M
k
-
1
p
i
,
j
+
l
o
g
p
M
i
,
j
∣
ξ
k
p
,
z
k
l
1
-
p
M
i
,
j
∣
ξ
k
p
,
z
k
l
-
l
0
. Using Algorithm 2 as our basis, we rewrite the occupancy grid update in vector form. If we assume that the full occupancy grid is stored in vector form, then the occupancy grid update can be performed using(18)
m
k
p
=
m
k
-
1
p
+
α
k
p
-
β
,where α
k
[
p
] is constructed using Algorithm 4 and β is vector with every element containing l
0.Algorithm 4:Construction of α
[
p
].
(
1) procedure Al
phaConstruction(m
,
z
k)
(2)  for  i
=
1, n  do
(3)    α
(
i
)
=
l
0
(4)  end for
(5)  for  i
=
1, n  do
(6)    for  j
=
1, m  do
(7)      if  m
(
i
) in the perceptual range of z
k
(
j
)  then
(8)        α
i
=
log
⁡
m
i
∣
ξ
1
:
k
[
p
]
,
z
k
j
1
-
p
m
i
∣
ξ
1
:
k
[
p
]
,
z
k
j
(9)      end if
(10)    end for
(11)  end for
(12) end procedureAlgorithm4 first initializes α
k
[
p
] using l
0. This ensures that cells that do not fall within the perceptual range of the sensor remain unchanged by the algorithm. For cells that fall into the perceptual range of the sensor, their α value is set as the log odds form of the inverse sensor model and it is this value that either increases or decreases the likelihood that the cell is occupied. Equation (18) updates the occupancy grid in vector form; however we need to modify it so that it updates the occupancy grid which is stored using an alternative basis. Incorporating (14), the vector form of the occupancy grid update in the alternate basis becomes(19)
c
k
p
=
c
k
-
1
p
+
Φ
T
α
k
p
-
Φ
T
β
,where Φ
T is used to convert α
k
[
p
] and β to the alternate basis. In (19), Φ
T is used to change basis as opposed to Φ
-
1 because we assume that our choice of basis is orthonormal so Φ
-
1
=
Φ
T; however if the alternative basis that is selected is not orthonormal then Φ
-
1 should replace Φ
T in (19). We can now simplify (19) by grouping the two components that are multiplied by Φ
T to reduce the number of matrix-vector multiplications; this yields(20)
c
k
p
=
c
k
-
1
p
+
Φ
T
α
k
p
-
β
.Equation (20) updates the occupancy grid in the alternate basis; hence the final step is to incorporate compression into the update. The occupancy grid maintained by each particle contains the compressed set of basis of coefficients; thus (20) can be updated using (15) to yield(21)
c
k
p
=
H
r
y
k
-
1
p
+
Φ
T
α
k
p
-
β
.Finally, the basis coefficients used to represent the occupancy grid must be recompressed in order to be stored by the particle. Including the final compression the compressed occupancy grid update can be performed according to(22)
y
k
p
=
H
H
r
y
k
-
1
p
+
Φ
T
α
k
p
-
β
,which is seen in Algorithm 3 Line (8).


## 4. Compressed Signal Reconstruction Methods

As previously described we would like to store a compressed version of the occupancy grid as opposed to the complete occupancy grid to decrease the memory requirements of the algorithm. In order to make use of the compressed occupancy grid, a method for reconstructing the full occupancy grid from the compressed version must be selected. In this section several reconstruction methods are examined with the goal of selecting an approach to be used in FastSLAM COG. In FastSLAM COG we are attempting to compress the vector form of an occupancy grid; however in this section we will examine methods for reconstructing a generic discrete signalx
∈
R
n that has been compressed.We representx using some alternate basis in which it is sparse and the relationship between x and its sparse representation is given by(23)
x
=
Φ
c
,where c
∈
R
n is a vector of coefficients that represent x in the alternate basis and Φ
∈
R
n
×
n is a matrix of basis functions that determines the relationship between x and c. The discrete signal used in this section for testing is displayed in Figure 1(a) and the set of coefficients used to represent the signal in the discrete cosine basis [10] can be seen in Figure 1(b). The test signal is generated by evaluating(24)
f
t
=
s
i
n
t
-
c
o
s
2
t
-
3
sin
⁡
0.25
t
, at 128 evenly spaced points in 0,10.Figure 1
(a) The test function given by (24) and (b) the test function represented in the discrete cosine basis.
(a)
(b)Ifc is sparse then x can be compressed; this sparseness can be seen by examining Figure 1(b) and noticing that a large number of the coefficients are zero. The compression is performed by selecting a subset of the coefficient information using(25)
y
=
H
c
,where H
∈
R
m
×
n is the compression matrix and m
∈
N is the size of the compressed signal with m
<
n.


### 4.1.l
2 Reconstruction (l
2)

One of the most simple methods for reconstructing a compressed signal is to reconstruct the signal while minimizing the error between the original and reconstructed signals as measured byl
2 norm, where l
2 norm of a signal x
∈
R
n is defined as(26)
x
2
≜
∑
i
=
1
n
x
i
2
.In examining (25), if the size of the compressed signal is the same as the original then the reconstruction can be performed according to(27)
c
=
H
-
1
y
.In reality the system is underdetermined due to m
<
n. A common solution to problems of this form uses the Moore-Penrose pseudoinverse of H.The Moore-Penrose pseudoeinverse [11, 12] is used to find the solution that minimizes l
2 error for systems of the form(28)
A
x
=
b
,where x
∈
R
n, b
∈
R
m, and A
∈
R
m
×
n. The solution to the system found using the pseudoinverse is defined as(29)
z
=
A
†
b
,where A
† is the pseudoinverse of A and it minimizes l
2 error so that(30)
A
x
-
b
2
≥
A
z
-
b
2
,for all x
∈
R
n if x is defined as(31)
x
=
A
†
b
+
I
-
A
†
A
w
,for any vector w
∈
R
n.Using the pseudoinverse ofH a set of reconstructed coefficients is generated according to(32)
c
^
=
H
†
y
,and the reconstructed value of the signal is generated from(33)
x
^
=
Φ
c
^
.The results ofl
2 reconstruction using four separate amounts of signal information, 25
%, 50
%, 75
%, and 95
%, are presented in Figure 2. The form selected for H needed to obtain such level of compression is an orthonormal Gaussian random matrix as it possesses the RIP property whose importance will be discussed in the following section.Figure 2
The test function reconstructed by minimizingl
2 error for compressed size of (a) 25
%, (b) 50
%, (c) 75
%, and (d) 95
% of the original data.
(a)
(b)
(c)
(d)


### 4.2. Compressed Sensing Reconstruction (CS)

The second reconstruction algorithm selected is based on an approach referred to in the literature as compressed sensing [13]. Compressed sensing allows for sparse signals to be reconstructed using fewer measurements that would be required according to the Nyquist/Shannon sampling principle. In compressed sensing the full set of sparse coefficients can be reconstructed from the compressed set by solving(34)
m
i
n
c
^
0
s
u
b
j
e
c
t
t
o
y
-
H
c
^
2
2
≤
ε
, for some sufficiently small ε. In (34) c
^
0
≜
#
s
u
p
p
c
^ is a count of the number of nonzero elements in the vector c
^. Solving (34) is NP-hard and the solution cannot be found efficiently. A key finding of compressed sensing is that if the measurement matrix, H, is constructed in such a way that it obeys the Restricted Isometry Property (RIP), which is defined as(35)
1
-
δ
c
2
2
≤
H
c
2
2
≤
1
+
δ
c
2
2
, form some small δ
≥
0, then ·
0 in (34) can be replaced by ·
1 resulting in(36)
m
i
n
c
^
1
s
u
b
j
e
c
t
t
o
y
-
H
c
^
2
2
≤
ε
. As opposed to being NP-hard to solve, (36) is a constrained convex optimization problem and can be solved efficiently using one of several available software packages with Lasso [14], NESTA [15], and l1 magic [16] being some examples. As in l
2 case, once the complete set of coefficients have been estimated using the compressed set, an estimate of the original signal is generated according to (33).In order to examine the performance of the compressed sensing reconstruction approach, the test signal is reconstructed using the same four amounts of data,25
%, 50
%, 75
%, and 95
%, with the same orthonormal Gaussian random matrix being used to perform the compression. Equation (36) is solved using the NESTA software library and the results of the reconstruction are shown in Figure 3.Figure 3
The test function reconstructed by solving the compressed sensing convex optimization problem for compressed size of (a)25
%, (b) 50
%, (c) 75
%, and (d) 95
% of the original data.
(a)
(b)
(c)
(d)


### 4.3. Compressed Sensing Embedded Kalman Filtering (CSEKF)

In the previous section a reconstruction approach was introduced that allows for the reconstruction of compressed signals using less data than expected based on the Nyquist/Shannon sampling principle. As seen in Figure3 this method does a better job at reconstructing a sparse signal than the approach that reconstructs the signal by minimizing l
2 norm. The authors of [17] introduce an approach that performs the compressed sensing reconstructing using an augmented Kalman filter (KF) [18] which is easy to implement, compared to the compressed sensing approach. The compressed sensing approach must solve a complex convex minimization problem, where problems of this type are typically being solved using an external software package. In order to use the KF it is assumed that the full set of coefficients evolve from time step to time step according to(37)
c
k
=
A
k
c
k
-
1
+
ε
k
,where A
k
∈
R
n
×
n is the state transition matrix of the system which describes how the signal coefficients change between time steps, and ε
k
∈
R
n is a zero mean Gaussian vector with covariance R
k
≥
0. The compressed form of the coefficients is generated according to(38)
y
k
=
H
k
c
k
+
δ
k
,where H
k
∈
R
m
×
n is the same compression matrix discussed in (25) and δ
k
∈
R
m is a zero mean Gaussian vector with covariance Q
k
>
0.The CSEKF algorithm first performs a standard KF estimation that is carried out in a two parts, aprediction phase and acorrection phase. The KF estimates p
c
k
∣
y
k as a Gaussian distribution that can be represented using a mean vector c
^
k and covariance matrix Σ
k. The KF generates the estimate using a standard set of five equations [18]:(39)
c
^
¯
k
=
A
k
c
^
k
-
1
,
(40)
Σ
¯
k
=
A
k
Σ
k
-
1
A
k
T
+
R
k
,
(41)
K
k
=
Σ
¯
k
H
k
T
H
k
Σ
¯
k
H
k
T
+
Q
k
-
1
,
(42)
c
^
k
=
c
^
¯
k
+
K
k
y
k
-
H
k
c
^
¯
k
,
(43)
Σ
k
=
I
-
K
k
H
k
Σ
¯
k
,where I in (43) is m dimensional identity matrix. The estimate produced by the KF minimizes the error, using l
2 norm, between the estimate and the actual set of coefficients(44)
m
i
n
c
k
-
c
^
k
2
2
;however, this is not what we are trying to solve according to the compressed sensing approach. The CSEKF presented by the authors replaces the classic compressed sensing convex optimization problem (36) with the dual problem:(45)
m
i
n
c
k
-
c
^
k
2
2
s
u
b
j
e
c
t
t
o
c
^
k
1
≤
ε
′
.They solve this constrained optimization problem by iteratively applying the pseudoobservation method presented in [19] to the estimate generated by the KF. According to this approach a pseudoobservation is generated using the constraint being applied to the estimate which is defined as(46)
z
^
k
=
c
^
k
1
-
ε
′
,where the constraining value ε
′ is now treated as a Gaussian noise on the pseudomeasurement with a covariance of P. Using the definition of ·
1 the pseudomeasurement can be written as(47)
z
^
k
=
H
¯
k
c
^
k
-
ε
′
,where the observation matrix of the pseudoobservation is defined as(48)
H
¯
=
s
i
g
n
c
^
k
1
⋯
s
i
g
n
c
^
k
n
,
s
i
g
n
x
=
1
x
≥
0
-
1
o
t
h
e
r
w
i
s
e
.The constraint is then iteratively appliedτ times to the estimate produced by the KF using the standard KF correction equations (41)–(43). First, a new Kalman matrix is generated using the observation matrix of the pseudomeasurement and the covariance of the constraint:(49)
K
¯
=
Σ
k
H
¯
k
T
H
¯
k
Σ
k
H
¯
k
T
+
P
-
1
.Next, the mean of the estimate is updated using the standard mean update equation with the assumption that the measured value is 0. Using this assumption the mean update equation can be simplified to(50)
c
^
k
=
I
-
K
¯
H
¯
c
^
k
.Finally, the covariance of the estimate is constrained using the standard covariance measurement update equation(51)
Σ
k
=
I
-
K
¯
H
¯
k
Σ
k
.The full algorithm that combines all of these separate components to reconstruct the compressed signal using the CSEKF is shown in Algorithm 5.Algorithm 5:Compresed sensing embedded Kalman filtering reconstruction algorithm.
(
1) procedure CSEKF(
c
^
k
-
1
,
Σ
k
-
1
,
A
k
,
R
k
,
H
k
,
Q
k
,
y
,
τ
,
P
)
(2)   c
^
k
←
A
k
c
^
k
-
1
(3)   Σ
k
←
A
k
Σ
k
-
1
A
k
T
+
R
k
(4)   K
=
Σ
k
H
T
(
H
k
Σ
k
H
k
T
+
Q
k
)
-
1
(5)   c
^
k
+
K
(
y
-
H
k
c
^
k
)
(6)   for  i
=
1, τ
-
1  do
(7)     H
-
=
sign
⁡
c
^
k
1
⋯
sign
⁡
c
^
k
n
(8)     K
-
=
Σ
k
H
-
T
(
H
-
Σ
k
H
-
T
+
P
)
-
1
(9)     c
^
k
=
(
I
-
K
-
H
-
)
c
^
k
(10)     Σ
k
=
(
I
-
K
-
H
-
)
Σ
k
(11)   end for
(12) end procedureThe reconstructed test function for the same four test percentages as in the previous two algorithms is shown in Figure4. The algorithm is implemented as shown in Algorithm 5 with the tuning parameters being chosen as Q
k
=
0.0
1
2, R
k
=
0, P
=
10
0
2, and τ
=
1000. Since the input vector is static the initial estimate of the coefficient vector was zero, c
^
k
-
1
=
0
n, the state transition matrix is chosen as A
k
=
I
n, and the initial covariance matrix of the estimate is chosen to be Σ
k
-
1
=
I
n.Figure 4
The test function reconstructed using the compressed sensing Kalman filter for compressed size of (a)25
%, (b) 50
%, (c) 75
%, and (d) 95
% of the original data.
(a)
(b)
(c)
(d)


### 4.4. Kalman Filter Constrained with Quasinorm (CSEKF-p)

In the previous section an approach was presented that solves a problem similar to the standard compressed sensing problem (36) using a KF that is constrained by iteratively applying a pseudoobservation to the estimate generated by the KF. The standard approach to solving the compressed sensing problem replaces ·
0 in (34) with ·
1. A new approach has been presented by the authors of [20] that replaces the zero norm with p-norm ·
p which for 0
<
p
<
1 is defined as(52)
x
p
=
∑
i
=
1
n
x
i
p
1
/
p
.This approach has been shown to yield better accuracy than using ·
1 in some cases. Using this approach, we would like to constrain the estimate produced by the KF with the quasinorm ·
p as opposed to ·
1.Just as in the previous section the initial estimate is produced using the standard KF equations. The pseudoobservation used to apply the constraint to the KF estimate using·
p is defined as(53)
z
^
k
=
c
^
k
p
-
ε
′
,where ε
′ is again treated as noise on the pseudoobservation with a covariance of P. Unlike in the previous case where the constraint was linear and could be applied using the pseudoobservation matrix, the constraint applied by p-norm is nonlinear and can be rewritten as(54)
z
^
k
=
h
c
^
k
-
ε
′
,where(55)
h
c
^
k
=
c
^
k
p
.Since the constraint using p-norm is nonlinear the constraint is applied to the KF estimate by iteratively performing a measurement update using (54) and the Extended Kalman Filter (EKF) [21] update equations. Just as in the linear case the first step in applying the constraint is to construct the Kalman matrix according to(56)
K
¯
=
Σ
k
H
¯
k
T
H
¯
k
Σ
k
H
¯
k
T
+
P
-
1
,where H
¯ is now the Jacobian of h
(
·
) and is defined as(57)
H
¯
i
=
∑
i
=
1
n
c
^
k
∗
i
p
1
/
p
-
1
c
^
k
∗
i
p
-
1
,
c
^
k
∗
i
≥
0
,
-
∑
i
=
1
n
c
^
k
∗
i
p
1
/
p
-
1
c
^
k
∗
i
p
-
1
,
c
^
k
∗
i
<
0
,where c
^
k
∗
=
c
^
k. The constraint is applied to the mean of the estimate using the standard EKF mean measurement update equation, again with the assumption that the measured value is zero, which when simplified is written as(58)
c
^
k
=
c
^
k
-
K
¯
c
^
k
p
.The final step in applying the constraint is to update the covariance of the estimate using the standard EKF covariance measurement update equation(59)
Σ
k
=
I
-
K
¯
H
¯
k
Σ
k
.The complete algorithm for reconstructing a compressed signal using the estimate produced by the KF constrained using p-norm is shown in Algorithm 6.Algorithm 6:Sparse signal reconstruction algorithm using the KF constrained using the quasinorm.
(
1) procedure CSEKFP(
c
^
k
-
1
,
Σ
k
-
1
,
A
k
,
R
k
,
H
k
,
Q
k
,
y
,
τ
,
P
,
p
)
(2)   c
^
k
←
A
k
c
^
k
-
1
(3)   Σ
k
←
A
k
Σ
k
-
1
A
k
T
+
R
k
(4)   K
=
Σ
k
H
T
(
H
k
Σ
k
H
k
T
+
Q
k
)
-
1
(5)   c
^
k
+
K
(
y
-
H
k
c
^
k
)
(6)   for  i
=
1, τ
-
1  do
(7)     for  j
=
1, n  do
(8)       if  c
^
k
(
j
)
≥
0  then
(9)        H
-
j
=
(
∑
i
=
1
n
‍
c
^
k
∗
(
i
)
p
)
1
/
(
p
-
1
)
c
^
k
∗
(
i
)
p
-
1
(10)       else
(11)         H
-
i
=
-
(
∑
i
=
1
n
‍
c
^
k
∗
(
i
)
p
)
1
/
(
p
-
1
)
c
^
k
∗
(
i
)
p
-
1
(12)       end if
(13)     end for
(14)     H
-
=
sign
⁡
c
^
k
1
⋯
sign
⁡
c
^
k
n
(15)     K
-
=
Σ
k
H
-
T
(
H
-
Σ
k
H
-
T
+
P
)
-
1
(16)     c
^
k
=
(
I
-
K
-
H
-
)
c
^
k
(17)     Σ
k
=
(
I
-
K
-
H
-
)
Σ
k
(18)   end for
(19) end procedureThe reconstruction results for the compressed sensing Kalman filter using a quasinorm to constrain the estimate are presented in Figure5 for the same four data sizes used for the three previous algorithms. A parameter that greatly effects the performance of the algorithm is the value chosen for p. To determine the best value of p the algorithm was run for a single data percentage, 75
%, for 20 equally spaced values of p
∈
0,1. A plot of the mean square error (MSE) between the compressed signal and original signal can be seen in Figure 6. From this graph the value of p was chosen to be 0.4. The remainder of the tuning parameters for the algorithm were selected to be the same as those used for the CSEKF reconstruction.Figure 5
The test function reconstructed using the quasinorm constrained Kalman filter for compressed size of (a)25
%, (b) 50
%, (c) 75
%, and (d) 95
% of the original data.
(a)
(b)
(c)
(d)Figure 6
MSE for different values ofp for a single compressed percentage 75
%.


### 4.5. Algorithm Comparison

Before deciding which reconstruction algorithm should be used in the FastSLAM COG algorithm we examine the performance of each of the algorithms in two key areas: accuracy and run time. First, we examine how well each algorithm reproduces the original signal based on the amount of information that is stored in the compressed signal. To compare how well each algorithm reconstructs the signal, each algorithm was run for20 equally spaced percentages in (
0
%
,
100
%
]. The results for each of the algorithms are shown in Figure 7 which plots the MSE as a function of the amount of data stored. As seen in the results, in general each of the compressed sensing type approaches produces less of an error for a given data percentage than l
2 approach.Figure 7
MSE as a function of signal data used for each of the four reconstruction algorithms.The second key characteristic of each algorithm examined is the time it takes for each algorithm to run. During the error performance testing described above, the time taken to run each algorithm was recorded. The run time as a function of the amount of compressed data used to represent the signal can be seen in Figure8. As seen in the timing results the two algorithms that use the augmented Kalman filter have run times that are significantly larger than the other approaches. This increased run time is due to the number of matrix-matrix multiplications required by the Kalman filter which are computationally heavy, from [22] having O
(
n
l
o
g
2
7
). The remaining two algorithms, l
2 and compressed sensing, can be implemented completely using only matrix-vector multiplications, which can be performed more quickly than matrix-matrix multiplications.Figure 8
Run time as a function of compressed percentage for each of the four reconstruction algorithms.


## 4.1.l
2 Reconstruction (l
2)

One of the most simple methods for reconstructing a compressed signal is to reconstruct the signal while minimizing the error between the original and reconstructed signals as measured byl
2 norm, where l
2 norm of a signal x
∈
R
n is defined as(26)
x
2
≜
∑
i
=
1
n
x
i
2
.In examining (25), if the size of the compressed signal is the same as the original then the reconstruction can be performed according to(27)
c
=
H
-
1
y
.In reality the system is underdetermined due to m
<
n. A common solution to problems of this form uses the Moore-Penrose pseudoinverse of H.The Moore-Penrose pseudoeinverse [11, 12] is used to find the solution that minimizes l
2 error for systems of the form(28)
A
x
=
b
,where x
∈
R
n, b
∈
R
m, and A
∈
R
m
×
n. The solution to the system found using the pseudoinverse is defined as(29)
z
=
A
†
b
,where A
† is the pseudoinverse of A and it minimizes l
2 error so that(30)
A
x
-
b
2
≥
A
z
-
b
2
,for all x
∈
R
n if x is defined as(31)
x
=
A
†
b
+
I
-
A
†
A
w
,for any vector w
∈
R
n.Using the pseudoinverse ofH a set of reconstructed coefficients is generated according to(32)
c
^
=
H
†
y
,and the reconstructed value of the signal is generated from(33)
x
^
=
Φ
c
^
.The results ofl
2 reconstruction using four separate amounts of signal information, 25
%, 50
%, 75
%, and 95
%, are presented in Figure 2. The form selected for H needed to obtain such level of compression is an orthonormal Gaussian random matrix as it possesses the RIP property whose importance will be discussed in the following section.Figure 2
The test function reconstructed by minimizingl
2 error for compressed size of (a) 25
%, (b) 50
%, (c) 75
%, and (d) 95
% of the original data.
(a)
(b)
(c)
(d)


## 4.2. Compressed Sensing Reconstruction (CS)

The second reconstruction algorithm selected is based on an approach referred to in the literature as compressed sensing [13]. Compressed sensing allows for sparse signals to be reconstructed using fewer measurements that would be required according to the Nyquist/Shannon sampling principle. In compressed sensing the full set of sparse coefficients can be reconstructed from the compressed set by solving(34)
m
i
n
c
^
0
s
u
b
j
e
c
t
t
o
y
-
H
c
^
2
2
≤
ε
, for some sufficiently small ε. In (34) c
^
0
≜
#
s
u
p
p
c
^ is a count of the number of nonzero elements in the vector c
^. Solving (34) is NP-hard and the solution cannot be found efficiently. A key finding of compressed sensing is that if the measurement matrix, H, is constructed in such a way that it obeys the Restricted Isometry Property (RIP), which is defined as(35)
1
-
δ
c
2
2
≤
H
c
2
2
≤
1
+
δ
c
2
2
, form some small δ
≥
0, then ·
0 in (34) can be replaced by ·
1 resulting in(36)
m
i
n
c
^
1
s
u
b
j
e
c
t
t
o
y
-
H
c
^
2
2
≤
ε
. As opposed to being NP-hard to solve, (36) is a constrained convex optimization problem and can be solved efficiently using one of several available software packages with Lasso [14], NESTA [15], and l1 magic [16] being some examples. As in l
2 case, once the complete set of coefficients have been estimated using the compressed set, an estimate of the original signal is generated according to (33).In order to examine the performance of the compressed sensing reconstruction approach, the test signal is reconstructed using the same four amounts of data,25
%, 50
%, 75
%, and 95
%, with the same orthonormal Gaussian random matrix being used to perform the compression. Equation (36) is solved using the NESTA software library and the results of the reconstruction are shown in Figure 3.Figure 3
The test function reconstructed by solving the compressed sensing convex optimization problem for compressed size of (a)25
%, (b) 50
%, (c) 75
%, and (d) 95
% of the original data.
(a)
(b)
(c)
(d)


## 4.3. Compressed Sensing Embedded Kalman Filtering (CSEKF)

In the previous section a reconstruction approach was introduced that allows for the reconstruction of compressed signals using less data than expected based on the Nyquist/Shannon sampling principle. As seen in Figure3 this method does a better job at reconstructing a sparse signal than the approach that reconstructs the signal by minimizing l
2 norm. The authors of [17] introduce an approach that performs the compressed sensing reconstructing using an augmented Kalman filter (KF) [18] which is easy to implement, compared to the compressed sensing approach. The compressed sensing approach must solve a complex convex minimization problem, where problems of this type are typically being solved using an external software package. In order to use the KF it is assumed that the full set of coefficients evolve from time step to time step according to(37)
c
k
=
A
k
c
k
-
1
+
ε
k
,where A
k
∈
R
n
×
n is the state transition matrix of the system which describes how the signal coefficients change between time steps, and ε
k
∈
R
n is a zero mean Gaussian vector with covariance R
k
≥
0. The compressed form of the coefficients is generated according to(38)
y
k
=
H
k
c
k
+
δ
k
,where H
k
∈
R
m
×
n is the same compression matrix discussed in (25) and δ
k
∈
R
m is a zero mean Gaussian vector with covariance Q
k
>
0.The CSEKF algorithm first performs a standard KF estimation that is carried out in a two parts, aprediction phase and acorrection phase. The KF estimates p
c
k
∣
y
k as a Gaussian distribution that can be represented using a mean vector c
^
k and covariance matrix Σ
k. The KF generates the estimate using a standard set of five equations [18]:(39)
c
^
¯
k
=
A
k
c
^
k
-
1
,
(40)
Σ
¯
k
=
A
k
Σ
k
-
1
A
k
T
+
R
k
,
(41)
K
k
=
Σ
¯
k
H
k
T
H
k
Σ
¯
k
H
k
T
+
Q
k
-
1
,
(42)
c
^
k
=
c
^
¯
k
+
K
k
y
k
-
H
k
c
^
¯
k
,
(43)
Σ
k
=
I
-
K
k
H
k
Σ
¯
k
,where I in (43) is m dimensional identity matrix. The estimate produced by the KF minimizes the error, using l
2 norm, between the estimate and the actual set of coefficients(44)
m
i
n
c
k
-
c
^
k
2
2
;however, this is not what we are trying to solve according to the compressed sensing approach. The CSEKF presented by the authors replaces the classic compressed sensing convex optimization problem (36) with the dual problem:(45)
m
i
n
c
k
-
c
^
k
2
2
s
u
b
j
e
c
t
t
o
c
^
k
1
≤
ε
′
.They solve this constrained optimization problem by iteratively applying the pseudoobservation method presented in [19] to the estimate generated by the KF. According to this approach a pseudoobservation is generated using the constraint being applied to the estimate which is defined as(46)
z
^
k
=
c
^
k
1
-
ε
′
,where the constraining value ε
′ is now treated as a Gaussian noise on the pseudomeasurement with a covariance of P. Using the definition of ·
1 the pseudomeasurement can be written as(47)
z
^
k
=
H
¯
k
c
^
k
-
ε
′
,where the observation matrix of the pseudoobservation is defined as(48)
H
¯
=
s
i
g
n
c
^
k
1
⋯
s
i
g
n
c
^
k
n
,
s
i
g
n
x
=
1
x
≥
0
-
1
o
t
h
e
r
w
i
s
e
.The constraint is then iteratively appliedτ times to the estimate produced by the KF using the standard KF correction equations (41)–(43). First, a new Kalman matrix is generated using the observation matrix of the pseudomeasurement and the covariance of the constraint:(49)
K
¯
=
Σ
k
H
¯
k
T
H
¯
k
Σ
k
H
¯
k
T
+
P
-
1
.Next, the mean of the estimate is updated using the standard mean update equation with the assumption that the measured value is 0. Using this assumption the mean update equation can be simplified to(50)
c
^
k
=
I
-
K
¯
H
¯
c
^
k
.Finally, the covariance of the estimate is constrained using the standard covariance measurement update equation(51)
Σ
k
=
I
-
K
¯
H
¯
k
Σ
k
.The full algorithm that combines all of these separate components to reconstruct the compressed signal using the CSEKF is shown in Algorithm 5.Algorithm 5:Compresed sensing embedded Kalman filtering reconstruction algorithm.
(
1) procedure CSEKF(
c
^
k
-
1
,
Σ
k
-
1
,
A
k
,
R
k
,
H
k
,
Q
k
,
y
,
τ
,
P
)
(2)   c
^
k
←
A
k
c
^
k
-
1
(3)   Σ
k
←
A
k
Σ
k
-
1
A
k
T
+
R
k
(4)   K
=
Σ
k
H
T
(
H
k
Σ
k
H
k
T
+
Q
k
)
-
1
(5)   c
^
k
+
K
(
y
-
H
k
c
^
k
)
(6)   for  i
=
1, τ
-
1  do
(7)     H
-
=
sign
⁡
c
^
k
1
⋯
sign
⁡
c
^
k
n
(8)     K
-
=
Σ
k
H
-
T
(
H
-
Σ
k
H
-
T
+
P
)
-
1
(9)     c
^
k
=
(
I
-
K
-
H
-
)
c
^
k
(10)     Σ
k
=
(
I
-
K
-
H
-
)
Σ
k
(11)   end for
(12) end procedureThe reconstructed test function for the same four test percentages as in the previous two algorithms is shown in Figure4. The algorithm is implemented as shown in Algorithm 5 with the tuning parameters being chosen as Q
k
=
0.0
1
2, R
k
=
0, P
=
10
0
2, and τ
=
1000. Since the input vector is static the initial estimate of the coefficient vector was zero, c
^
k
-
1
=
0
n, the state transition matrix is chosen as A
k
=
I
n, and the initial covariance matrix of the estimate is chosen to be Σ
k
-
1
=
I
n.Figure 4
The test function reconstructed using the compressed sensing Kalman filter for compressed size of (a)25
%, (b) 50
%, (c) 75
%, and (d) 95
% of the original data.
(a)
(b)
(c)
(d)


## 4.4. Kalman Filter Constrained with Quasinorm (CSEKF-p)

In the previous section an approach was presented that solves a problem similar to the standard compressed sensing problem (36) using a KF that is constrained by iteratively applying a pseudoobservation to the estimate generated by the KF. The standard approach to solving the compressed sensing problem replaces ·
0 in (34) with ·
1. A new approach has been presented by the authors of [20] that replaces the zero norm with p-norm ·
p which for 0
<
p
<
1 is defined as(52)
x
p
=
∑
i
=
1
n
x
i
p
1
/
p
.This approach has been shown to yield better accuracy than using ·
1 in some cases. Using this approach, we would like to constrain the estimate produced by the KF with the quasinorm ·
p as opposed to ·
1.Just as in the previous section the initial estimate is produced using the standard KF equations. The pseudoobservation used to apply the constraint to the KF estimate using·
p is defined as(53)
z
^
k
=
c
^
k
p
-
ε
′
,where ε
′ is again treated as noise on the pseudoobservation with a covariance of P. Unlike in the previous case where the constraint was linear and could be applied using the pseudoobservation matrix, the constraint applied by p-norm is nonlinear and can be rewritten as(54)
z
^
k
=
h
c
^
k
-
ε
′
,where(55)
h
c
^
k
=
c
^
k
p
.Since the constraint using p-norm is nonlinear the constraint is applied to the KF estimate by iteratively performing a measurement update using (54) and the Extended Kalman Filter (EKF) [21] update equations. Just as in the linear case the first step in applying the constraint is to construct the Kalman matrix according to(56)
K
¯
=
Σ
k
H
¯
k
T
H
¯
k
Σ
k
H
¯
k
T
+
P
-
1
,where H
¯ is now the Jacobian of h
(
·
) and is defined as(57)
H
¯
i
=
∑
i
=
1
n
c
^
k
∗
i
p
1
/
p
-
1
c
^
k
∗
i
p
-
1
,
c
^
k
∗
i
≥
0
,
-
∑
i
=
1
n
c
^
k
∗
i
p
1
/
p
-
1
c
^
k
∗
i
p
-
1
,
c
^
k
∗
i
<
0
,where c
^
k
∗
=
c
^
k. The constraint is applied to the mean of the estimate using the standard EKF mean measurement update equation, again with the assumption that the measured value is zero, which when simplified is written as(58)
c
^
k
=
c
^
k
-
K
¯
c
^
k
p
.The final step in applying the constraint is to update the covariance of the estimate using the standard EKF covariance measurement update equation(59)
Σ
k
=
I
-
K
¯
H
¯
k
Σ
k
.The complete algorithm for reconstructing a compressed signal using the estimate produced by the KF constrained using p-norm is shown in Algorithm 6.Algorithm 6:Sparse signal reconstruction algorithm using the KF constrained using the quasinorm.
(
1) procedure CSEKFP(
c
^
k
-
1
,
Σ
k
-
1
,
A
k
,
R
k
,
H
k
,
Q
k
,
y
,
τ
,
P
,
p
)
(2)   c
^
k
←
A
k
c
^
k
-
1
(3)   Σ
k
←
A
k
Σ
k
-
1
A
k
T
+
R
k
(4)   K
=
Σ
k
H
T
(
H
k
Σ
k
H
k
T
+
Q
k
)
-
1
(5)   c
^
k
+
K
(
y
-
H
k
c
^
k
)
(6)   for  i
=
1, τ
-
1  do
(7)     for  j
=
1, n  do
(8)       if  c
^
k
(
j
)
≥
0  then
(9)        H
-
j
=
(
∑
i
=
1
n
‍
c
^
k
∗
(
i
)
p
)
1
/
(
p
-
1
)
c
^
k
∗
(
i
)
p
-
1
(10)       else
(11)         H
-
i
=
-
(
∑
i
=
1
n
‍
c
^
k
∗
(
i
)
p
)
1
/
(
p
-
1
)
c
^
k
∗
(
i
)
p
-
1
(12)       end if
(13)     end for
(14)     H
-
=
sign
⁡
c
^
k
1
⋯
sign
⁡
c
^
k
n
(15)     K
-
=
Σ
k
H
-
T
(
H
-
Σ
k
H
-
T
+
P
)
-
1
(16)     c
^
k
=
(
I
-
K
-
H
-
)
c
^
k
(17)     Σ
k
=
(
I
-
K
-
H
-
)
Σ
k
(18)   end for
(19) end procedureThe reconstruction results for the compressed sensing Kalman filter using a quasinorm to constrain the estimate are presented in Figure5 for the same four data sizes used for the three previous algorithms. A parameter that greatly effects the performance of the algorithm is the value chosen for p. To determine the best value of p the algorithm was run for a single data percentage, 75
%, for 20 equally spaced values of p
∈
0,1. A plot of the mean square error (MSE) between the compressed signal and original signal can be seen in Figure 6. From this graph the value of p was chosen to be 0.4. The remainder of the tuning parameters for the algorithm were selected to be the same as those used for the CSEKF reconstruction.Figure 5
The test function reconstructed using the quasinorm constrained Kalman filter for compressed size of (a)25
%, (b) 50
%, (c) 75
%, and (d) 95
% of the original data.
(a)
(b)
(c)
(d)Figure 6
MSE for different values ofp for a single compressed percentage 75
%.


## 4.5. Algorithm Comparison

Before deciding which reconstruction algorithm should be used in the FastSLAM COG algorithm we examine the performance of each of the algorithms in two key areas: accuracy and run time. First, we examine how well each algorithm reproduces the original signal based on the amount of information that is stored in the compressed signal. To compare how well each algorithm reconstructs the signal, each algorithm was run for20 equally spaced percentages in (
0
%
,
100
%
]. The results for each of the algorithms are shown in Figure 7 which plots the MSE as a function of the amount of data stored. As seen in the results, in general each of the compressed sensing type approaches produces less of an error for a given data percentage than l
2 approach.Figure 7
MSE as a function of signal data used for each of the four reconstruction algorithms.The second key characteristic of each algorithm examined is the time it takes for each algorithm to run. During the error performance testing described above, the time taken to run each algorithm was recorded. The run time as a function of the amount of compressed data used to represent the signal can be seen in Figure8. As seen in the timing results the two algorithms that use the augmented Kalman filter have run times that are significantly larger than the other approaches. This increased run time is due to the number of matrix-matrix multiplications required by the Kalman filter which are computationally heavy, from [22] having O
(
n
l
o
g
2
7
). The remaining two algorithms, l
2 and compressed sensing, can be implemented completely using only matrix-vector multiplications, which can be performed more quickly than matrix-matrix multiplications.Figure 8
Run time as a function of compressed percentage for each of the four reconstruction algorithms.


## 5. Performance Improvements

Our overall goal is to augment the FastSLAM algorithm to use compressed occupancy grids. In order for us to accomplish this task the reconstruction approach selected must run thousands or hundreds of thousands of times during the operation of a UMV. By using a Rao-Blackwellized particle filter, we can increase the probability that our SLAM posterior estimate matches the true SLAM posterior by increasing the number of particles maintained by the particle filter. As seen in Algorithm3 the reconstruction process must be performed by each particle at each iteration; thus the execution time of the reconstruction process is important in the overall performance of the algorithm.In Section4 four different algorithms were presented that can be used to reconstruct an occupancy grid from the compressed form. From the timing results (Figure 8), the two approaches that make use of the Kalman filter (CSEKF and CSEKF-p) take much longer time to run than l
2 and CS. The longer run times of the Kalman filter based approaches are due to the large number of matrix-matrix multiplications. Based on their performance the CSEKF and CSEKF-p algorithms will not be used in FastSLAM COG. We will now present a method that can be used to improve the accuracy of the remaining two algorithms and a method that improves the run time of l
2 approach.


### 5.1. Updated Compression Step

As seen in Section3.2 the compressed occupancy grid is generated by selecting a subset of the information stored in the coefficients. This compression is performed according to (15) which makes use of H to perform the compression. As stated in Section 4.2 we would like for this compression matrix to obey the RIP property. Several common matrices that are used in compressed sensing applications are the discrete Fourier transform [23], the discrete cosine transform [10], the Hadamard transform [24], and noiselet transform [25]. Each of these matrices is constructed in such a way that each of the elements stored in the compressed vector stores a small amount of information about each of the coefficients used to represent the original signal and this information can be recovered by knowing H. These types of matrices are useful when there is no knowledge about the sparse signal that is being compressed; however we are constructing the sparse signal thus we have full knowledge of the signal before the compression is performed. Using this knowledge we present an alternative form of H that allows for better reconstruction using a smaller amount of information. The form of H selected comes from the definition of the square error defined according to l
2 norm. For some discrete signal x
∈
R
n the square of the error between the signal and the approximation of the signal x
^ generated using m
<
n elements is defined as(60)
x
-
x
^
2
2
=
x
-
x
^
∣
x
-
x
^
,where ·
∣
· is the standard inner product. The discrete signal x
∈
R
n can be represented in some alternate basis so that a single element can be expressed as(61)
x
i
=
∑
j
=
1
n
Φ
i
,
j
c
j
.Recalling that y
^
∈
R
m denotes the compressed set of coefficients (25), a single element of the compressed signal can be expressed in the alternate basis as(62)
x
^
i
=
∑
j
=
1
m
Φ
^
i
,
j
c
^
j
,where(63)
Φ
^
=
Φ
H
+
,
c
^
=
y
.While this is a valid approach for compressing a signal, we would like to simplify the compression process for performance reasons. Instead of generating a new set of basis function it is easier to keep the basis functions the same at all times and just select a subset of the coefficients to represent the signal. In order to decide which coefficients should be selected that minimize the error based onl
2 norm, we define several sets. The set ζ
=
x
∣
x
∈
N
,
1
≤
x
≤
n is the full set of indices of the discrete vector being compressed. The set λ
∈
N
m is the set of indices that are used to represent the compressed signal, λ
⊂
ζ. Finally, the set γ
∈
N
n
-
m is the set of indices of coefficients not used in the compressed signal, γ
=
x
∣
x
∈
ζ
a
n
d
x
∉
λ. Using these sets and the fact that a constant basis is used to represent the uncompressed and compressed signal an element in the reconstructed signal can be written as(64)
x
^
i
=
∑
j
=
1
m
Φ
i
,
λ
j
c
λ
j
.The square of the error in l
2 norm for a single element becomes(65)
x
i
-
x
^
i
2
2
=
x
i
-
x
^
i
∣
x
i
-
x
^
i
=
∑
j
=
1
n
-
m
Φ
i
,
γ
j
c
γ
j
∣
∑
k
=
1
n
-
m
Φ
i
,
γ
k
c
γ
k
=
∑
j
=
1
n
-
m
∑
k
=
1
n
-
m
c
γ
j
c
γ
k
×
Φ
i
,
γ
j
∣
Φ
i
,
γ
k
=
∑
j
=
1
n
-
m
Φ
i
,
γ
j
2
c
γ
j
2
,and if we assume that the set of basis function is orthonormal, which is common, then l
2 error for the entire compressed signal becomes(66)
x
-
x
^
2
2
=
∑
i
=
1
n
∑
j
=
1
n
-
m
c
γ
j
2
.This tells us that the best way to minimize the error in the reconstructed signal is to select the coefficients that have the largest magnitudes. Using this knowledge a new compression algorithm that does not require matrix-vector multiplications and selects only m coefficients with the largest magnitudes was developed and shown in Algorithm 7.Algorithm 7:Updated compression algorithm that represents the compressed occupancy grids with a set of coefficients and corresponding indices.
(1) procedure Fa
stCompression
(
c
,
m
)
(2)   x = SORT(abs(c), “descend”)
(3)   for  i
=
1, m  do
(4)     i
(
i
)
=
x
(
i
)
(5)     y
(
i
)
=
c
(
x
(
i
)
)
(6)   end for
(7) end procedureTo examine how the use of the new compression algorithms affects the performance of the reconstruction algorithms, the occupancy grid shown in Figure9(a) is reconstructed from the compressed form. The compression matrix selected for the baseline results is different from the orthonormal Gaussian random matrix used in Section 4; in its place a normalized Hadamard matrix was used. Each of the two algorithms was then run using each of the two compression methods for 20 separate compression percentages in 0,100
%. The performance results of each of the two algorithms are provided in Figure 10. As seen from the results for each of the two reconstruction algorithms the occupancy grid is reconstructed with a smaller MSE using the compression matrix that keeps m most significant coefficients as opposed to the compression step using the normalized Hadamard matrix using the same amount of data.Figure 9
An example occupancy grid for an unknown environment (a) and the set of coefficients used to represent the occupancy grid when represented using the Haar wavelet basis (b).
(a)
(b)Figure 10
Performance results of each of the reconstruction methods using a normalized Hadamard matrix and a selection matrix to keepm most significant coefficients.


### 5.2. Updatedl
2 Reconstruction

In Section4.1 a method for reconstructing a compressed signal was introduced that minimizes the error according to l
2 norm. The method makes uses of the Moore-Penrose pseudoinverse to reconstruct the set of coefficients from their compressed form according to(67)
c
^
=
H
†
y
.This method of reconstructing the signal is made up of two steps where the performance of each step is based on the size of the signal (n). The first step in the process is the calculation of the pseudoinverse of the compression matrix H. Many common software implementations of this calculation, for example, Matlab’s p
i
n
v
(
) function [26], makes use of the Singular Value Decomposition (SVD) of the matrix. The process of calculating the SVD of a matrix is worse from a performance perspective than that of matrix-matrix multiplication which is O
(
m
n
2
) [27] for m
×
n matrix.The second step of the algorithm is a matrix-vector multiplication operation which in a naive implementation has a performance ofO
(
n
2
). We would like to improve the performance of the reconstruction algorithm to get rid of these computationally intensive steps. By making use of the compression matrix developed in the previous section that just selects m coefficients with the largest magnitudes, the performance of the reconstruction process can be improved. Each of the rows in the compression matrix H is orthogonal; that is, for two rows in the selection matrices H
(
i
,
:
) and H
(
j
,
:
) the discrete inner product is zero; H
(
i
,
:
)
∣
H
(
j
,
:
)
=
0. A second property is that each row of H is normalized; ∑
j
=
1
n
H
i
,
j
=
1; thus H is orthonormal. The pseudoinverse of a orthonormal matrix can be easily computed without the computationally intensive SVD process according to(68)
H
†
=
H
∗
, where H
∗ is the conjugate transpose of H and is found by taking the transpose of H followed by the complex conjugate of each element in H. Since the compression matrix is composed of elements containing just 0 or 1 the pseudoinverse is just the transpose H
†
=
H
T.Finally, in (67) we can replace the pseudoinverse of H with the transpose; thus the reconstruction process becomes(69)
c
^
=
H
T
y
.We can improve the performance even more if we assume that the compression is performed as discussed above, that is, where m coefficients with the largest magnitudes are selected. In that case then H is full of zeros except for elements that correspond to the coefficients with largest magnitudes. Using this information we simplify l
2 reconstruction algorithm to just generate a vector of zeros and then copy the stored coefficients into their proper locations in the reconstructed vector. This replaces the matrix-vector multiplication with m value copies. The updated form of l
2 reconstruction algorithm that does not require the potentially expensive calculation of the matrix pseudoinverse or matrix-vector multiplication is presented in Algorithm 8.Algorithm 8:Updated form of l
2 reconstruction algorithm that is performed without needing to calculate the pseudoinverse of the compression matrix or perform the matrix-vector multiplication.
(1) procedure  Fa
stLTWOReconstruction
(
c
,
m
)
(2)  for  i
=
1, n  do
(3)    c
(
i
)
=
0
(4)  end for
(5)  for  i
=
1, m  do
(6)    c
(
i
(
i
)
)
=
y
(
i
)
(7)  end for
(8) end procedureTo examine the performance benefit of the newl
2 reconstruction algorithm a compressed form of the occupancy grid seen in Figure 11 is reconstructed using the original algorithm explained in Section 4.1 and the new algorithm described in Algorithm 8. The algorithm is run for x varying compressed data sizes in 0
%
,
100
% and the timing results are seen in Figure 11. As seen from the results the run time for the new l
2 reconstruction remains small as the size of the compressed signal grows while the original l
2 reconstruction approach slows down as the size of the compressed signal grows.Figure 11
The comparison in run time for the originall
2 reconstruction along with the updated form of the reconstruction algorithm.


## 5.1. Updated Compression Step

As seen in Section3.2 the compressed occupancy grid is generated by selecting a subset of the information stored in the coefficients. This compression is performed according to (15) which makes use of H to perform the compression. As stated in Section 4.2 we would like for this compression matrix to obey the RIP property. Several common matrices that are used in compressed sensing applications are the discrete Fourier transform [23], the discrete cosine transform [10], the Hadamard transform [24], and noiselet transform [25]. Each of these matrices is constructed in such a way that each of the elements stored in the compressed vector stores a small amount of information about each of the coefficients used to represent the original signal and this information can be recovered by knowing H. These types of matrices are useful when there is no knowledge about the sparse signal that is being compressed; however we are constructing the sparse signal thus we have full knowledge of the signal before the compression is performed. Using this knowledge we present an alternative form of H that allows for better reconstruction using a smaller amount of information. The form of H selected comes from the definition of the square error defined according to l
2 norm. For some discrete signal x
∈
R
n the square of the error between the signal and the approximation of the signal x
^ generated using m
<
n elements is defined as(60)
x
-
x
^
2
2
=
x
-
x
^
∣
x
-
x
^
,where ·
∣
· is the standard inner product. The discrete signal x
∈
R
n can be represented in some alternate basis so that a single element can be expressed as(61)
x
i
=
∑
j
=
1
n
Φ
i
,
j
c
j
.Recalling that y
^
∈
R
m denotes the compressed set of coefficients (25), a single element of the compressed signal can be expressed in the alternate basis as(62)
x
^
i
=
∑
j
=
1
m
Φ
^
i
,
j
c
^
j
,where(63)
Φ
^
=
Φ
H
+
,
c
^
=
y
.While this is a valid approach for compressing a signal, we would like to simplify the compression process for performance reasons. Instead of generating a new set of basis function it is easier to keep the basis functions the same at all times and just select a subset of the coefficients to represent the signal. In order to decide which coefficients should be selected that minimize the error based onl
2 norm, we define several sets. The set ζ
=
x
∣
x
∈
N
,
1
≤
x
≤
n is the full set of indices of the discrete vector being compressed. The set λ
∈
N
m is the set of indices that are used to represent the compressed signal, λ
⊂
ζ. Finally, the set γ
∈
N
n
-
m is the set of indices of coefficients not used in the compressed signal, γ
=
x
∣
x
∈
ζ
a
n
d
x
∉
λ. Using these sets and the fact that a constant basis is used to represent the uncompressed and compressed signal an element in the reconstructed signal can be written as(64)
x
^
i
=
∑
j
=
1
m
Φ
i
,
λ
j
c
λ
j
.The square of the error in l
2 norm for a single element becomes(65)
x
i
-
x
^
i
2
2
=
x
i
-
x
^
i
∣
x
i
-
x
^
i
=
∑
j
=
1
n
-
m
Φ
i
,
γ
j
c
γ
j
∣
∑
k
=
1
n
-
m
Φ
i
,
γ
k
c
γ
k
=
∑
j
=
1
n
-
m
∑
k
=
1
n
-
m
c
γ
j
c
γ
k
×
Φ
i
,
γ
j
∣
Φ
i
,
γ
k
=
∑
j
=
1
n
-
m
Φ
i
,
γ
j
2
c
γ
j
2
,and if we assume that the set of basis function is orthonormal, which is common, then l
2 error for the entire compressed signal becomes(66)
x
-
x
^
2
2
=
∑
i
=
1
n
∑
j
=
1
n
-
m
c
γ
j
2
.This tells us that the best way to minimize the error in the reconstructed signal is to select the coefficients that have the largest magnitudes. Using this knowledge a new compression algorithm that does not require matrix-vector multiplications and selects only m coefficients with the largest magnitudes was developed and shown in Algorithm 7.Algorithm 7:Updated compression algorithm that represents the compressed occupancy grids with a set of coefficients and corresponding indices.
(1) procedure Fa
stCompression
(
c
,
m
)
(2)   x = SORT(abs(c), “descend”)
(3)   for  i
=
1, m  do
(4)     i
(
i
)
=
x
(
i
)
(5)     y
(
i
)
=
c
(
x
(
i
)
)
(6)   end for
(7) end procedureTo examine how the use of the new compression algorithms affects the performance of the reconstruction algorithms, the occupancy grid shown in Figure9(a) is reconstructed from the compressed form. The compression matrix selected for the baseline results is different from the orthonormal Gaussian random matrix used in Section 4; in its place a normalized Hadamard matrix was used. Each of the two algorithms was then run using each of the two compression methods for 20 separate compression percentages in 0,100
%. The performance results of each of the two algorithms are provided in Figure 10. As seen from the results for each of the two reconstruction algorithms the occupancy grid is reconstructed with a smaller MSE using the compression matrix that keeps m most significant coefficients as opposed to the compression step using the normalized Hadamard matrix using the same amount of data.Figure 9
An example occupancy grid for an unknown environment (a) and the set of coefficients used to represent the occupancy grid when represented using the Haar wavelet basis (b).
(a)
(b)Figure 10
Performance results of each of the reconstruction methods using a normalized Hadamard matrix and a selection matrix to keepm most significant coefficients.


## 5.2. Updatedl
2 Reconstruction

In Section4.1 a method for reconstructing a compressed signal was introduced that minimizes the error according to l
2 norm. The method makes uses of the Moore-Penrose pseudoinverse to reconstruct the set of coefficients from their compressed form according to(67)
c
^
=
H
†
y
.This method of reconstructing the signal is made up of two steps where the performance of each step is based on the size of the signal (n). The first step in the process is the calculation of the pseudoinverse of the compression matrix H. Many common software implementations of this calculation, for example, Matlab’s p
i
n
v
(
) function [26], makes use of the Singular Value Decomposition (SVD) of the matrix. The process of calculating the SVD of a matrix is worse from a performance perspective than that of matrix-matrix multiplication which is O
(
m
n
2
) [27] for m
×
n matrix.The second step of the algorithm is a matrix-vector multiplication operation which in a naive implementation has a performance ofO
(
n
2
). We would like to improve the performance of the reconstruction algorithm to get rid of these computationally intensive steps. By making use of the compression matrix developed in the previous section that just selects m coefficients with the largest magnitudes, the performance of the reconstruction process can be improved. Each of the rows in the compression matrix H is orthogonal; that is, for two rows in the selection matrices H
(
i
,
:
) and H
(
j
,
:
) the discrete inner product is zero; H
(
i
,
:
)
∣
H
(
j
,
:
)
=
0. A second property is that each row of H is normalized; ∑
j
=
1
n
H
i
,
j
=
1; thus H is orthonormal. The pseudoinverse of a orthonormal matrix can be easily computed without the computationally intensive SVD process according to(68)
H
†
=
H
∗
, where H
∗ is the conjugate transpose of H and is found by taking the transpose of H followed by the complex conjugate of each element in H. Since the compression matrix is composed of elements containing just 0 or 1 the pseudoinverse is just the transpose H
†
=
H
T.Finally, in (67) we can replace the pseudoinverse of H with the transpose; thus the reconstruction process becomes(69)
c
^
=
H
T
y
.We can improve the performance even more if we assume that the compression is performed as discussed above, that is, where m coefficients with the largest magnitudes are selected. In that case then H is full of zeros except for elements that correspond to the coefficients with largest magnitudes. Using this information we simplify l
2 reconstruction algorithm to just generate a vector of zeros and then copy the stored coefficients into their proper locations in the reconstructed vector. This replaces the matrix-vector multiplication with m value copies. The updated form of l
2 reconstruction algorithm that does not require the potentially expensive calculation of the matrix pseudoinverse or matrix-vector multiplication is presented in Algorithm 8.Algorithm 8:Updated form of l
2 reconstruction algorithm that is performed without needing to calculate the pseudoinverse of the compression matrix or perform the matrix-vector multiplication.
(1) procedure  Fa
stLTWOReconstruction
(
c
,
m
)
(2)  for  i
=
1, n  do
(3)    c
(
i
)
=
0
(4)  end for
(5)  for  i
=
1, m  do
(6)    c
(
i
(
i
)
)
=
y
(
i
)
(7)  end for
(8) end procedureTo examine the performance benefit of the newl
2 reconstruction algorithm a compressed form of the occupancy grid seen in Figure 11 is reconstructed using the original algorithm explained in Section 4.1 and the new algorithm described in Algorithm 8. The algorithm is run for x varying compressed data sizes in 0
%
,
100
% and the timing results are seen in Figure 11. As seen from the results the run time for the new l
2 reconstruction remains small as the size of the compressed signal grows while the original l
2 reconstruction approach slows down as the size of the compressed signal grows.Figure 11
The comparison in run time for the originall
2 reconstruction along with the updated form of the reconstruction algorithm.


## 6. Experimental Results

We would now like to examine how using compressed occupancy grids affects the FastSLAM OG algorithm. Based on the timing results presented in Section4 only two of the uncompression methods, l
2 (Section 4.1) and CS (Section 4.2), were selected to use in the FastSLAM COG algorithm. Both of the algorithms are compared using data captured by a small unmanned ground vehicle (UGV). The UGV is equipped with Mecanum wheels [28] which allow the vehicle to move in any direction without the constraints that are typical with many ground vehicles. The use of Mecanum wheels makes it difficult to use standard wheel encoders for localization purposes; to overcome this a downward facing camera is attached to the UGV which provides visual odometry data for localization using the approach described in [29]. Other sensors attached to the UGV include a digital compass to provide the global heading of the vehicle and a Hokuyo UTM-30LX LiDAR sensor, with reported accuracy of 0.1 to 10m ± 30 mm and 10 to 30m ± 50 mm, to provide range and bearing measurements to objects in the environment. In order to provide a “ground truth” trajectory for our test, a Hagisonic StarGazer indoor localization system was used. This sensor uses an upward facing camera and static landmarks placed on the ceiling to triangulate a robotic vehicle in an environment. This sensor has a precision between 1mm and 10mm based on how many landmarks the sensor can see at a given time. This accuracy was much higher than the visual odometry algorithm that provided the odometry input to the SLAM algorithm; thus this trajectory was used to compare the effect of using compressed occupancy grids in our FastSLAM algorithm as opposed to the full occupancy grid.For comparison purposes, the standard FastSLAM OG algorithm described in Algorithm1 was used to generate a baseline vehicle path and occupancy grid map. Once the baseline was generated the FastSLAM COG algorithm described in Algorithm 3 was run using the same data used by the standard approach. For each of the two reconstruction algorithms the compression method used is the optimized approach described in Section 5.1 and l
2 approach is implemented as described in Section 5.2. For the CS reconstruction algorithm the convex optimization problem is solved using the NESTA library.The UGV was remotely driven around a small enclosed 10 ft × 12 ft environment while all of the sensor data was logged to memory on a small attached computer. The data was then postprocessed to produce a baseline vehicle path and an occupancy grid representing the testing environment. The baseline vehicle path, along with the true vehicle path, can be seen in Figure12(a) and the occupancy grid with a 0.1 m cell size is shown in Figure 12(b). The environment used to generate the baseline shown in Figures 12(a) and 12(b) is a simple rectangular indoor environment. Our choice of this simple environment, as opposed to a more complex environment, was made for two reasons. First, in order to properly compare how the use of compressed occupancy grids affected the localization and mapping an accurate baseline was required for comparison purposes. Based on the sensors that we had at our disposal, an indoor testing was more ideally suited for this task. Secondly, the goal of this research was not to examine how well an occupancy grid based SLAM algorithm performs in complex environment that was beyond our scope and addressed by others; rather our goal was to examine how the performance of the approach was affected by compressing the occupancy grid used to represent the environment, thus testing in complex environment was not attempted for this initial research.Figure 12
The baseline path generated by the FastSLAM algorithm with occupancy grids algorithm along with the true path of the vehicle (a) and the occupancy grid generated by the algorithm that represents the environment in which the vehicle is operating (b).
(a)
(b)The vehicle path estimate generated by FastSLAM COG algorithm usingl
2 reconstruction can be seen in Figure 13(a) and the reconstructed occupancy grid is shown in Figure 13(b). The generated path estimate using compressed sensing reconstruction approach is shown in Figure 14(a) and the reconstructed occupancy grid is shown in Figure 14(b). For each of the algorithms 40
% of the amount of data used to represent the full occupancy grid is used. Figure 15 shows the mean square error (MSE) in the path estimate and reconstructed occupancy grid between the uncompressed form of the algorithm and the compressed form of the algorithm. As Figure 15 shows using more than 40
% of the data does not decrease the MSE any more than the error that exists when using 40
% of the data.Figure 13
The path generated by the FastSLAM COG algorithm along withl
2 reconstruction method using 40
% of the original data (a) and the reconstructed occupancy grid generated by the algorithm (b).
(a)
(b)Figure 14
The path generated by the FastSLAM COG algorithm using40
% of the original data (a) and the reconstructed occupancy grid generated by the algorithm (b).
(a)
(b)Figure 15
The MSE between the path generated by the original algorithm and that generated by the compressed version as a function of the amount of data stored (a) and the MSE between the occupancy grid generated by the original algorithm and that generated using the compressed form as a function of the amount of data stored (b).
(a)
(b)The plots provided in Figure16 illustrate how a decrease in the amount of stored data decreases the quality of the reconstructed occupancy grid. This set of plots shows a series of occupancy grids that were generated as the final map of by the FastSLAM COG algorithm using l
2 reconstruction approach for decreasing amounts of data. As can be seen from the first several grids, up to 40
% of the data show no decrease in quality; however as the amount of data used continues to decrease the amount of error in the final occupancy grid increases until the grid becomes unusable. A similar set of plots are provided in Figure 17 for the FastSLAM COG algorithm using the compressed sensing reconstruction approach and similar results can be seen.Figure 16
A series of final occupancy grids generated by the FastSLAM COG algorithm usingl
2 reconstruction approach using 75
% (a), 70
% (b), 60
% (c), 50
% (d), 40
% (e), 30
% (f), 25
% (g), 20
% (h), and 10
% (i) of the full amount of data.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)Figure 17
A series of final occupancy grids generated by the FastSLAM COG algorithm using the compressed sensing reconstruction approach using75
% (a), 70
% (b), 60
% (c), 50
% (d), 40
% (e), 30
% (f), 25
% (g), 20
% (h), and 10
% (i) of the full amount of data.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)By examining Figure15 it can be seen that there is a constant error that appears in both the path estimate and the occupancy grid reconstruction from using the compressed sensing approach. This error comes from the parameters that are chosen for the NESTA library. Of most significance is the value of ε from (36) which is the amount of error that is allowed between the coefficients that represent the compressed form of the occupancy grid and the value of those coefficients in the reconstructed occupancy grid. For the presented results the value of ε was chosen to be 1
0
-
10 which is quite small; however it does allow for small errors to occur in the reconstructed occupancy grid. These errors can be reduced or removed completely, from the estimate by choosing a smaller value of ε and adjusting the parameters used by the NESTA library. However, as previously discussed, the accuracy of the grid reconstruction is not the only property of importance to us when deciding which approach should be used for storing and reconstructing the compressed occupancy grid. The other significant property of each algorithm that must be investigated is the time it takes to execute the algorithm. A comparison of the average time it takes to run a single iteration of the FastSLAM COG algorithm using l
2 approach and the compressed sensing approach for varying amounts of data is shown in Figure 18. As seen in these results the average run time of the FastSLAM COG algorithm using the compressed sensing approach is significantly longer at each compression level than the algorithm using l
2 reconstruction approach.Figure 18
The average time to complete a single iteration of the FastSLAM COG algorithm usingl
2 reconstruction approach and the compressed sensing reconstruction approach for varying amounts of data stored.


### 6.1. How Much Compression Can Be Achieved?

There are two key components that affect how much an occupancy grid can be compressed; first is the complexity of the environment in which the UMV is operating. If the environment is simple, such as a rectangular room, then a relatively small amount of data is needed to represent it with respect to a more complex environment. The second component that affects how much an occupancy grid can be compressed is the cell size used. If a large cell size is selected then there are fewer cells to represent the environment compared to when a smaller cell size is selected. It follows that when a larger cell size is selected a larger percentage of the cells must be used to store the information about the environment, even for simple environments so the grid cannot be compressed as much. To illustrate these two components, three separate environments of increasing complexity where generated and a compressed occupancy grid was generated for each environment using four separate cell sizes (1.0 m, 0.5 m, 0.2 m, and 0.1 m) and varying amounts of compression. The first environment is a simple rectangular room and an occupancy grid for this environment generated using the full set of data with the smallest cell size is shown in Figure19. The MSE as function of data stored for this environment is shown in Figure 20 for each of the four cell sizes. As expected the environment with the largest cell size, when fewer coefficients are needed to store all of the environment information, has the largest error when a small amount of data is used to represent the environment. It can also can be seen that, for each of the four cell sizes, as more information is stored, the error in the reconstructed form of the occupancy grid decreases.Figure 19
Occupancy grid of simple rectangular environment using100
% of the data and a cell size of 0.1 m.Figure 20
MSE between the true occupancy grid of the simple environment and the reconstructed form of the grid as a function of percent of data stored for each of the four cell sizes.A second environment with more complexity is shown in Figure21 which is a rectangular room with complex corners. The MSE between the true occupancy grid and the reconstructed grid as a function of the amount of data stored is shown in Figure 22 for each of the cell sizes. The overall behavior of the error is the same as in the simple case with the largest cell size having the largest error when small amounts of data are used. However, because of the added complexity in this environment, the errors at the lower data percentages are larger than the simple case which is what we expected because more information is required to store the more complex environment.Figure 21
Occupancy grid of rectangular environment with complex corners using100
% of the data and a cell size of 0.1 m.Figure 22
MSE between the true occupancy grid of the environment with medium complexity and the reconstructed form of the grid as a function of percent of data stored for each of the four cell sizes.Finally, a complex environment of a rectangular room with complex corners along with rectangular objects located throughout the rectangular room is shown in Figure23. As with the previous two environments the MSE between the true occupancy grid and the reconstructed occupancy as a function of data is shown in Figure 24 for each cell size. As expected the use of larger cell sizes causes more errors in the reconstructed form of the grid when small amounts of data are used. Also, the additional complexity causes the errors at lower data percent to be larger than in the more simple environments. It can be seen from the presented results that there appears to be a “magic” compression level of 40
% at which, no matter the complexity, the compressed occupancy can be reconstructed with significant accuracy.Figure 23
Occupancy grid of the complex environment using100
% of the data and a cell size of 0.1 m.Figure 24
MSE between the true occupancy grid of the complex environment and the reconstructed form of the grid as a function of percent of data stored for each of the four cell sizes.


## 6.1. How Much Compression Can Be Achieved?

There are two key components that affect how much an occupancy grid can be compressed; first is the complexity of the environment in which the UMV is operating. If the environment is simple, such as a rectangular room, then a relatively small amount of data is needed to represent it with respect to a more complex environment. The second component that affects how much an occupancy grid can be compressed is the cell size used. If a large cell size is selected then there are fewer cells to represent the environment compared to when a smaller cell size is selected. It follows that when a larger cell size is selected a larger percentage of the cells must be used to store the information about the environment, even for simple environments so the grid cannot be compressed as much. To illustrate these two components, three separate environments of increasing complexity where generated and a compressed occupancy grid was generated for each environment using four separate cell sizes (1.0 m, 0.5 m, 0.2 m, and 0.1 m) and varying amounts of compression. The first environment is a simple rectangular room and an occupancy grid for this environment generated using the full set of data with the smallest cell size is shown in Figure19. The MSE as function of data stored for this environment is shown in Figure 20 for each of the four cell sizes. As expected the environment with the largest cell size, when fewer coefficients are needed to store all of the environment information, has the largest error when a small amount of data is used to represent the environment. It can also can be seen that, for each of the four cell sizes, as more information is stored, the error in the reconstructed form of the occupancy grid decreases.Figure 19
Occupancy grid of simple rectangular environment using100
% of the data and a cell size of 0.1 m.Figure 20
MSE between the true occupancy grid of the simple environment and the reconstructed form of the grid as a function of percent of data stored for each of the four cell sizes.A second environment with more complexity is shown in Figure21 which is a rectangular room with complex corners. The MSE between the true occupancy grid and the reconstructed grid as a function of the amount of data stored is shown in Figure 22 for each of the cell sizes. The overall behavior of the error is the same as in the simple case with the largest cell size having the largest error when small amounts of data are used. However, because of the added complexity in this environment, the errors at the lower data percentages are larger than the simple case which is what we expected because more information is required to store the more complex environment.Figure 21
Occupancy grid of rectangular environment with complex corners using100
% of the data and a cell size of 0.1 m.Figure 22
MSE between the true occupancy grid of the environment with medium complexity and the reconstructed form of the grid as a function of percent of data stored for each of the four cell sizes.Finally, a complex environment of a rectangular room with complex corners along with rectangular objects located throughout the rectangular room is shown in Figure23. As with the previous two environments the MSE between the true occupancy grid and the reconstructed occupancy as a function of data is shown in Figure 24 for each cell size. As expected the use of larger cell sizes causes more errors in the reconstructed form of the grid when small amounts of data are used. Also, the additional complexity causes the errors at lower data percent to be larger than in the more simple environments. It can be seen from the presented results that there appears to be a “magic” compression level of 40
% at which, no matter the complexity, the compressed occupancy can be reconstructed with significant accuracy.Figure 23
Occupancy grid of the complex environment using100
% of the data and a cell size of 0.1 m.Figure 24
MSE between the true occupancy grid of the complex environment and the reconstructed form of the grid as a function of percent of data stored for each of the four cell sizes.


## 7. Conclusions

In this paper an approach for solving the SLAM problem was presented that makes use of compressed occupancy grids. The approach is based on an extension to the FastSLAM algorithm that represents an unknown environment by using occupancy grids. A modified form of the SLAM algorithm, FastSLAM COG, was presented in Section3 which makes use of a generic compression and reconstruction method to solve the SLAM problem. Four specific reconstruction methods were examined in Section 4 in order to see how well each of the approaches could reconstruct a compressed signal along with an analysis of the time needed for each algorithm to complete. Based on the reconstruction time, two of the four algorithms were used to replace the generic reconstruction method in the FastSLAM COG algorithm and their performance was examined in Section 6 using experimental data. From the presented experimental results it was concluded that the FastSLAM COG algorithm using l
2 reconstruction approach could solve the SLAM problem with almost no errors compared to the FastSLAM OG algorithm while storing only 40
% of the data required to store the complete occupancy grid. The experimental results presented in this paper were achieved by exploring how compressing the occupancy grid affects the performance of the algorithm in simple environments. In order to further examine our approach, a next step would be to examine how well this approach performs in more complex and unstructured environments. Also, further investigation into the effect of using more complex compression and reconstruction approaches as well as the possibility of dynamically resizing the cell size could lead to higher data compression rates without inducing errors into the SLAM solution.



---

*Source: 3891865-2016-07-05.xml*