1 # -*- coding: utf-8 -*-
 2 import numpy as np
 3 import matplotlib.pyplot as plt
 4
 5 # Initial State x0
 6 x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T
 7
 8 # Initial Uncertainty P0
 9 P = np.diag([1000.0, 1000.0, 1000.0, 1000.0])
10
11 dt = 0.1 # Time Step between Filter Steps
12
13 # Dynamic Matrix A
14 A = np.matrix([[1.0, 0.0, dt, 0.0],
15               [0.0, 1.0, 0.0, dt],
16               [0.0, 0.0, 1.0, 0.0],
17               [0.0, 0.0, 0.0, 1.0]])
18
19 # Measurement Matrix
20 # We directly measure the velocity vx and vy
21 H = np.matrix([[0.0, 0.0, 1.0, 0.0],
22               [0.0, 0.0, 0.0, 1.0]])
23
24 # Measurement Noise Covariance
25 ra = 10.0**2
26 R = np.matrix([[ra, 0.0],
27               [0.0, ra]])
28
29 # Process Noise Covariance
30 # The Position of the car can be influenced by a force (e.g. wind), which leads
31 # to an acceleration disturbance (noise). This process noise has to be modeled
32 # with the process noise covariance matrix Q.
33 sv = 8.8
34 G = np.matrix([[0.5*dt**2],
35                [0.5*dt**2],
36                [dt],
37                [dt]])
38 Q = G*G.T*sv**2
39
40 I = np.eye(4)
41
42 # Measurement
43 m = 200 # 200个测量点
44 vx= 20  # in X
45 vy= 10  # in Y
46 mx = np.array(vx+np.random.randn(m))
47 my = np.array(vy+np.random.randn(m))
48 measurements = np.vstack((mx,my))
49
50 # Preallocation for Plotting
51 xt = []
52 yt = []
53
54
55 # Kalman Filter
56 for n in range(len(measurements[0])):
57
58     # Time Update (Prediction)
59     # ========================
60     # Project the state ahead
61     x = A*x
62
63     # Project the error covariance ahead
64     P = A*P*A.T + Q
65
66
67     # Measurement Update (Correction)
68     # ===============================
69     # Compute the Kalman Gain
70     S = H*P*H.T + R
71     K = (P*H.T) * np.linalg.pinv(S)
72
73     # Update the estimate via z
74     Z = measurements[:,n].reshape(2,1)
75     y = Z - (H*x)                            # Innovation or Residual
76     x = x + (K*y)
77
78     # Update the error covariance
79     P = (I - (K*H))*P
80
81
82     # Save states for Plotting
83     xt.append(float(x[0]))
84     yt.append(float(x[1]))
85
86
87 # State Estimate: Position (x,y)
88 fig = plt.figure(figsize=(16,16))
89 plt.scatter(xt,yt, s=20, label='State', c='k')
90 plt.scatter(xt[0],yt[0], s=100, label='Start', c='g')
91 plt.scatter(xt[-1],yt[-1], s=100, label='Goal', c='r')
92
93 plt.xlabel('X')
94 plt.ylabel('Y')
95 plt.title('Position')
96 plt.legend(loc='best')
97 plt.axis('equal')
98 plt.show()