卡尔曼滤波的一个简单demo： 恒定加速度模型

P = 100.0*np.eye(9)

#define dt = 0.05 rather than dt = 1
dt = 0.05

#define H and A according to what we have discused
......
###
#define observation noise covariance
R = 100*np.eye(3)

#define process noise matrix Q
#from the blog we mentioned how to compute Q from G
G = ...
a = 0.1
Q = G*G.T*a**2


#system initialization
px=0.0
py=0.0
pz=0.0

vx=1
vy=2
vz=3

Xr=[]
Yr=[]
Zr=[]

# #generate position:
for i in range(100):

    # we assume constant acceleratoin for this demo
    accx = 3
    vx += accx * dt
    px += vx * dt

    accy = 1
    vy += accy * dt
    py += vy * dt

    accz = 1
    vz += accz * dt
    pz += vz * dt

    Xr.append(px)
    Yr.append(py)
    Zr.append(pz)

    # position noise
    sp = 0.5

Xm = Xr + sp * (np.random.randn(100))
Ym = Yr + sp * (np.random.randn(100))
Zm = Zr + sp * (np.random.randn(100))

#stack measurements
measurements = np.vstack((Xm,Ym,Zm))

#define initial state
X = np.matrix([0.0, 0.0, 0.0, 1.0, 2.0, 3.0, 3.0, 1.0, 1.0]).T
I = np.eye(9)
Xx = []
Xy = []
Xz = []

for each_step in range(100):

    #prediction step:
    X = A*X
    #state covariance propogation
    P = A*P*A.T + Q

    #measurement covariance propogation
    S = H*P*H.T + R

    #Kalman Gain update
    K = (P*H.T) * np.linalg.pinv(S)
    # print(K)
    Z = measurements[:,each_step].reshape(3,1)

    #corrected/updated x
    X = X + K*(Z-(H*X))

    #update state covariance P:
    P = (I - (K * H)) * P

    #store corrected/updated x
    Xx.append(float(X[0]))
    Xy.append(float(X[1]))
    Xz.append(float(X[2]))