4
$\begingroup$

I'm struggling with fully understanding the concept behind a Kalman filter. For the sake of simplicity, let's ingore the input variable $u$ and assume constant process $Q$ and measurement noise $R$. We can now predict the new state by using the state transition matrix $F$ and the old state estimation $$x^-_k = Fx_{k-1}$$. Same does apply for the estimate uncertainty matrix : $P^-_k = FP_{k-1}F +Q$. Now we observe some data and depending on the ratio between the estimate uncertainty and the measurement uncertainty, we update our belief of the state estimation by calculating the innovation $$x_k = x^-_k + K(z_k - Hx^-_k)$$ where $z_k$ is the current measurement, $H$ is the observation matrix and $K$ is the kalman gain $P^-_kH^T(HP^-_kH^T+R)^{-1}$. To incorporate the new measurement the state uncertainty is updated with $P_k = (1-KH)P^-_k$.

  1. Do I see it correctly that $P_k$ decreases with each iteration because $ (1−KH)≤1$? Assuming that this is true, doesn't this mean that the uncertainty in the state estimation becomes very small with enough time steps and eventually tends towards $Q$ since the values within KH become smaller with each step?

  2. If we are at a point where the values in $P$ are very small, which yes means that the uncertainty in our estimated state and the estimated measurement is very small, then the influence of the measurements is probably very small as the Kalman gain becomes small - I.e. even if the innovation $z_k-Hx^-_k$ is very large, we trust the prediction of the model, although perhaps the true observation would have been correct?

  3. Suppose the values in $P_k$ are very small and we get measurements that are very different from the previous ones, but (for whatever reason) are true. The predictions of the kalman filter will deviate strongly from this and still have a small variance, simply because according to the previous data the prediction of the KL is more probable than the measurement? I.e. At a certain point we have only predictions with a small variance, no matter whether they correspond to the truth and I see now way how the values in $P_k$ could grow again. But I think they should grow because if the innovation is big for some iterations, then there must be something wrong with the predictions

Thankful for any tip or reference

$\endgroup$

1 Answer 1

5
$\begingroup$

The Kalman filter is one of those interesting algorithms which are completely impenetrable if you don't have the underlying math background (multivariate statisics, in this case), but become utterly obvious in in hindsight when you do.

Every "why" question about the plain old Kalman filter can be answered by looking at its problem statement, which is to design a filter that:

  • Assumes zero-mean Gaussian noise driving the processes and measurement error.
  • Assumes the states of a linear system is being estimated.
  • Assumes that -- aside from noise -- the system parameters and inputs are perfectly known.
  • Defines "optimal" as being that solution which, at each step, gives the state estimate whose expected value is the smallest Euclidean distance from the system's actual states (i.e., sum-of-squares).

Most of the time, when the operation of the Kalman filter isn't intuitive, it's because your intuition comes from real world systems, and real world systems aren't linear, real-world noise isn't Gaussian, and real world system models aren't 100% accurate.

Do I see it correctly that Pk decreases with each iteration because (1−KH)≤1? Assuming that this is true, doesn't this mean that the uncertainty in the state estimation becomes very small with enough time steps and eventually tends towards Q since the values within KH become smaller with each step?

For constant $Q$ and $R$ it does decrease asymptotically, but it doesn't tend toward $Q$ because of the update step $P_k = (1-KH)P^-_k$ -- and $K$ depends on $R$. Basically, high $R$ (high measurement noise) increases the uncertainty in the state estimation. Do a web search on "Steady State Kalman" for much more information than will fit into a StackExchange post.

If we are at a point where the values in P are very small, which yes means that the uncertainty in our estimated state and the estimated measurement is very small, then the influence of the measurements is probably very small as the Kalman gain becomes small - I.e. even if the innovation zk−Hx−k is very large, we trust the prediction of the model, although perhaps the true observation would have been correct?

If that is the case, then $Q$ or $R$ or both are wrong. $Q$ and $R$ aren't chosen arbitrarily -- they must match the problem. If (and only if) they do, then the Kalman is optimal in the least-squares sense.

Suppose the values in Pk are very small and we get measurements that are very different from the previous ones, but (for whatever reason) are true. The predictions of the kalman filter will deviate strongly from this and still have a small variance, simply because according to the previous data the prediction of the KL is more probable than the measurement? I.e. At a certain point we have only predictions with a small variance, no matter whether they correspond to the truth and I see now way how the values in Pk could grow again. But I think they should grow because if the innovation is big for some iterations, then there must be something wrong with the predictions

Then $Q$ or $R$ or both are wrong, or the probability distributions aren't Gaussian and the assumptions of a plain old Kalman filter don't hold.

The Kalman filter assumes that the system whose states are being estimated is linear, that $Q$ and $R$ are known, and that the processes they describe are Gaussian. If they aren't known (or are not matched to the problem at hand) then the Kalman won't give optimal results. If the processes are not Gaussian then the Kalman filter will be the best linear filter in the mean-squared sense, but it won't be the best filter overall, and there may be some other sense (i.e., min-max) where it isn't optimal.

You seem to either be confused about the necessary condition (sometimes impossible to meet) that $Q$ and $R$ are known, or that the plain old Kalman assumes Gaussian processes.

If, for example, your measurement usually has a Gaussian error, but occasionally has a really bad measurement (i.e., it's got some almost-Gaussian long-tailed distribution), then you may be able to get close to an optimal filter with a filter that's a plain old Kalman at its core, but which monitors the error between $\hat z$ and the measured $z$, and if that error is much larger than predicted by $H\ P\ H^T$ you substitute in a much larger $R$ than "normal".

OTOH, if your system usually runs along smoothly, but every once in a while it acts like it got hit with a rock, you could again detect that $\hat z$ is not matching measurement, and substitute in a large $Q$ and recompute $P$ (especially if the "got hit by a rock" mechanism is known, and has a known effect on $P$).

Neither of these is indicative of problems with the plain old Kalman filter per se -- just that it is only optimal in one sense, and only for a certain model, and only when reality actually matches the model that you use to generate the filter. Stray outside of the bounds where it is the right answer and -- you'll get the wrong answer.

$\endgroup$
6
  • $\begingroup$ Thanks for your detailed answer. I still don't get it completely, why we decrease tge covariance of the state covaraince without looking at the measurement itself. Is it because if we set all the parameters correctly than we can be sure with enough data the kalman filter approximates the true state/target and therefore it's ok to decrease the variance? If we model s.th. wrong or the System is not linear, then we might reduce the althought the filter performs bad but this simply because the assumption of linear systen or noise does Not Hold? $\endgroup$
    – user57647
    May 30, 2021 at 10:10
  • $\begingroup$ what also confuses me is a Statement found here: towardsdatascience.com/… , it says that the KF will be always confident on where it is, as long as the readings do not deviate too much from the predicted value. - this implys that when the deviation is big the filter is uncertain, i can't see that with the equations $\endgroup$
    – user57647
    May 30, 2021 at 10:31
  • $\begingroup$ One of three things is going on vis. the statement in that article: the author is assuming that the deviation is known but didn't express it well, the author is using some sort of extended Kalman filter that monitors for large deviations and varies the measurement or process noise covariance as I mention above, or the author is mistaken about how the Kalman filter works. $\endgroup$
    – TimWescott
    May 31, 2021 at 15:13
  • $\begingroup$ The covariance of the filter operates independently of the measurement because of the assumptions that the KF are based upon (see my edit at the top of the answer). If the noise is Gaussian, if the system model is correct, then the actual covariance would not depend on the actual measurement -- but then, if those two conditions were met, a truly "wild" measurement or state excursion would be of vanishingly low probability. $\endgroup$
    – TimWescott
    May 31, 2021 at 15:27
  • $\begingroup$ Thanks so much! $\endgroup$
    – user57647
    May 31, 2021 at 20:53

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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