kalman 滤波

日作诗歌三千首

于 2021-09-08 18:15:09 发布

阅读量86

点赞数

本文链接：https://blog.csdn.net/u012483097/article/details/120185972

版权

python

def kalman_filter(last_predict_point, last_P, input_point):
    # 系统观测误差，根据模型进行调节
    R = 0.05
    max_distance = 100
    eps = 0.001
    # 输入为前一个时刻的预测值、前一个时刻的预测误差，当前时刻的观测值
    # 输出为当前时刻的预测值、当前时刻的预测误差
    # 通过当前观测值与前一时刻预测值的距离，计算系统过程噪声
    distance = np.linalg.norm(input_point - last_predict_point)
    alpha = np.clip(distance / max_distance, 0.0, 1.0)
    # print(distance)
    # 系统过程噪声误差
    Q = alpha + eps
    # 系统预测误差
    P = last_P + Q
    # 卡尔曼增益
    K = P / (P + R)
    print(K)
    # 当前时刻的预测值
    cnt_predict_point = last_predict_point + K * (input_point - last_predict_point)
    # 更新当前时刻预测误差
    cnt_P = (1 - K) * P
    return cnt_predict_point, cnt_P