生成用卡尔曼滤波进行粒子径迹重建的代码

以下是使用Python进行粒子径迹重建的卡尔曼滤波代码示例：

import numpy as np
import matplotlib.pyplot as plt

# 生成随机粒子径迹数据
np.random.seed(10)
n_samples = 100
x = np.linspace(0, 10, n_samples)
y = 0.5 * x ** 2 + 5 * np.sin(2 * np.pi * x / 5) + np.random.randn(n_samples)

# 定义卡尔曼滤波模型
class KalmanFilter:
    def __init__(self, F, H, Q, R, x0, P0):
        self.F = F  # 状态转移矩阵
        self.H = H  # 观测矩阵
        self.Q = Q  # 状态噪声方差
        self.R = R  # 观测噪声方差
        self.x = x0  # 初始状态
        self.P = P0  # 初始协方差矩阵

    def predict(self):
        self.x = np.dot(self.F, self.x)
        self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q

    def update(self, z):
        y = z - np.dot(self.H, self.x)
        S = np.dot(np.dot(self.H, self.P), self.H.T) + self.R
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
        self.x += np.dot(K, y)
        self.P = self.P - np.dot(np.dot(K, self.H), self.P)

# 定义卡尔曼滤波模型参数
dt = 1.0  # 时间间隔
F = np.array([[1, dt], [0, 1]])  # 状态转移矩阵
H = np.array([[1, 0]])  # 观测矩阵
Q = np.array([[0.1, 0], [0, 0.1]])  # 状态噪声方差
R = np.array([1])  # 观测噪声方差
x0 = np.array([y[0], 0])  # 初始状态
P0 = np.eye(2)  # 初始协方差矩阵

# 进行卡尔曼滤波
kf = KalmanFilter(F, H, Q, R, x0, P0)
y_pred = np.zeros(n_samples)
for i in range(n_samples):
    kf.predict()
    kf.update(y[i])
    y_pred[i] = kf.x[0]

# 绘制粒子径迹和重建结果
plt.plot(x, y, 'b-', label='Particle Trajectory')
plt.plot(x, y_pred, 'r-', label='Kalman Filter')
plt.legend()
plt.show()

以上代码生成了随机粒子径迹数据，并使用卡尔曼滤波进行重建。首先定义了卡尔曼滤波模型和参数，然后进行卡尔曼滤波，并绘制了原始粒子径迹和重建结果。

需要注意的是，卡尔曼滤波需要根据实际问题选择合适的模型和参数，并根据具体需求进行调整。