卡尔曼滤波python代码

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

import numpy as np

def kalman_filter(x, P):
    # 系统噪声协方差矩阵 Q
    Q = np.array([[0.01, 0],
                  [0, 0.01]])

    # 测量噪声协方差矩阵 R
    R = np.array([[0.1, 0],
                  [0, 0.1]])

    # 状态转移矩阵 A
    A = np.array([[1, 1],
                  [0, 1]])

    # 观测矩阵 H
    H = np.array([[1, 0],
                  [0, 1]])

    # 卡尔曼增益 K
    K = P.dot(H.T).dot(np.linalg.inv(H.dot(P).dot(H.T) + R))

    # 更新状态向量 x 和协方差矩阵 P
    x = x + K.dot(y - H.dot(x))
    P = (np.eye(2) - K.dot(H)).dot(P)

    # 预测下一时刻的状态向量 x 和协方差矩阵 P
    x = A.dot(x)
    P = A.dot(P).dot(A.T) + Q

    return x, P

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

# 测量结果
y = np.array([1, 1])

# 进行卡尔曼滤波
x, P = kalman_filter(x, P)

print(x)

这个例子演示了如何使用卡尔曼滤波来对一个二维状态向量进行滤波。在这个例子中，我们使用了一个简单的线性系统，其中状态向量包含两个元素，表示位置和速度。我们还使用了一个简单的观测矩阵，可以直接观测到位置和速度。在每个时刻，我们将测量结果作为输入，然后使用卡尔曼滤波来估计下一个时刻的状态向量。