卡尔曼滤波 python代码

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

import numpy as np

class KalmanFilter:
    def __init__(self, F, H, Q, R, x0, P0):
        self.F = F
        self.H = H
        self.Q = Q
        self.R = R
        self.x = x0
        self.P = P0

    def predict(self):
        self.x = np.dot(self.F, self.x)
        self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q

    def update(self, z):
        y = z - np.dot(self.H, self.x)
        S = np.dot(np.dot(self.H, self.P), self.H.T) + self.R
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
        self.x = self.x + np.dot(K, y)
        self.P = np.dot(np.eye(self.P.shape[0])-np.dot(K, self.H), self.P)

# 测试代码
dt = 1.0/60.0
F = np.array([[1, dt], [0, 1]])
H = np.array([1, 0]).reshape(1, 2)
Q = np.array([[1, 0], [0, 1]])
R = np.array([0.01]).reshape(1, 1)
x = np.array([0, 0]).reshape(2, 1)
P = np.diag((0.01, 0.01))
kf = KalmanFilter(F, H, Q, R, x, P)

measurements = [1.1, 1.9, 3.1, 3.9, 5.0, 5.9, 7.1]
for z in measurements:
    kf.predict()
    kf.update(z)
    print(kf.x)

该代码实现了一个简单的一维卡尔曼滤波器，用于估计一个运动物体的位置。我们首先定义了状态转移矩阵 `F`、测量矩阵 `H`、过程噪声协方差矩阵 `Q`、测量噪声协方差矩阵 `R`、初始状态向量 `x0` 和初始状态协方差矩阵 `P0`。然后我们定义了一个 `KalmanFilter` 类，它包含了卡尔曼滤波的预测和更新步骤。在测试代码中，我们假设从某个传感器中得到了一些测量，然后用卡尔曼滤波器对其进行处理。

注意，这个代码只是一个简单的例子，实际上卡尔曼滤波器的实现可能会更加复杂，具体取决于应用场景和需要解决的问题。