python实现简单的卡尔曼滤波，，预测小车的位移和速度（匀速）

初始化状态向量X（xt,vt）

# -*- coding: utf-8 -*-
"""
Time    : 2022/4/20 17:50
Author  : cong
# 小车匀速前进
"""
import numpy as np
import random
import matplotlib.pyplot as plt
import seaborn as sns
# 观测值
Z = list(range(1000))
noise = [random.normalvariate(0, 1) for i in range(1000)]  # 标准正态分布：均值为0方差为1
Z = np.sum([Z , noise], axis= 0)
# plt.plot(noise)
# plt.hist(noise, bins=100, color="#FF0000", alpha=.8)# bin:划分的区间个数。alpha：矩形透明度。
# sns.kdeplot(noise, shade=True,color="#FF0000")
# plt.show()

X = np.array([0, 0]).T # 初始状态 pt, vt
# X = np.reshape(X, (2,1))

P = np.eye(2)  # 协方差矩阵

F = np.array([[1, 1],
              [0, 1]])   # 状态转移矩阵   pt = pt-1 + vt-1 * t; vt = vt-1 + a*t

Q = np.array([[0.001, 0],
              [0, 0.001]])  # 预测噪声矩阵

H = np.array([1,0]).T  # 观测矩阵

R = np.array([1])  # 观测噪声误差

X0 = []
X1 = []
result = []
for i in range(1000):
    # 预测过程
    X_ = np.dot(F, X)
    P_ = np.dot(np.dot(F, P), F.T) + Q
    # 更新过程
    K = np.dot(np.reshape(np.dot(P_, H.T), (2, 1)), 1 / (np.dot(np.dot(H, P_), H.T) + R))
    X = X_ + np.dot(K, (Z[i] - np.dot(H, X_)))
    P = np.dot((np.eye(2) - np.dot(K, H)), P_)
    result.append((X[0], X[1]))
    X0.append(X[0])
    X1.append(X[1])

plt.subplot(2, 2, 1)
plt.plot(result)
plt.title("result")
plt.legend(['displacement','volicity'])
plt.subplot(2, 2, 3)
plt.plot(X0)
plt.title("displacement")

plt.subplot(2, 2, 4)
plt.plot(X1)
plt.title("velocity")
plt.tight_layout()
plt.show()

# # 以下两种方式一样，黄线是y
# plt.plot([5,4,3,2,1])
# plt.plot([5,6,7,8,9])
# plt.show()
# plt.plot([(5,5),(4,6),(3,7),(2,8),(1,9)])
# plt.show()
#
# ## plt.plot(x,y)画图才是x对应y的点
# # 其余方式索引  对应列表的值
# plt.plot([5,4,3,2,1],[5,6,7,8,9])
# plt.show()

结果如下展示：