python实现卡尔曼滤波算法的代码

以下是Python实现卡尔曼滤波算法的代码：

import numpy as np

class KalmanFilter:
    def __init__(self, A, H, Q, R, P, x):
        self.A = A    # 状态转移矩阵
        self.H = H    # 观测矩阵
        self.Q = Q    # 过程噪声协方差
        self.R = R    # 测量噪声协方差
        self.P = P    # 状态估计协方差矩阵
        self.x = x    # 初始状态估计

    def predict(self):
        # 预测状态
        self.x = np.dot(self.A, self.x)
        # 预测协方差矩阵
        self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.Q
        return self.x

    def update(self, z):
        # 计算卡尔曼增益
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(np.dot(np.dot(self.H, self.P), self.H.T) + self.R))
        # 更新状态估计
        self.x = self.x + np.dot(K, (z - np.dot(self.H, self.x)))
        # 更新协方差矩阵
        self.P = np.dot((np.eye(self.P.shape[0]) - np.dot(K, self.H)), self.P)
        return self.x

其中，A、H、Q、R、P、x 分别代表状态转移矩阵、观测矩阵、过程噪声协方差、测量噪声协方差、状态估计协方差矩阵和初始状态估计。predict()方法用于预测下一时刻的状态，update()方法用于根据当前时刻的观测值更新状态估计和协方差矩阵。