代码来自：https://www.cnblogs.com/zjuhjm/archive/2012/12/29/2838472.html
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
test_array = [0.618,0.672,0.905,0.985,1.028,1.255,1.320,1.396,1.724,1.753,1.769,2.091,2.113,2.367,2.414,2.817,2.840,2.897,3.073,3.254,3.289,4.717,4.407,5.113,5.200,5.091,4.967,5.580,5.888,6.338,6.473,6.542,5.795,5.907,5.850,4.987,5.028,5.028,4.987,5.291,5.084,5.156,5.481,5.200,4.507,4.575,4.541,3.457,3.450,3.477,2.884,2.596,2.983,3.012,2.753,2.468,2.453,1.358,1.281,1.249,0.908]
plt.plot(test_array)
adc=[]
for i in range(len(test_array)):
adc.append(kalman(test_array[i]))
plt.plot(adc)
plt.title('kalman')
#plt.plot(array)
