Bayes Filter
Bayes Filter makes use of both observations from a visual sensors such as a LiDARs, and also motion sensors such as wheel encoders, to create an accurate pose for where we are.
With Bayes Filter, we estimate the state or pose \({X}\), given a set of observations \({Z}\) and set of control inputs \({U}\). It is a probabilistic approach that computes how likely the system is to be at a pose in a set of possible poses, allowing us to determine which pose is the most probable.
While often referred to as an algorithm, the Bayes Filter is more accurately described as a probabilistic framework that provides a recipe for building various pose-estimation algorithms including Particle Filter.
Bayesian Measurement Update
Often, when using only control inputs, our robot’s localization becomes inaccurate. To counter this, Bayes Filters use a core concept called the Bayesian Update. Using visual data collected from a LiDAR or any other observation sensors, we are able to correct inaccuracies in our localization.
The core idea of the Bayesian Update is derived from the following expression: \(p(Z_{t}|X_{t})\). This expression represents the probability of observing LiDAR scan \(Z_{t}\), given the predicted pose \(X_{t}\).Comparing the measured and predicted scans from the LiDAR, we can estimate how likely it is that the robot is at the predicted position and adjust our belief accordingly.
To calculate \(p(Z_{t} \mid X_{t})\), we first transform the LiDAR scan from the sensor frame into the world frame using the particle’s pose \(x_{n}\).
We can then use the following formula to calculate it for beam \(i\) shot from the lidar at a given moment:
\(p_{\text{hit}}(Z_t^{k}{}_{i} \mid x_t, m) = \frac{1}{\sqrt{2\pi\sigma_{hit}^2}} e^{-\frac{1}{2}\frac{(z_t^k - z_t^{k*})^2}{\sigma_{hit}^2}}\)
-
\(\sigma_{hit}\) represents the standard deviation of your LiDAR sensor's noise(how much we trust our lidar sensor's measurements). It's a key tuning parameter in the particle filter's measurement model. A smaller \(\sigma_{hit}\) means you expect high accuracy, while a larger value accounts for a less precise or noisier sensor.
-
\(z_t^{k}\) represents the actual distance measured by the LiDAR beam k at time t. This is the value obtained directly from the sensor's measurement.
-
\(z_t^{k*}\) represents the expected distance to the first obstacle, as predicted by ray tracing through the Occupancy Grid Map (OGM). This is a theoretical value calculated from your map's representation of the world.
However, it is important to note this calculation is only for a single beam \(i\). To extend it to all beams in a LiDAR scan, we combine the probabilities across all beams.
We can calculate the overall \(p_{hit}(z_t^k | x_t, m)\), by multiplying together all \(p_{\text{hit}}(z_t^{k}{}_{i} \mid x_t, m)\) for each beam.
We will talk about further use-case of this update method in the Particle Filter section.