卡尔曼滤波器行为

Question

I have used the kalman filter implented here: https://gist.github.com/alexbw/1867612

I have a very basic understanding of it. this is the test code I have:

import matplotlib.pyplot as plt
import numpy as np
from Kalman import Kalman

n = 50    
d = 5

xf = np.zeros(n - d)
yf = np.zeros(n - d)

xp = np.zeros(d)
yp = np.zeros(d)

x = np.zeros(n)
y = np.zeros(n)

for i in range(n):

    if i==0:
        x[i] = 05
        y[i] = 20
        KLF = Kalman(6, 2)

    elif i< (n - d):
        xf[i], yf[i] = KLF.predict()  
        x[i] = x[i-1] + 1
        y[i] = y[i-1] + np.random.random() * 10
        NewPoint = np.r_[x[i], y[i]]
        KLF.update(NewPoint)
    else:
        x[i] = x[i-1] + 1
        y[i] = y[i-1] + np.random.random() * 10
        xp[n - i -1], yp[n - i -1] = KLF.predict()  
        NewPoint = np.r_[x[i] , yp[n - i -1]]
        KLF.update(NewPoint)

plt.figure(1)
plt.plot(x, y, 'ro') #original
plt.plot(xp, yp, 'go-') #predicted kalman
plt.plot(xf, yf, 'b') #kalman filter
plt.legend( ('Original', 'Prediction', 'Filtered') ) 
plt.show()

My question is, why does the kalman filtering starts at 0 if the data starts at x=5, y=20 ? Is that some sort of standard behaviour?

Thanks

Answer 1

The current state of the Kalman instance is stored in the x attribute:

In [48]: KLF = Kalman(6, 2)

In [49]: KLF.x
Out[49]: 
matrix([[ 0.],
        [ 0.],
        [ 0.],
        [ 0.],
        [ 0.],
        [ 0.]])

The six components represent the position, velocity and acceleration. So by default the Kalman instance starts at (0,0) with zero velocity and acceleration.

After instantiating KLF , when i=1 , the first modification to xf and yf is made by calling KLF.predict :

xf[i], yf[i] = KLF.predict()

There are two problems with this. First, xf[0], yf[0] is never updated, so it remains at (0, 0) . Hence the blue line starting at (0, 0) .

The second problem is that the current state of KLF.x is at (0, 0) by default, due to the way the Kalman class is defined. If you want the KLF instance to begin with a position at (5, 20) then you'll need to modify KLF.x yourself.

Also bear in mind that the Kalman filter is meant to be updated with an observation first and then make a prediction second . This is mentioned in the class docstring.

Now I don't quite understand the intent of your code so I'm not going to try to sort out how the update s should come before the predict s, but as far as setting the initial state is concerned, you could use this:

if i==0:
    x[i] = 5
    y[i] = 20
    KLF = Kalman(6, 2)
    KLF.x[:2] = np.matrix((x[0], y[0])).T
    xf[i], yf[i] = KLF.predict()  

which yields