参考博客:无人驾驶汽车系统入门(一)——卡尔曼滤波与目标追踪_AdamShan的博客-CSDN博客_卡尔曼滤波预测汽车轨迹
import numpy as np
%matplotlib inline
import matplotlib.pyplot as plt
from scipy.stats import norm
from sympy import Symbol, Matrix #科学计算库,可计算方程、微风、无穷级数
from sympy.interactive import printing #可打印出根号、求集合、无穷级数——为数学表达式打印unicode字符
printing.init_printing()
## Initialize related variables
x = np.matrix([[0.0,0.0,0.0,0.0]]).T # 初始状态
P = np.diag([1000.0,1000.0,1000.0,1000.0]) # 提前预测误差协方差
dt = 0.1 # 时间片
F = np.matrix([[1.0,0.0,dt,0.0], # 预测矩阵F(4,4)
[0.0,1.0,0.0,dt],
[0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,1.0]])
H = np.matrix([[0.0,0.0,1.0,0.0], # 测量矩阵H(2,4)
[0.0,0.0,0.0,1.0]])
Ra = 10.0**2
R = np.matrix([[Ra,0.0] # 测量噪声协方差矩阵R
,[0.0,Ra]])
pv = 0.5
G = np.matrix([[0.5*dt**2],
[0.5*dt**2],
[dt],
[dt]])
Q = G*G.T*pv**2 # 过程噪声的协方差矩阵Q
I = np.eye(4) # 单位矩阵
## Initialize sensor data and graphs
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('Standard Deviation of Velocity Measurements=%.2f' % np.std(mx))
fig = plt.figure(figsize=(16,5))
plt.step(range(m),mx, label='$\dot x$')
plt.step(range(m),my, label='$\dot y$')
plt.ylabel(r'Velocity $m/s$')
plt.title('Measurements')
plt.legend(loc='best',prop={'size':18})
## Data in the stored procedure
xt = []
yt = []
dxt= []
dyt= []
Zx = []
Zy = []
Px = []
Py = []
Pdx= []
Pdy= []
Rdx= []
Rdy= []
Kx = []
Ky = []
Kdx= []
Kdy= []
def savestates(x, Z, P, R, K): # 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]))
## KF function
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)
## figure
def plot_velocity():
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')
def plot_path():
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')
plot_velocity()
plot_path()