利用Python实现卡尔曼滤波算法_卡尔曼滤波目标跟踪python

1、实现代码##

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)>=80):
        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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

2、仿真结果##

运行下述程序：

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)   
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

运行结果

绿线为原值，黄线为滤波后的数据，蓝线为加入噪声的数据。

3、改进版##

import numpy as np
import matplotlib.pyplot as plt
class kalman_filter:
    def __init__(self,Q,R):
        self.Q = Q
        self.R = R
        
        self.P_k_k1 = 1
        self.Kg = 0
        self.P_k1_k1 = 1
        self.x_k_k1 = 0
        self.ADC_OLD_Value = 0
        self.Z_k = 0
        self.kalman_adc_old=0
        
    def kalman(self,ADC_Value):
       
        self.Z_k = ADC_Value
        
        if (abs(self.kalman_adc_old-ADC_Value)>=60):
            self.x_k1_k1= ADC_Value*0.382 + self.kalman_adc_old*0.618
        else:
            self.x_k1_k1 = self.kalman_adc_old;
    
        self.x_k_k1 = self.x_k1_k1
        self.P_k_k1 = self.P_k1_k1 + self.Q
    
        self.Kg = self.P_k_k1/(self.P_k_k1 + self.R)
    
        kalman_adc = self.x_k_k1 + self.Kg * (self.Z_k - self.kalman_adc_old)
        self.P_k1_k1 = (1 - self.Kg)*self.P_k_k1
        self.P_k_k1 = self.P_k1_k1
    
        self.kalman_adc_old = kalman_adc
        
        return kalman_adc
        
if __name__ == '__main__':
    kalman_filter =  kalman_filter(0.001,0.1)
    a= [100]*200
    array = np.array(a)
    
    s = np.random.normal(0, 15, 200)
    test_array = array + s
    adc=[]
    for i in range(200):
        adc.append(kalman_filter.kalman(test_array[i]))
    
    plt.plot(adc)   
    plt.plot(array)
    plt.plot(test_array)
    plt.show()
    
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53