[code]一阶卡尔曼滤波python实现

# 1阶kalman
KF_PARAMS_A = 1  # 变换矩阵
KF_PARAMS_B = 0  # 外部输入为0 控制矩阵
KF_PARAMS_U = 0  # 外部输入为0 控制向量
KF_PARAMS_W = 0  # 噪声
KF_PARAMS_V = 0  # 噪声
KF_PARAMS_H = 1  # 传感器数据变换矩阵
KF_PARAMS_Q = 0.01  # 外部环境的干扰
KF_PARAMS_R = 0.05  # 噪声协方差
KF_PARAMS_P = 1  # 误差协方差矩阵


class KalmanBase:
    def __init__(self):
        self.A = KF_PARAMS_A  # 状态转移矩阵
        self.B = KF_PARAMS_B  # 控制输入模型
        self.u = KF_PARAMS_U  # 外部输入为0 控制向量
        self.w = KF_PARAMS_W  # 高斯噪声
        self.v = KF_PARAMS_V  # 高斯噪声
        self.H = KF_PARAMS_H  # 观测矩阵
        self.Q = KF_PARAMS_Q  # 状态噪声协方差矩阵
        self.R = KF_PARAMS_R  # 观测噪声协方差矩阵
        self.P = KF_PARAMS_P  # 误差协方差矩阵

        self.K = float('nan')  # 卡尔曼增益 无需初始化
        self.z = float('nan')  # 这里无需初始化，每次使用kf_update之前需要输入观察值z
        self.x = float('nan')  # 观测值

    def init_x(self, x):
        self.x = x  # 预测值 初始化为估计值

    def get_x(self):
        return self.x

    def kf_update(self, z):
        # init z
        self.z = self.H * z
        # P_ = APA' + Q
        a1 = self.A * self.P
        P_ = a1 * self.A + self.Q  # 误差协方差预测
        # K = P_ * H' / inv(H * P_* H' + R)
        b1 = P_ * self.H
        b2 = self.H * b1 + self.R
        self.K = b1 / b2
        # x = x_ + K(z - Hx_)
        c1 = self.B * self.u
        x_ = self.A * self.x + c1  # 状态预测
        c3 = self.z - self.H * x_
        self.x = x_ + self.K * c3  # 更新校正
        # P = (I - KH)P_
        d1 = 1 - self.K * self.H
        self.P = d1 * P_  # 更新误差协方差


if __name__ == '__main__':
	kalman_filter_params = KalmanBase()
    for i in range(迭代长度):
        if i == 0:
            kalman_filter_params.init_x(x=初始值)  # 初始化
        else:
            kalman_filter_params.update(z=观测值)
        kf_params_record[i] = kalman_filter_params.get_x() # 记录结果 用于画图