卡尔曼滤波(一) 行人状态估计Python

import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
'''
prediction:
x_k+1 = F*x_k + B*u_k (B=0)
P_k+1 = F*P_K*F + Q
correction:
K_k = P_k*H^T/(H*P_k*H^T+R)
x_k = x_k +K_k(z_k-H*x_k)
P_k = (1-K_k*H)P_k
'''

'''
始化行人状态x
x = [   p_x,
        p_y,
        v_x,
        v_y    ]
'''
x = np.matrix([0.0,0.0,0.0,0.0]).T
print(x,x.shape)
'''
行人的不确定性(协方差矩阵)P,np.diag(),输入为1维数组时,结果形成一个以一维数组为对角线元素的矩阵
P = [   1000    0     0     0
        0     1000    0     0
        0       0   1000    0
        0       0     0   1000    ]
'''
P = np.diag([1000.0,1000.0,1000.0,1000.0])
print(P,P.shape)
#测量的时间间隔dt
dt = 0.1
#处理矩阵F
F = np.matrix([[1.0,0.0,dt,0.0],
              [0.0,1.0,0.0,dt],  
              [0.0,0.0,1.0,0.0],  
              [0.0,0.0,0.0,1.0]]  )
print(F,F.shape)
#测量矩阵H
H = np.matrix([[0.0,0.0,1.0,0.0],[0.0,0.0,0.0,1.0]])
print(H,H.shape)
#测量过程的噪声的协方差矩阵R
ra = 0.09
R = np.matrix([[ra, 0.0],
              [0.0, ra]])
print(R, R.shape)
#处理噪声的协方差矩阵Q
sv = 0.5
G = np.matrix([[0.5*dt**2],
               [0.5*dt**2],
               [dt],
               [dt]])
Q = G*G.T*sv**2

from sympy import Symbol, Matrix
from sympy.interactive import printing
printing.init_printing()

dts = Symbol('dt')
Qs = Matrix([[0.5*dts**2],[0.5*dts**2],[dts],[dts]])
Qs*Qs.T

I = np.eye(4)
print(I, I.shape)

m = 200 # Measurements
vx= 20 # in X
vy= 10 # in Y

mx = np.array(vx+np.random.randn(m))
my = np.array(vy+np.random.randn(m))
measurements = np.vstack((mx,my))

print(measurements.shape)
print('Standard Deviation of Acceleration Measurements=%.2f' % np.std(mx))
print('You assumed %.2f in R.' % R[0,0])

xt = []
yt = []
dxt= []
dyt= []
Zx = []
Zy = []
Px = []
Py = []
Pdx= []
Pdy= []
Rdx= []
Rdy= []
Kx = []
Ky = []
Kdx= []
Kdy= []

def savestates(x, Z, P, R, K):
    xt.append(float(x[0]))
    yt.append(float(x[1]))
    dxt.append(float(x[2]))
    dyt.append(float(x[3]))
    Zx.append(float(Z[0]))
    Zy.append(float(Z[1]))
    Px.append(float(P[0,0]))
    Py.append(float(P[1,1]))
    Pdx.append(float(P[2,2]))
    Pdy.append(float(P[3,3]))
    Rdx.append(float(R[0,0]))
    Rdy.append(float(R[1,1]))
    Kx.append(float(K[0,0]))
    Ky.append(float(K[1,0]))
    Kdx.append(float(K[2,0]))
    Kdy.append(float(K[3,0]))  

for n in range(len(measurements[0])):

    # Time Update (Prediction)
    # ========================
    # Project the state ahead
    x = F*x

    # Project the error covariance ahead
    P = F*P*F.T + Q

    # Measurement Update (Correction)
    # ===============================
    # Compute the Kalman Gain
    S = H*P*H.T + R
    K = (P*H.T) * np.linalg.pinv(S)

    # Update the estimate via z
    Z = measurements[:,n].reshape(2,1)
    y = Z - (H*x)                            # Innovation or Residual
    x = x + (K*y)

    # Update the error covariance
    P = (I - (K*H))*P

    # Save states (for Plotting)
    savestates(x, Z, P, R, K)

def plot_x():
    fig = plt.figure(figsize=(16,9))
    plt.step(range(len(measurements[0])),dxt, label='$estimateVx$')
    plt.step(range(len(measurements[0])),dyt, label='$estimateVy$')

    plt.step(range(len(measurements[0])),measurements[0], label='$measurementVx$')
    plt.step(range(len(measurements[0])),measurements[1], label='$measurementVy$')

    plt.axhline(vx, color='#999999', label='$trueVx$')
    plt.axhline(vy, color='#999999', label='$trueVy$')

    plt.xlabel('Filter Step')
    plt.title('Estimate (Elements from State Vector $x$)')
    plt.legend(loc='best',prop={'size':11})
    plt.ylim([0, 30])
    plt.ylabel('Velocity')
    fig = plt.figure(figsize=(16,16))
    plt.scatter(xt,yt, s=20, label='State', c='k')
    plt.scatter(xt[0],yt[0], s=100, label='Start', c='g')
    plt.scatter(xt[-1],yt[-1], s=100, label='Goal', c='r')

    plt.xlabel('X')
    plt.ylabel('Y')
    plt.title('Position')
    plt.legend(loc='best')
    plt.axis('equal')
    plt.show()

plot_x()

参考 http://blog.csdn.net/AdamShan/article/details/78248421