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
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
|
import numpy as np
from matplotlib import pyplot as plt
def disturb(scale):
"""
返回一个指定标准差的正太扰动
:param scale: 标准差
:return:
"""
return np.random.normal(0, scale)
def calc_prior_hx(A, hx_k_1, B, u_k_1):
"""
计算先验估计值
:param A: 预测使用的状态转移矩阵
:param hx_k_1: 上一次的后验估计值
:param B: 将输入转为状态的转移矩阵
:param u_k_1: 系统输入
:return: 先验估计值
"""
hx_prior_k = np.dot(A, hx_k_1)
if B is not None and u_k_1 is not None:
hx_prior_k += np.dot(B, u_k_1)
return hx_prior_k
def calc_prior_P(A, P_k_1, Q):
"""
计算先验状态估计协方差矩阵
:param A: 预测使用的状态转移矩阵
:param P_k_1: 上一次的后验状态估计协方差矩阵
:param Q: 预测过程的噪声协方差矩阵
:return: 先验的状态估计协方差矩阵
"""
P_prior_k = np.dot(A, P_k_1)
P_prior_k = np.dot(P_prior_k, A.T) + Q
return P_prior_k
def kalman_gain(P_prior_k, H, R):
"""
计算kalman gain卡尔曼增益
:param P_prior_k: 先验的状态估计协方差矩阵
:param H: 状态变量到测量值的转换矩阵
:param R: 测量噪声协方差矩阵
:return: 卡尔曼增益系数矩阵
"""
up = np.dot(P_prior_k, H.T)
down = np.dot(H, P_prior_k)
down = np.dot(down, H.T) + R
K_k = np.dot(up, np.linalg.inv(down))
return K_k
def calc_posterior_hx(hx_prior_k, K_k, z_k, H):
"""
计算后验估计值
:param hx_prior_k: 先验估计值
:param K_k: 卡尔曼增益系数矩阵
:param z_k: 测量值
:param H: 状态变量到测量值的转换矩阵
:return: 后验估计值
"""
hx_k = hx_prior_k + np.dot(K_k, (z_k - np.dot(H, hx_prior_k)))
return hx_k
def update_posterior_P(K_k, H, P_prior_k):
"""
更新后验状态估计协方差矩阵
:param K_k: 卡尔曼增益
:param H: 状态变量到测量值的转换矩阵
:param P_prior_k: 先验的状态估计协方差矩阵
:return: 后验状态估计协方差矩阵
"""
P_k = P_prior_k - np.dot(np.dot(K_k, H), P_prior_k)
return P_k
if __name__ == "__main__":
"""
kalman filter 案例实践
"""
position_true = []
position_measure = []
position_prior_est = []
position_posterior_est = []
speed_true = []
speed_measure = []
speed_prior_est = []
speed_posterior_est = []
A = np.array([1, 1, 0, 1]).reshape([2, 2])
B, u_k_1 = None, None
hx_k_1 = np.array([0, 1]).reshape([2, 1])
P_k_1 = np.array([1, 0, 0, 1]).reshape([2, 2])
Q = np.array([5, 0, 0, 5]).reshape((2, 2))
H = np.array([1, 0, 0, 1]).reshape([2, 2])
R = np.array([[1, 0, 0, 1]]).reshape([2, 2])
x_k = np.array([3, 3]).reshape([2, 1]) + np.array([disturb(Q[0][0]), disturb(Q[1][1])])
for i in range(30):
if i != 0:
x_k = np.dot(A, x_k) + np.array([disturb(Q[0][0]), disturb(Q[1][1])])
position_true.append(x_k[0][0])
speed_true.append(x_k[1][0])
z_k = x_k + np.array([disturb(R[0][0]), disturb(R[1][1])])
position_measure.append(z_k[0][0])
speed_measure.append(z_k[1][0])
hx_prior_k = calc_prior_hx(A, hx_k_1, B, u_k_1)
P_prior_k = calc_prior_P(A, P_k_1, Q)
K_k = kalman_gain(P_prior_k, H, R)
hx_k = calc_posterior_hx(hx_prior_k, K_k, z_k, H)
P_k = update_posterior_P(K_k, H, P_prior_k)
position_prior_est.append(hx_prior_k[0][0])
speed_prior_est.append(hx_prior_k[1][0])
position_posterior_est.append(hx_k[0][0])
speed_posterior_est.append(hx_k[1][0])
hx_k_1 = hx_k
P_k_1 = P_k
if True:
fig, axs = plt.subplots(1, 2)
axs[0].plot(speed_true, "-", label="speed_true", linewidth=1)
axs[0].plot(speed_measure, "-", label="speed_measure", linewidth=1)
axs[0].plot(speed_prior_est, "-", label="speed_prior_est", linewidth=1)
axs[0].plot(speed_posterior_est, "-", label="speed_posterior_est", linewidth=1)
axs[0].set_title("speed")
axs[0].set_xlabel('k')
axs[0].legend()
axs[1].plot(position_true, "-", label="position_true", linewidth=1)
axs[1].plot(position_measure, "-", label="position_measure", linewidth=1)
axs[1].plot(position_prior_est, "-", label="position_prior_est", linewidth=1)
axs[1].plot(position_posterior_est, "-", label="position_posterior_est", linewidth=1)
axs[1].set_title("position")
axs[1].set_xlabel('k')
axs[1].legend()
plt.show()
|
