一阶卡尔曼滤波python实现——已封装

import matplotlib.pyplot as plt class KalmanBase: def __init__(self, W=0, V=0, A=1, H=1, P=1, B=0, U=0, Q=0, R=0, K=float('nan'), Z=float('nan'), X=float('nan')): self.A = A # 状态转移矩阵 self.w = W # 高斯噪声 self.v = V # 高斯噪声 self.H = H # 观测矩阵 self.P = P # 误差协方差矩阵 self.B = B # 控制输入模型 self.u = U # 外部输入为0 控制向量 self.Q = Q # 状态噪声协方差矩阵 self.R = R # 观测噪声协方差矩阵 self.K = K # 卡尔曼增益 无需初始化 self.z = Z # 这里无需初始化，每次使用kf_update之前需要输入观察值z self.x = X # 观测值 @property def P_(self): """ Error covariance prediction """ return self.A * self.P * self.A + self.Q @property def x_(self): """ state prediction """ return self.A * self.x + self.B * self.u def _init_z(self, z): # z = Hz self.z = self.H * z def _update_K(self): # K = P_ * H' / inv(H * P_* H' + R) self.K = self.P_ * self.H / (self.H * self.P_ * self.H + self.R) def _update_x(self): # x = x_ + K(z - Hx_) self.x = self.x_ + self.K * (self.z - self.H * self.x_) def _update_P(self): # P = (I - KH)P_ self.P = (1 - self.K * self.H) * self.P_ def kalman_update(self, x): self.x = x self._update_K() self._update_x() self._update_P() class KalmanFilter(KalmanBase): def __init__(self, W=0, V=0, A=1, H=1, P=1, B=0, U=0, Q=0, R=0): super(KalmanFilter, self).__init__(A, W, V, H, P, B, U, Q, R) self.x_set = [] # other parameter write below def update(self, x): self.kalman_update(x) self.x_set.append(self.x) def predict(self): return self.x_set def plot(self): fig = plt.figure() ax1 = fig.add_subplot(111) ax1.set(title='Result', ylabel='Value', xlabel='id') ax1.plot(range(len(self.x_set)), self.x_set, color='green') plt.show()