#https://pysource.com/2021/10/29/kalman-filter-predict-the-trajectory-of-an-object/ import cv2 import numpy as np import matplotlib.pyplot as plt class KalmanFilter: #实例属性 kf = cv2.KalmanFilter(4, 2) #其值为4,因为状态转移矩阵transitionMatrix有4个维度 #需要观测的维度为2 kf.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32) #创建测量矩阵 kf.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 0.7, 0], [0, 0, 0, 0.7]], np.float32) #创建状态转移矩阵 # 创建一个0-99的一维矩阵 z = [i for i in range(100)] z_watch = np.mat(z) # 创建一个方差为1的高斯噪声,精确到小数点后两位 noise = np.round(np.random.normal(0, 1, 100), 2) noise_mat = np.mat(noise) # 将z的观测值和噪声相加 z_mat = z_watch + noise_mat # 定义x的初始状态,即位置和速度 x_mat = np.mat([[0, ], [0, ]]) y_mat = np.mat([[0, ], [0, ]]) def predict(self, coordX, coordY): #实例方法,自己实现一个predict ''' This function estimates the position of the object''' measured = np.array([[np.float32(coordX)], [np.float32(coordY)]]) self.kf.correct(measured) #结合观测值更新状态值,correct为卡尔曼滤波器自带函数 predicted = self.kf.predict() #调用卡尔曼滤波器自带的预测函数 x, y = int(predicted[0]), int(predicted[1]) #得到预测后的坐标值 # 绘制结果 plt.plot(measured[0], 'k+', label='Measured_x') plt.plot(x, 'b-', label='Kalman Filter_x') #plt.plot(real_state, 'g-', label='Real state') plt.legend(loc='upper left') plt.title('Kalman Filter Results') plt.xlabel('Time (s)') plt.ylabel('Position (m)') plt.show() return x, y predict(self,x_mat,y_mat)优化这段python代码,随机生成x和y并实现对x和y的输入值的预测,并画出图像,实现可视化
时间: 2023-06-20 17:04:51 浏览: 122
# 导入必要的库
import cv2
import numpy as np
import matplotlib.pyplot as plt
class KalmanFilter:
def __init__(self):
# 创建卡尔曼滤波器
self.kf = cv2.KalmanFilter(4, 2)
# 创建测量矩阵
self.kf.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
# 创建状态转移矩阵
self.kf.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 0.7, 0], [0, 0, 0, 0.7]], np.float32)
# 创建过程噪声协方差矩阵
self.kf.processNoiseCov = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0.5, 0], [0, 0, 0, 0.5]], np.float32)
# 定义x的初始状态,即位置和速度
self.x = np.array([[0], [0], [0], [0]], np.float32)
def predict(self, coordX, coordY):
# 将输入值转换为测量值
measured = np.array([[np.float32(coordX)], [np.float32(coordY)]])
# 预测下一时刻的状态值
self.x = self.kf.predict()
# 结合观测值更新状态值
self.x = self.kf.correct(measured)
# 得到预测后的坐标值
x, y = int(self.x[0]), int(self.x[1])
return x, y
# 创建KalmanFilter实例
kf = KalmanFilter()
# 随机生成x和y
x = np.array([i for i in range(1, 101)])
y = np.array([i for i in range(1, 101)])
# 添加随机噪声
x = x + np.random.normal(0, 10, 100)
y = y + np.random.normal(0, 10, 100)
# 预测输入值的位置
predicted_x = []
predicted_y = []
for i in range(100):
px, py = kf.predict(x[i], y[i])
predicted_x.append(px)
predicted_y.append(py)
# 绘制结果
plt.plot(x, y, 'k+', label='Measured')
plt.plot(predicted_x, predicted_y, 'b-', label='Kalman Filter')
plt.legend(loc='upper left')
plt.title('Kalman Filter Results')
plt.xlabel('X')
plt.ylabel('Y')
plt.show()