Path Finding: Dijkstra's and A* Algorithms

• Problem Set 4 is due tonight.
• Problem Set 5 will be released on Gradescope tonight.

Path Finding

• Dijkstra's Algorithm is a graph search algorithm that finds the shortest path between nodes in a graph, expanding search in all directions.
• A* Algorithm is an informed search algorithm and an extension of Dijkstra's algorithm that uses a heuristic to guide the search.
• A* explores more promising paths first, making it more efficient than Dijkstra's algorithm in many cases.
• A* maintains 2 data structures: a closed set and an open set.
• The closed Set is a list of nodes that have already been visited.
• The open set is a priority queue of nodes to be visited.
• The priority in the open set determined by the cost function.
• Cost Function: $f(n) = g(n) + h(n)$
• $g(n)$ is the cost from the start node to node $n$
• $h(n)$ is the heuristic estimating the cost from node $n$ to the goal.
• The heuristic $h(n)$ must be admissible (never overestimates the true cost) to guarantee that A* finds the optimal path.
• A* iteratively expands the node with the lowest $f(n)$ value from the open set until the goal node is reached.

Probabilistic Robotics

• Robots operate in an uncertain world due to noisy sensor measurements, imprecise actuator commands, and dynamic, unpredictable environments.
• Probabilistic robotics is a framework for robots to reason about and make decisions under uncertainty by representing it using probability distributions.
• Bayesian inference is used to update beliefs based on new information.

Bayes Filter

• The Bayes filter is a probabilistic framework for estimating the state of a system over time given a series of observations and actions.
• Belief at time $t$: $bel(x_t) = p(x_t | z_{1:t}, u_{1:t})$
• $x_t$: State of the system at time $t$.
• $z_{1:t}$: Sequence of observations from time 1 to $t$.
• $u_{1:t}$: Sequence of actions from time 1 to $t$.
• Prediction Step: $\overline{bel}(x_t) = \int p(x_t | u_t, x_{t-1}) bel(x_{t-1}) dx_{t-1}$. This predicts the new belief based on the previous belief and the action taken.
• Integrating over all possible previous states $x_{t-1}$, weights each state by the probability of transitioning to the current state $x_t$ given the action $u_t$.
• Correction Step: $bel(x_t) = \eta \cdot p(z_t | x_t) \overline{bel}(x_t)$. This updates the belief based on the new observation
• A normalization factor, $\eta$, ensures that the belief is a probability distribution.
• Correction step multiplies the predicted belief $\overline{bel}(x_t)$ by the likelihood of the observation $z_t$ given the current state $x_t$

Localization

• Global Localization Problem exists when given a map of the environment and a sequence of observations and actions.
• The goal of Global Localization Problem is determine the robot's location in the map.
• Kidnapped Robot Problem occurs when the robot is placed in an unknown location and must relocalize itself based on sensor readings and map information.
• Markov Localization is a grid-based approach that discretizes the environment into a grid and maintains a probability distribution over all grid cells.
• Belief is updated based on sensor readings and motion commands.
• Particle Filter Localization (Monte Carlo Localization) is a sample-based approach.
• Belief is represented as a set of samples (particles), where each particle represents a possible robot pose.
• Sample sets/particles are updated based on sensor readings and motion commands.
• Particles are resampled to focus on areas where the robot is more likely to be located.