写一段卡尔曼滤波降噪的代码

以下是一个简单的卡尔曼滤波器降噪的Python代码示例：

import numpy as np

# 定义初始状态和噪声估计
x = np.array([[0.], [0.]])  # 状态向量
P = np.array([[1000., 0.], [0., 1000.]])  # 状态协方差矩阵
dt = 0.1  # 时间步长
A = np.array([[1., dt], [0, 1.]])  # 状态转移矩阵
H = np.array([[1., 0.]])  # 观测矩阵
R = np.array([[0.1]])  # 观测噪声协方差矩阵
Q = np.array([[0.01, 0.], [0., 0.01]])  # 系统噪声协方差矩阵

# 定义观测数据
measurements = [1.1, 1.9, 3.4, 3.5, 4.2, 4.7, 5.5, 6.1, 6.8, 7.2]

# 卡尔曼滤波
for i in range(len(measurements)):
    # 预测
    x = np.dot(A, x)
    P = np.dot(np.dot(A, P), A.T) + Q

    # 更新
    z = np.array([[measurements[i]]])
    y = z - 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(2) - np.dot(K, H)), P)

    # 打印结果
    print("预测值：", x[0][0])
    print("观测值：", measurements[i])

该示例中，我们使用了一个简单的一维观测数据集，通过卡尔曼滤波器对其进行降噪处理。在代码中，我们定义了状态向量、协方差矩阵、时间步长、状态转移矩阵、观测矩阵、观测噪声协方差矩阵和系统噪声协方差矩阵等参数。然后，我们对每个观测值进行了预测和更新，并输出了预测值和观测值。