卡尔曼滤波的代码python

卡尔曼滤波是一种用于估计系统的算法，它结合了系统模型和测量数据，通过递归地更新状态估计值来提供最优的状态估计结果。下面是一个简单的卡尔曼滤波的Python代码示例：

import numpy as np

def kalman_filter(measurements):
    # 初始化状态估计值
    x = np.array([, ])
    # 初始化状态协方差矩阵
    P = np.array([[1, 0], [0, 1]])
    # 系统模型矩阵
    F = np.array([[1, 1], [0, 1]])
    # 测量模型矩阵
    H = np.array([[1, 0]])
    # 测量噪声协方差矩阵
    R = np.array([])
    # 过程噪声协方差矩阵
    Q = np.array([[0.0001, 0], [0, 0.0001]])

    filtered_measurements = []
    for measurement in measurements:
        # 预测步骤
        x = np.dot(F, x)
        P = np.dot(np.dot(F, P), F.T) + Q

        # 更新步骤
        y = measurement - 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)

        filtered_measurements.append(x)

    return filtered_measurements

# 测试代码
measurements = [1, 2, 3, 4, 5]
filtered_measurements = kalman_filter(measurements)
print(filtered_measurements)

这段代码实现了一个简单的一维卡尔曼滤波器，输入是一系列测量值，输出是经过滤波后的估计值。在代码中，我们首先初始化了状态估计值和协方差矩阵，然后通过预测步骤和更新步骤来递归地更新状态估计值。最后，我们将滤波后的估计值存储在`filtered_measurements`列表中并打印出来。