Introduction
Localization is the problem of determining and tracking the robot's
position, relative to a map of its environment.
The Kalman Filter is a localization algorithm that maintains
an estimate of the robot's state, expressed as a mean and
covariance matrix.
The Kalman Filter is:
- passive
- local
- probabilistic
Actions subject to error
Recall how we can model actions that experience error. State transition function: \[ x_{k+1} = f(x_k, u_k, \theta_k) \]| element | space of all | intuition | known? | |
| State | $x$ | $X$ | Where is the robot? | no |
| Action | $u$ | $U$ | What did the robot do? | yes |
| Action Error | $\theta$ | $\Theta$ | How accurately? | no |
Simple example
A one dimensional system with additive actions and additive error: \[X = U = \Theta = \R\] \[x_{k+1} = x_k + u_k + \theta_k\]
Observations
Remember how we can model observations that provide partial information about the current state, subject to error. Observation function: \[ y_k = h(x_k, \psi_k) \]| element | space of all | intuition | known? | |
| Observation | $y$ | $Y$ | sensor data | yes |
| Observation Error | $\psi$ | $\Psi$ | How accurately? | no |
Example continued
Example: Same as previous, with: \[ Y = \psi = \R \] \[ y_k = x_k + \psi_k\]
Probabilistic Localization
Problem: The robot does not generally know $x$!- Known: history of actions and observations in the past
- Not enough detail in the history to compute $x$ exactly
Linear-Gaussian systems
One special class of robot models that is very well-understood are the Linear-Gaussian (LG) systems.- Linear transitions: $x_{k+1} = Ax_k + Bu_k + G\theta_k$
- Linear sensing: $y_{k} = Cx_k + H\psi_k$
- Gaussian noise: Both $\theta_k$ and $\psi_k$ are chosen randomly according to independent, zero-mean Gaussian densities with covariance matrices $\Sigma_\theta$ and $\Sigma_\psi$ respectively.
Who cares?
LG systems are special because we can prove that the probability density \[ p(x_k \mid u_1,\ldots,u_k, y_1,\ldots,y_k) \] is always a Gaussian, no matter what actions the robot takes nor what observations it receives. This distribution can be represented by:- $\mu_k$: A state, representing the most likely location of the robot.
- $\Sigma_k$: A square covariance matrix, representing the “spread” of the distribution in each direction.
Kalman filter
The Kalman filter is a passive, local, probabilistic localization algorithm that computes the Gaussian probability density over states in an LG system.
Inputs:
- $\mu_k$
- $\Sigma_k$
- $u_k$
- $y_k$
- $A$, $B$, $C$, $G$, $H$
- $\Sigma_\theta$, $\Sigma_\psi$
Outputs:
- $\mu_{k+1}$
- $\Sigma_{k+1}$
Kalman update rule
To execute the Kalman filter, there are four steps.- Compute $\Sigma^\prime_{k+1}$, an intermediate covariance that accounts for $u_k$ but not $y_k$: \[ \Sigma^\prime_{k+1} = A \Sigma_k A^\top + G \Sigma_\theta G^\top \]
- Compute $L_{k+1}$, a partial result that makes the equations simpler: \[ L_{k+1} = \Sigma^\prime_{k+1}C^\top ( C \Sigma^\prime_{k+1} C^\top + H \Sigma_\psi H )^{-1} \]
- Compute $\mu_{k+1}$: \[ \mu_{k+1} = A \mu_k + B u_k + L_{k+1}( y_k - C(A \mu_k + B u_k) ) \]
- Compute $\Sigma_{k+1}$: \[\Sigma_{k+1} = (I - L_{k+1}C)\Sigma^\prime_{k+1} \]