卡尔曼滤波python代码

以下是一个简单的 Python 实现卡尔曼滤波的代码：

import numpy as np

# 初始化
def kalman_filter(x, P):
    # 测量噪声
    Q = np.array([[0.0001, 0], [0, 0.0001]])
    # 状态转移矩阵
    F = np.array([[1, 1], [0, 1]])
    # 测量矩阵
    H = np.array([[1, 0]])
    # 测量噪声协方差
    R = np.array([[1]])

    for n in range(len(measurements)):
        # 预测
        x = np.dot(F, x)
        P = np.dot(np.dot(F, P), F.T) + Q

        # 更新
        y = measurements[n] - 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)

    return x, P

# 测量值
measurements = [1, 2, 3, 4, 5]

# 初始状态
x = np.array([[0], [0]])
P = np.array([[1000, 0], [0, 1000]])

# 运行滤波器
x, P = kalman_filter(x, P)

print("最终状态估计：")
print("x = ", x)
print("P = ", P)

在这个代码示例中，我们使用了一个简单的一维卡尔曼滤波器来估计一组测量值的状态。我们首先定义了一些必要的矩阵和向量，然后在一个循环中运行滤波器。在每次循环中，我们先执行预测步骤，然后使用测量值进行更新。最后，我们输出滤波器的最终状态估计。

请注意，这个代码示例只是一个简单的实现，实际的卡尔曼滤波器可能需要更多的调整和优化，以适应不同的应用场景。