用python写一个经典卡尔曼滤波算法

好的，以下是一个简单的Python实现：

import numpy as np

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

    def predict(self, u=0):
        # 预测状态
        self.x = np.dot(self.A, self.x) + np.dot(self.B, u)
        # 预测协方差
        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 = self.P - np.dot(np.dot(K, self.H), self.P)
        return self.x

其中，A是状态转移矩阵，B是控制矩阵，H是观测矩阵，Q是状态噪声协方差矩阵，R是观测噪声协方差矩阵，P是状态估计协方差矩阵，x0是初始状态向量。

predict函数用于预测下一时刻的状态，update函数用于根据观测值更新状态估计。