卡尔曼滤波器（一维）：几种方法？

Question

I try to understand how the Kalman filter works and because the multi-dimensional variants were too confusing for the beginning I started off with a one-dimensional example.

I found 3 different sources explaining the scenario of a thermometer but all of these scenarios implement slightly different equations and I do not get the point.

I implemented solution 2 but my kalman filter was not really working (it highly adapted itself to the measurements and not really considered the noise on it).

So, before I waste more time trying solution 1 or 3 (which I have just read until now): Can someone supply a clean explanation and/or code example for a one dimensional Kalman filter?

---

Solution 1

// x_est: current estimate;           p: current estimate error;
// a:     constant of the system;    kg: kalman gain
// z:     current observation; 

// Predict
x_est   =   a * x_est
p       =   a * p * a

// Update
kg      =   p  / (p  + r)
x_est   =   x_est + kg * (z - x_est)
p       =   (1 - kg) * p

The author (here) only explains that we are changing only current values because there is no need for a thermometer to consider the last value.

So he simplified:

p[k] = (1 - kg) * p[k-1] to p = (1 - kg) * p

x_est[k] = x_est[k-1] + kg * (z - x_est[k-1]) to x_est = x_est + kg * (z - x_est)

...and so on...

I do not understand why this is even possible. I thought one of the main parts of the Kalman filter is to consider wether the current observation z is useful or not (via the Kalman gain). So that for a high Kalman gain kg * (z - x_est[k-1]) a "big chunk" of the delta z - x_est[k-1] is added to the new estimate. Isn't this whole thing getting pointless, if one always calculates the current values?

---

Solution 2

# q: process variance / process noise
# r: error in measurement

x_est = x_est
p     = p + q;

k     = p / (p + r);
x_est = x_est + k * (z – x_est);
p     = (1 – k) * p;

This is pretty much the same, but the author did not even give an explanation why x[k-1] and p[k-1] can be altered to x and p .

---

Solution 3

# Q: process variance / process noise
# R: error in measurement

# prediction
x_est_kminus1[k] = x_est[k - 1]
p_kminus1[k]        = p[k - 1] + Q

# update
kg[k]     = p_kminus1[k] / (p_kminus1[k] + R)
x_est[k] = x_est_kminus1[k] + kg[k] * (z[k] - x_est_kminus1[k])
p[k]     = (1 - kg[k]) * p_kminus1[k]

In this solution the author had two different lists for x_est ( x_est itself and x_est_kminus1 ) and p ( p itself and p_kminus1 ).

Are two lists needed because otherwise p[k] would be calculated twice (in the prediction and the update step)?

Answer 1

All of these solutions are special cases of the general equations and we'll have to see what's special about each one.

Proper equations

Let's start with the proper general equations for the 1D case:

# prediction
x[k] = a * x[k - 1]
p[k] = a * p[k - 1] * a + q
# update
y = z - h * x[k]
kg = p * h / (h * p * h + r)
x[k] = x[k] + kg * y
p[k] = (1 - kg * h) * p[k]

• x - state
• p - error (covariance)
• a - state transition
• q - transition error
• z - measurement
• h - state-to-measurement transformation
• y - difference between what we would have expected to measure based on the prediction and what we actually measured
• kg - kalman gain
• r - measurement error

All of the parameters of the model ( a , q , r , h ) could in principal also have an index k and change as the system evolves. But in simple cases they can all be taken as constant.

How the solutions differ from the proper equations

Only solution 1 implements an a and that's fine. a tells you how the state changes from one step to the other, if you assume the temperature to be stationary then a == 1 , like in solution 2 and 3.

Solution 1 does not have a q . q is where we can give an estimate of the process error. Again, if the process is about the system being stationary ( a == 1 ) then we could set q = 0 .

None of your solutions have an h , which is the observation transformation (how to get from measurement to state). If you are estimating the temperature, based on measurements of the temperature then h = 1 .

An example of when h may be different from 1 is if you were measuring something else than you are interested in estimating, eg using a measurement of humidity to estimate the temperature. Then h would be the linear transformation T(humidity) = h * humidity . I emphasize linear because the above are the linear Kalman filter equations and they only apply to linear (in the mathematical sense) systems.

Current and previous step issue

The question of k vs. k - 1 and having x_est and x_est_kminus1 is purely a matter of implementation. In this regard all your solutions are the same.

Your thinking about k and k - 1 in solution 1 is off. Only the prediction stage needs to think about the current and the previous step (since it's a prediction of the current state based on the previous one), not the update step. The update step acts on the prediction.

From a readability point of view solution 3 is closest to the mathematical equations. In principal the prediction step does not give us x_est[k] yet but more like predicted_x_est[k] . Then the update step runs on this predicted_x_est[k] and gives us our actual x_est[k] .

However as I said, all implementations are equivalent because when they are programmed, you can see that after the prediction step, the past is not needed any more. So you can safely use one variable for p and x without needing to keep a list.

About kalman gain

You wrote:

So that for a high Kalman gain kg * (z - x_est[k-1]) a "big chunk" of the delta z - x_est[k-1] is added to the new estimate. Isn't this whole thing getting pointless, if one always calculates the current values?

In these cases the kalman gain can only be between 0 and 1. When is it biggest? When r (measurement error) is 0, which means that we infinitely trust our measurements. The equation then simplifies to

x_est = x_est + z - x_est

which means that we discard our predicted value (the x_est on the right hand side) and set our updated estimate equal to our measurement. This is a valid thing to do when we infinitely trust what we measure.

Adapting to measurements

I implemented solution 2 but my kalman filter was not really working (it highly adapted itself to the measurements and not really considered the noise on it).

Tuning a Kalman Filter is tricky, and requires deep knowledge of the system and proper estimates of q and r . Remember that q is the error on the process (state evolution) and r is the error on our measurements. If your Kalman filter is adapting itself too much to the measurements it means that:

• q is too large
• r is too small

or a combination of the two. You will have to play with the values to find ones that work.