Kalman filter 1D

References

The most helpful tutorial for me was the paper by Gary Bishop and Greg Welch “An introduction to the Kalman filter.” (2001)

Kalman filter is a very useful technique when you need to combine several sources of uncertainty to estimate value of the target variable.

Code for a one dimensional case

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
def kalman1d(n, z, sigma0=0.1, r=0.1):
    xhatminus = np.zeros(n)  # Predictions / priors
    xhatminus[0] = 1
    xhat = np.zeros(n)  # updated estimates
    Pminus = np.zeros(n)  # Projected variance values
    sigmas = np.zeros(n)  # Variance of the estimate
    sigmas[0] = sigma0
    P = np.zeros(n)  # Updated error variance
    K = np.zeros(n)  # Kalman gain values
    R = r  # Mesuarement variance during the inspection
    Q = sigma0  # Process variance
    P[0] = 1.0
    A = 1.0
    for k in range(1, n):
        xhatminus[k] = xhatminus[k-1] * A + 1
        if z[k] == -1:
            Pminus[k] = Pminus[k-1] + Q
            xhat[k] = xhatminus[k]
        else:
            Pminus[k] = P[k-1] + Q
            K[k] = Pminus[k]/(Pminus[k]+R)
            xhat[k] = xhatminus[k] + K[k] * (z[k] - xhatminus[k])
            P[k] = (1-K[k]) * Pminus[k]
    return xhat, Pminus

code on github