The idea of using Kalman gain in EKF SLAM is to figure out how much we trust our motion model and sensor/observation model. As explained in The Extended Kalman Filter: An Interactive Tutorial for Non-Experts - Part 5: Computing the Gain, the Kalman gain can be calculated as,

$$K_t = \frac{p}{(p +r)}$$

where $p$ denotes prediction error and $r$ denotes sensor noise.

Now, if we look into the equation in the image,

$$K_t = \bar{\Sigma_t}H_t^T(H_t\bar{\Sigma_t}H_t^T +Q_t)^{-1}$$

we can see that Kalman gain is calculated using Covariance matrix ($\Sigma$), Jacobian of observation model ($H$) and Sensor noise ($Q$). Comparing with earlier equation, $p$ can be considered equivalent of $\Sigma$, while $r$ can be equivalent of $Q$.

How does $H$ fit in, in this equation? What would be an intuitive explanation?


Let's try breaking it down.

Projection of uncertainty

$H\Sigma H^T$ is projecting the state uncertainty into measurement space. How do we know that?

$\Sigma$ denotes the the covariance of our belief in the filter's state

$h(x)$ maps some belief about state to a measurement that we would expect if that were the true state

$H(x)$ is a linear approximation of $h(x)$.

Why? Covariance itself provides little intuition for what is happening under the "hood". What is more informative is a square root of the covariance. Imagine that $\Sigma = LL^T$ for some $L$. $L$ is what is really stretching the space in a linear fashion. If you were to deform the unit circle by $L$, you would get the uncertainty ellipse that we all know and love. By stretching and shrinking $L$, we can change our distribution.

That means that $HL$ would first deform the unit circle into an ellipse governed by $L$, and then $H$ would stretch and shear that ellipse into a different ellipse. Now we can recover the covariance of the deformed points: it would be $HL(HL)^T$, which is $HLL^TH^T = H\Sigma H^T$

Why does any of this matter?

The kalman gain is like a ratio, governing how much we should trust a new measurement. Often, your measurements will have different dimension and different units than your state. So you need to express uncertainty in the filter state as uncertainty in observation space.

The first equation is meaningless if $p$ and $r$ are different shapes or different units. We need to munge $p$ into the same shape and units as $r$ by projecting into $p$ observation space. The above rambling outlines how one does that.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.