Visualize the Kalman Filter and the Covariance Intersection Algorithm!

The Kalman Filter is a famous algorithm used to estimate the state (e.g. of a robot) over time. It fuses estimates produced by a dynamic/kinematic model of the object with sensor data.

For example, if a robot is commanded to move forward at 1 m/s for a second, a kinematic model would predict the robot moved forward a meter. The estimate predicted by the kinematic model would most likely be slightly incorrect due to, for example, environmental conditions. Over time, as small errors in the prediction accumulate, the estimate would be be completely unreliable. Sensor data, such as that from a LiDAR, can be used to augment this estimate. Because sensors have some associated error, the estimate of a robot's position using only one sensor measurement is also inaccurate, however.

Using a Covariance Matrix to describe the uncertainty of each estimate, the Kalman Filter intelligently fuses the prediction provided by the kinematic model and the estimate provided by the sensor at each time step.

For two estimates \( \vec a_{n\times1} \) and \( \vec b_{n\times 1} \) with associated covariance matrices \(A_{n \times n}\) and \(B_{n\times n}\) the update step of the Kalman Filter produces the fused estimate \( \vec c_{n\times 1}\) and its covariance matrix \( C_{n \times n}\): \[ \vec c = \vec a + K (\vec b - \vec a) \] \[ C = (I - K) A \] where \( I_{n \times n}\) is the identity matrix and \( K_{n \times n} \) is the Kalman gain: \[ K = A (A + B)^{-1} \] In the case of this example and visualization tool, \(n=2\).

Note that these equations are often provided with the addition of a transformation matrix \(H\). This is the "observation matrix": it performs the transformation that produces the output that a sensor would see given a state estimate \( \vec a \). The observation matrix is left out for simplicity (\(H\) is set to be equal to the identy matrix, and the "sensor" is assumed to be directly estimating the state). Note that the observation matrix can be rectangular (i.e, the sensor measures fewer dimensions than are in the state), which would cause the Kalman gain to be rectangular.

The assumptions made in deriving the Kalman Filter might not hold in practice. In particular, the Kalman filter assumes that the independent estimates are uncorrelated. This can cause serious problems if the estimates share a common source. For example, in the problem of two robots performing cooperative localization, they might take measurements of each other. One robot could measure the second robot's position relative to its own, and the second robot might use that to update its own position estimate, fusing the other robot's measurement into its own current estimate. If the first robot later updates its estimated position using a measurement from the second, it will be fusing two estimates that are derived from the same source (its own previous state estimate). The Kalman method would be incorrect to apply in such circuimstances, since its assumptions are not met. In particular, the Kalman method would underestimate the uncertainty of the fused estimate.

The covariance intersection algorithm performs fusion without the assumption that the two estimates are independent. For two estimates \( \vec a_{n\times1} \) and \( \vec b_{n\times 1} \) with associated covariance matrices \(A_{n \times n}\) and \(B_{n\times n}\) the Covariance Intersection algorithm produces the fused estimate \( \vec c_{n\times 1}\) and its covariance matrix \( C_{n \times n}\): \[ C = \bigl( \omega A^{-1} + (1 - \omega) B^{-1} \bigr)^{-1}\] \[ \vec c = C \bigl( \omega A^{-1} \vec a + (1 - \omega) B^{-1} \vec b \bigr) \] Where \(\omega\) is a weighting value between 0 and 1 that is chosen by finding the value that minimizes the trace of the resulting covariance matrix \(C\): \[ \min tr(C) \text{ s.t. } \omega \in [0, 1] \] In practice, \(\omega = 0\) or \(\omega = 1\) can be chosen. If the uncertainty of one estimate is in all respects less than the uncertainty of another (i.e., one confidence ellipse can completely swallow the other), covariance intersection (CI) will choose to use the better estimate completely.