物体追跡におけるターゲットドリフトの問題

import numpy as np

class KalmanFilter:
    def __init__(self, dt, u, std_acc, std_meas):
        self.dt = dt
        self.u = u
        self.std_acc = std_acc
        self.std_meas = std_meas
        self.A = np.array([[1, dt], [0, 1]])
        self.B = np.array([0.5 * dt**2, dt])
        self.H = np.array([[1, 0]])
        self.Q = np.array([[0.25 * dt**4, 0.5 * dt**3], [0.5 * dt**3, dt**2]]) * std_acc**2
        self.R = std_meas**2
        self.state = np.zeros((2, 1))
        self.P = np.zeros((2, 2))
    
    def update(self, z):
        prediction = self.A @ self.state + self.B * self.u
        predict_cov = self.A @ self.P @ self.A.T + self.Q
        K = predict_cov @ self.H.T @ np.linalg.inv(self.H @ predict_cov @ self.H.T + self.R)
        self.state = prediction + K @ (z - self.H @ prediction)
        self.P = (np.eye(2) - K @ self.H) @ predict_cov

# 使用示例
dt = 0.1
u = 0
std_acc = 0.1
std_meas = 0.1

kf = KalmanFilter(dt, u, std_acc, std_meas)

# 假设在第0时刻目标位置为[0, 0]
true_position = np.array([[0, 0]]).T

# 生成时间序列
T = 10
time = np.arange(0, T, dt)

# 生成测量序列
meas = true_position + np.random.randn(len(time), 1) * std_meas

# 进行物体跟踪
for i, z in enumerate(meas):
    kf.update(z)
    print("Time: {:.1f}, Measured Position: [{:.1f}, {:.1f}], Estimated Position: [{:.1f}, {:.1f}]".format(
        time[i], z[0], z[1], kf.state[0], kf.state[1]))