Kalman Filters
1. Difference of Tracking Techniques
- Kalman Filters : continuous states, uni-modal
- Monte Calro Localization: discrete states, multi-modal
- Particle Filters (Radar): continuous states, multi-modal
Localization
Measurement: product, Bayesian rule
Motion: convolution, total probability
measurement meant updating our belief (and re-normalizing our distribution).
Motion meant keeping track of where all of our probability “went” when we moved (which meant using the law of Total Probability).
The new belief will be more certain than either the previous belief OR the measurement.
Kalman Filter
- Predict
- Predicted (a priori) state estimate $ \ \ \ \ \ \ \ \ \ \ \ \ X’ = F \cdot X+ u$
- Predicted (a priori) estimate covariance $ \ \ \ P’ = F \cdot P \cdot F^T$
- Update
- Innovation or measurement residual $ \ \ \ \ \ \ \ \ y = Z - H \cdot X$
- Innovation (or residual) covariance $ \ \ \ \ \ \ \ \ \ \ S = H\cdot P \cdot H^T + R$
- Optimal Kalman gain $ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K = P \cdot H^T \cdot S^{-1}$
- Updated (a posteriori) state estimate $ \ \ \ \ \ \ X’ = X + (K \cdot y)$
- Updated (a posteriori) estimate covariance $P’ = (I-K\cdot H)P$
X = estimates
P = uncertainity covariance
F = state transition matrix
u = motion vector
Z = measurement
H = measurement function
R = measurement noise
I = identity matrix