卡尔曼滤波演示球追踪

以下是卡尔曼滤波演示球追踪的Python代码：

import numpy as np
import cv2

# 定义视频文件路径
video_path = 'ball_tracking_example.mp4'

# 打开视频文件
cap = cv2.VideoCapture(video_path)

# 定义测量矩阵H
H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtype=np.float32)

# 定义状态转移矩阵F
F = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=np.float32)

# 定义过程噪声协方差矩阵Q
Q = np.eye(4, dtype=np.float32) * 0.1

# 定义测量噪声协方差矩阵R
R = np.eye(2, dtype=np.float32) * 0.01

# 定义初始状态估计值x
x = np.array([[0], [0], [0], [0]], dtype=np.float32)

# 定义初始状态协方差矩阵P
P = np.eye(4, dtype=np.float32)

# 定义观测值
measurements = []

# 定义预测值
predictions = []

# 定义估计值
estimations = []

while True:
    # 读取视频帧
    ret, frame = cap.read()

    if not ret:
        break

    # 转换为灰度图像
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # 进行高斯滤波
    blurred = cv2.GaussianBlur(gray, (11, 11), 0)

    # 进行二值化处理
    thresh = cv2.threshold(blurred, 200, 255, cv2.THRESH_BINARY)[1]

    # 进行形态学操作
    kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
    closed = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)

    # 寻找轮廓
    contours, hierarchy = cv2.findContours(closed.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    # 如果没有找到轮廓，则跳过
    if len(contours) == 0:
        continue

    # 找到面积最大的轮廓
    c = max(contours, key=cv2.contourArea)

    # 计算轮廓的外接圆
    ((x, y), radius) = cv2.minEnclosingCircle(c)

    # 计算轮廓的质心
    M = cv2.moments(c)
    center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))

    # 将测量值添加到列表中
    measurements.append(np.array([[np.float32(center[0])], [np.float32(center[1])]]))

    # 如果测量值列表长度大于1，则进行卡尔曼滤波
    if len(measurements) > 1:
        # 计算时间间隔dt
        dt = 1.0 / 30

        # 预测状态
        x = np.dot(F, x)
        P = np.dot(np.dot(F, P), F.T) + Q

        # 更新状态
        y = measurements[-1] - np.dot(H, x)
        S = np.dot(np.dot(H, P), H.T) + R
        K = np.dot(np.dot(P, H.T), np.linalg.inv(S))
        x = x + np.dot(K, y)
        P = np.dot((np.eye(4) - np.dot(K, H)), P)

        # 将预测值和估计值添加到列表中
        predictions.append(np.dot(H, x))
        estimations.append(x)

    # 在图像上绘制圆和质心
    cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2)
    cv2.circle(frame, center, 5, (0, 0, 255), -1)

    # 显示图像
    cv2.imshow("Frame", frame)

    # 按下q键退出
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# 释放视频文件和窗口
cap.release()
cv2.destroyAllWindows()