卡尔曼滤波python代码实例

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

import numpy as np

def kalman_filter(z):
    # 初始化
    x = np.array([[0.], [0.]])  # 状态向量，初始值为 0
    P = np.array([[1., 0.], [0., 1.]])  # 状态协方差矩阵，初始值为单位矩阵
    F = np.array([[1., 1.], [0, 1.]])  # 状态转移矩阵
    H = np.array([[1., 0.]])  # 观测矩阵
    R = np.array([[1.]])  # 观测噪声协方差矩阵
    I = np.identity(2)  # 单位矩阵

    # 预测
    x = np.dot(F, x)
    P = np.dot(np.dot(F, P), F.T)

    # 更新
    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((I - np.dot(K, H)), P)  # 更新状态协方差矩阵

    return x[0][0], P[0][0]

# 测试
measurements = [1, 2, 3, 4, 5]  # 测量值
results = []
for z in measurements:
    x, P = kalman_filter(z)
    results.append(x)
print(results)

以上代码实现了一个简单的一维卡尔曼滤波器。在实际应用中，需要根据具体问题进行调整和优化。