kalman处理realsense数据

代码来自：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)