四个位置状态的匀速卡尔曼滤波模型 python代码

以下是一个简单的四个位置状态的匀速卡尔曼滤波模型 Python 代码示例：

import numpy as np

# 定义模型参数
dt = 0.1  # 时间步长
F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])  # 状态转移矩阵
H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])  # 观测矩阵
Q = np.array([[0.01, 0, 0, 0], [0, 0.01, 0, 0], [0, 0, 0.01, 0], [0, 0, 0, 0.01]])  # 状态噪声协方差
R = np.array([[1, 0], [0, 1]])  # 观测噪声协方差

# 初始化状态和协方差矩阵
x = np.array([0, 0, 0, 0])  # 初始状态
P = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])  # 初始协方差矩阵

# 定义观测数据
data = np.array([[1.2, 0.8], [2.1, 1.9], [3.3, 3.1], [4.2, 4.5], [5.1, 5.2]])

# 开始滤波
for i in range(len(data)):
    # 预测
    x = np.dot(F, x)
    P = np.dot(F, np.dot(P, F.T)) + Q

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

    print("预测状态：", x)
    print("协方差矩阵：", P)

该模型使用了 NumPy 库来进行矩阵运算。在代码中，使用了一个简单的观测数据，每次循环将观测数据与模型状态进行比较，并根据卡尔曼滤波算法进行预测和更新。