python实现卡尔曼滤波的代码

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

import numpy as np

class KalmanFilter:
    def __init__(self, F, B, H, Q, R, X0, P0):
        self.F = F   # 状态转移矩阵
        self.B = B   # 控制输入矩阵
        self.H = H   # 测量矩阵
        self.Q = Q   # 过程噪声协方差矩阵
        self.R = R   # 测量噪声协方差矩阵
        self.X = X0  # 初始状态向量
        self.P = P0  # 初始协方差矩阵

    def predict(self, U):
        self.X = self.F @ self.X + self.B @ U
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.X

    def update(self, Z):
        K = self.P @ self.H.T @ np.linalg.inv(self.H @ self.P @ self.H.T + self.R)
        self.X = self.X + K @ (Z - self.H @ self.X)
        self.P = (np.eye(self.P.shape[0]) - K @ self.H) @ self.P
        return self.X

其中，`F`是状态转移矩阵，`B`是控制输入矩阵，`H`是测量矩阵，`Q`是过程噪声协方差矩阵，`R`是测量噪声协方差矩阵，`X0`是初始状态向量，`P0`是初始协方差矩阵。

`predict`方法用于预测下一个状态，`U`是控制输入。

`update`方法用于根据测量值`Z`更新卡尔曼滤波器的状态。

可以使用以下代码创建一个卡尔曼滤波器，并对一些数据进行滤波：

# create a Kalman filter
F = np.array([[1, 1], [0, 1]])
B = np.array([[0], [0]])
H = np.array([[1, 0]])
Q = np.array([[0.01, 0], [0, 0.01]])
R = np.array([[0.1]])
X0 = np.array([0, 0])
P0 = np.eye(2) * 1000
kf = KalmanFilter(F, B, H, Q, R, X0, P0)

# filter some data
data = [1.2, 1.4, 1.6, 1.8, 2.0]
filtered_data = []
for z in data:
    # predict
    kf.predict(np.array([[0]]))
    # update
    x = kf.update(np.array([[z]]))
    filtered_data.append(x[0])

在上述代码中，我们使用一个简单的一维卡尔曼滤波器对一些数据进行滤波，最终得到了滤波后的数据`filtered_data`。