卡尔曼滤波器python实现

好的，以下是一个简单的卡尔曼滤波器的 Python 实现：

import numpy as np

class KalmanFilter:
    def __init__(self, A, H, Q, R, P):
        self.A = A  # 状态转移矩阵
        self.H = H  # 观测矩阵
        self.Q = Q  # 状态噪声协方差矩阵
        self.R = R  # 观测噪声协方差矩阵
        self.P = P  # 初始估计误差协方差矩阵
        self.x = np.zeros((A.shape[1], 1))  # 状态向量

    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

    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 = self.P - np.dot(np.dot(K, self.H), self.P)

    def filter(self, zs):
        for z in zs:
            self.predict()
            self.update(z)
            yield self.x

其中，`A` 为状态转移矩阵，`H` 为观测矩阵，`Q` 为状态噪声协方差矩阵，`R` 为观测噪声协方差矩阵，`P` 为初始估计误差协方差矩阵，`x` 为状态向量。`predict` 函数用来进行预测，`update` 函数用来进行更新，`filter` 函数用来进行过滤。在 `filter` 函数中，`zs` 表示观测值序列，`yield` 语句用来生成一个迭代器，每次产生一个状态向量。