7 ms·
Kalman Filters track both an estimate x_hat and the variance of that estimate, P. There is also some true state, x_true. The definition of the variance of the e
by arch-dawson 3y ago
Kalman Filters track both an estimate x_hat and the variance of that estimate, P. There is also some true state, x_true. The definition of the variance of the estimate is E[(x_true - x_hat)^2], where E[.] is the expected value operator.
Using the example from the post, there is a known dynamics function that propagates the state one time step forward. That function is f(x, w) = x + velocity * Delta_t + w, where w is zero-mean Gaussian noise (meaning that E[w] = 0) with some known variance sigma_w^2 (meaning that E[w^2] = sigma_w^2). To update the true state, we do x_true_k+1 = f(x_true_k, w) = x_true_k + velocity * Delta_t + w.
So what should the filter's estimate of the state at time t_k+1 be? Well, if our estimator has been working up to this point, then x_hat_k = E[x_true_k] (this is just saying that our estimate is the "best guess" of the true state). We also want it to be true that x_hat_k+1 = E[x_true_k+1]. Plugging in the true dynamics from earlier, we get that x_hat_k+1 = E[x_true_k + velocity * Delta_t + w] = E[x_true_k] + E[velocity * Delta_t] + E[w] = x_hat_k + velocity * Delta_t + 0. Note that we are not adding noise to the estimate. The filter has no way of knowing the noise that enters the system, the filter will just be correct /on average/ for the noise that comes up.
It is also important to update the uncertainty in the estimate. State estimates are of little use without knowledge of the uncertainty. If the estimate is being fed into a self-driving car or something, "The car is here!" is pretty useless on its own. "The car is here, plus or minus 10 cm" means the car can drive as normal, but "The car is here, I think, or maybe in one of the surrounding states" means there's a problem.
We already know that the definition of the variance is P_k=E[(x_true_k - x_hat_k)^2]. Let's say that at time t_k, P_k = sigma^2. To get the variance at time t_k+1, we can just apply the definition. P_k+1 = E[(x_true_k+1 - x_hat_k+1)^2] = E[((x_true_k + velocity * Delta_t + w) - (x_hat_k + velocity * Delta_t))^2] = <skipping some steps> = E[(x_true_k - x_hat_k)^2] + E[w^2] = P_k + sigma_w^2 = sigma^2 + sigma_w^2. So the variance at the new time is equal to the old covariance sigma^2, plus some uncertainty accounting for the noise added in the dynamics sigma_w^2.
Sorry about the poor math formatting, but I hope that answers your question!