卡尔曼滤波算法的个人学习

开始

最近开始接触到电池SOC的估算问题，无意中了解到卡尔曼滤波算法，在此记录一下自己的学习过程。
学习参考大部分来自B站，发现一位大佬讲的很不错，这里贴一下地址，有兴趣的同学可以去详细学习。
以下内容为我根据大佬的讲解，使用python验证自己的理解，加深印象所用，对于卡尔曼滤波原理我只是初步了解和测试，因此有错误的地方欢迎各位老铁指正！
大佬的视频：【卡尔曼滤波器】1_递归算法_Recursive Processing

我的代码测试

下面是我的代码测试，公式解释已经放在注释中，测量值实际随机了1010次

import random

# 一枚硬币实际宽度为50mm 估计初值为40mm 估计误差为5mm 测量初值为51mm 测量误差为3mm
E_esti = 5  # 估计误差
E_meas = 3  # 测量误差
K_k = 1  # 卡尔曼增益

X_k = 40
Z_k = 51

# 步骤1：计算卡尔曼增益  K_k = E_esti/(E_esti + E_meas)
# 步骤2：计算当前估计结果  x^k = x^(k-1) + K_k(Z_k - x^(k-1))
# 步骤3：更新当前估计误差 E_esti = (1 - K_k)E_esti

measure_list = [52, 47, 48, 49, 50, 53, 51, 52, 54, 52, 51]

for i in range(1, 1000):
    x = random.randint(47, 54)
    measure_list.append(x)

for i, measure in enumerate(measure_list):
    K_k = E_esti / (E_esti + E_meas)
    X_k = X_k + K_k * (Z_k - X_k)
    E_esti = (1 - K_k) * E_esti
    Z_k = measure
    print("第{}次循环的结果为{}".format(i, X_k))

实际测试的结果如下

可以看到，第十次之后估算值已经开始在真值附近震荡，而之后的数值也一直如此，随着迭代的增加，预估值将一直努力处于稳定，震荡会减少，但显然不会消失。