python实现卡尔曼滤波

最好的入门视频 没有之一
https://www.bilibili.com/video/BV1ez4y1X7eR/?spm_id_from=333.788.videocard.1

import numpy as np
import matplotlib.pyplot as plt
Q = 0.00001
R = 0.1
P_k_k1 = 1
Kg = 0
P_k1_k1 = 1
x_k_k1 = 0
ADC_OLD_Value = 0

kalman_adc_old = 0
def kalman(ADC_Value):
    global kalman_adc_old
    global P_k1_k1
    Z_k = ADC_Value
    
    if (abs(kalman_adc_old-ADC_Value)>=30):
        x_k1_k1= ADC_Value*0.382 + kalman_adc_old*0.618
    else:
        x_k1_k1 = kalman_adc_old;

    x_k_k1 = x_k1_k1
    P_k_k1 = P_k1_k1 + Q

    Kg = P_k_k1/(P_k_k1 + R)

    kalman_adc = x_k_k1 + Kg * (Z_k - kalman_adc_old)
    P_k1_k1 = (1 - Kg)*P_k_k1
    P_k_k1 = P_k1_k1

    ADC_OLD_Value = ADC_Value
    kalman_adc_old = kalman_adc
    return kalman_adc

    
a= [100]*200
array = np.array(a)

s = np.random.normal(0, 25, 200)

test_array = array + s
plt.plot(test_array)
adc=[]
for i in range(200):
    adc.append(kalman(test_array[i]))
    
plt.plot(adc)   
plt.plot(array)   
 

更进一步的卡尔曼滤波 参考https://blog.csdn.net/varyshare/article/details/97891376
卡尔曼滤波理论与实践 参考https://zhuanlan.zhihu.com/p/77327349