filterpy中线性卡尔曼滤波算法_实战_可视化

东方耀

filterpy中线性卡尔曼滤波算法_实战_可视化

1. # -*- coding: utf-8 -*-
   
2. __author__ = u'东方耀 微信：dfy_88888'
   
3. __date__ = '2022/3/2 下午14:50'
   
4. __product__ = 'PyCharm'
   
5. __filename__ = '01_filterpy跟踪狗_实战完整'
   
6. 
   
7. import math
   
8. import numpy as np
   
9. from filterpy.stats import plot_covariance
   
10. from filterpy.kalman import KalmanFilter, EKF, UKF
    
11. from filterpy.common import Q_discrete_white_noise
    
12. import matplotlib.pyplot as plt
    
13. import matplotlib.ticker as mticker
    
14. import random
    
15. from matplotlib.font_manager import FontProperties
    
16. 
    
17. # font_fname = "C:\Windows\Fonts\simfang.ttf"
    
18. font_fname = "/usr/share/fonts/wps-office/simfang.ttf"
    
19. font = FontProperties(fname=font_fname)
    
20. 
    
21. 
    
22. # filterpy中线性卡尔曼滤波算法_实战_可视化
    
23. np.random.seed(88)
    
24. 
    
25. 
    
26. def randomcolor():
    
27.     colorArr = ['1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F']
    
28.     color = ""
    
29.     for i in range(6):
    
30.         color += colorArr[random.randint(0, 14)]
    
31.     return "#" + color
    
32. 
    
33. 
    
34. def compute_dog_data(z_var, process_var, count=1, dt=1.):
    
35.     "returns track, measurements 1D ndarrays"
    
36.     x, vel = 0., 1.   # 这是构造数据的初始位置 与 理想速度
    
37.     # 观测值的 标准差
    
38.     z_std = math.sqrt(z_var)
    
39.     # p_std 速度变化的标准差
    
40.     p_std = math.sqrt(process_var)
    
41.     xs, zs = [], []
    
42.     for _ in range(count):
    
43.         # 每次的速度在1m/s上下波动
    
44.         v = vel + (np.random.randn() * p_std)
    
45.         # x为真实的位置
    
46.         x += v * dt
    
47.         xs.append(x)
    
48.         zs.append(x + np.random.randn() * z_std)
    
49.     # xs:狗真实的位置   zs：观测的位置
    
50.     return np.array(xs), np.array(zs)
    
51. 
    
52. 
    
53. def pos_vel_filter(x, P, R, Q=0., dt=1.0):
    
54.     """ Returns a KalmanFilter which implements a
    
55.     constant velocity model for a state [x dx].T
    
56.     位置position 速度vel 滤波器
    
57.     """
    
58.     # dim_x的状态向量的维度(位置 速度)   dim_z是测量的维度(位置)
    
59.     # 带有dim_x的状态向量和测量向量的大小
    
60.     kf = KalmanFilter(dim_x=2, dim_z=1)
    
61.     kf.x = np.array([x[0], x[1]])  # 这是初始的location and velocity
    
62.     kf.F = np.array([[1., dt],
    
63.                      [0., 1.]])  # state transition matrix
    
64.     kf.H = np.array([[1., 0]])  # Measurement function
    
65.     kf.R *= R  # measurement uncertainty  因为R的维度为1
    
66.     if np.isscalar(P):
    
67.         kf.P *= P  # covariance matrix
    
68.     else:
    
69.         kf.P[:] = P  # [:] makes deep copy
    
70.     if np.isscalar(Q):
    
71.         kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q)
    
72.         print("Q过程噪声协方差矩阵kf.Q：", kf.Q)
    
73.     else:
    
74.         kf.Q[:] = Q
    
75.     return kf
    
76. 
    
77. 
    
78. def run(x0=(0., 0.), P=500, R=0, Q=0, dt=1.0,
    
79.         track=None, zs=None,
    
80.         count=0, do_plot=True, **kwargs):
    
81.     """
    
82.     track is the actual position of the dog, zs are the
    
83.     corresponding measurements.
    
84.     """
    
85. 
    
86.     # Simulate dog if no data provided.
    
87.     if zs is None:
    
88.         # 生成 真实的track 测量值zs
    
89.         track, zs = compute_dog_data(z_var=R, process_var=Q, count=count, dt=dt)
    
90. 
    
91.     # create the Kalman filter
    
92.     kf = pos_vel_filter(x0, R=R, P=P, Q=Q, dt=dt)
    
93.     print("构建的卡尔曼滤波器对象：\n", kf.__repr__)
    
94. 
    
95.     # run the kalman filter and store the results
    
96.     xs, cov = [], []
    
97.     for z in zs:
    
98.         kf.predict()
    
99.         # # P = (I-KH)P(I-KH)' + KRK'
    
100.         #         # This is more numerically stable
     
101.         #         # and works for non-optimal K vs the equation
     
102.         #         # P = (I-KH)P usually seen in the literature.
     
103.         kf.update(z)
     
104.         # xs是后验估计值  最优估计值
     
105.         xs.append(kf.x)
     
106.         # 看 P矩阵的变化
     
107.         cov.append(kf.P)
     
108. 
     
109.     xs, cov = np.array(xs), np.array(cov)
     
110. 
     
111.     # if do_plot:
     
112.     #     plot_track(xs[:, 0], track, zs, cov, **kwargs)
     
113.     return track, zs, xs, cov
     
114. 
     
115. 
     
116. x0 = (10., 10.)  # 初始的位置与速度
     
117. P = np.diag([500., 49.])  # 初始的误差协方差矩阵P
     
118. frame_num = 50
     
119. x_position_true, x_position_measure, X_kalman_est, all_P_cov_mat = run(x0=x0, count=frame_num, R=10, Q=1, P=P)
     
120. 
     
121. print("所有的真实的位置：", x_position_true.shape)
     
122. print("所有的测量的位置：", x_position_measure.shape)
     
123. print("所有的后验估计值(位置与速度)：", X_kalman_est.shape)
     
124. print("所有的误差的协方差矩阵P：", all_P_cov_mat.shape)
     
125. 
     
126. my_color = []
     
127. for _ in range(frame_num):
     
128.     # 每一帧的颜色不一样
     
129.     my_color.append(randomcolor())
     
130. 
     
131. 
     
132. # 画图展示
     
133. plt.figure(figsize=(14, 12))
     
134. plt.figure(1)
     
135. plt.xlabel("位置", fontproperties=font, fontsize=15)
     
136. plt.ylabel("速度", fontproperties=font, fontsize=15)
     
137. 
     
138. # plot_covariance([20.0, 10.], np.diag([50, 50]), edgecolor='r')
     
139. plot_covariance(x0, P, edgecolor='r', ls='dashed')
     
140. 
     
141. for i in range(X_kalman_est.shape[0]):
     
142.     plt.title('误差的协方差矩阵P的变化(协方差椭圆)_第%d帧' % i, fontproperties=font, fontsize=16)
     
143.     # print("第%d帧的后验估计值：" % i, X_kalman_est[i, :])
     
144.     # print("第%d帧的误差协方差矩阵：" % i, all_P_cov_mat[i, :])
     
145.     plot_covariance(X_kalman_est[i, :], all_P_cov_mat[i, :], edgecolor=my_color[i])
     
146.     plt.pause(0.01)
     
147.     # if(i >= 3):
     
148.     #     plt.pause(100000000)
     
149. 
     
150. 
     
151. plt.figure(figsize=(14, 12))
     
152. plt.figure(2)
     
153. plt.xlabel("时间", fontproperties=font, fontsize=15)
     
154. plt.ylabel("位置", fontproperties=font, fontsize=15)
     
155. plt.title('线性kalman滤波算法', fontproperties=font, fontsize=16)
     
156. # plt.ylim()
     
157. plt.plot(x_position_true, color="g", linestyle=":", marker='o', markersize=6, label="x_position_true")
     
158. plt.plot(x_position_measure, color="b", linestyle=":", marker='o', markersize=6, label="x_position_measure")
     
159. print("看看位置改变的顺序：", X_kalman_est[:8, 0])
     
160. plt.plot(X_kalman_est[:, 0], color="r", linestyle="-", marker='o', markersize=6, label="x_position_kalman_est")
     
161. plt.legend()
     
162. 
     
163. 
     
164. plt.show()
     

复制代码

东方耀

在这个问题中，我们将跟踪狗的位置和速度  状态向量
状态变量可以是观测变量（由传感器直接测量），也可以是隐藏变量（从观测变量推断）
对于我们的狗跟踪问题，传感器只读取位置，所以位置被观察，速度被隐藏

重要的是要理解，跟踪位置和速度是一种设计选择，具有我们尚未准备探索的含义和假设
例如，我们还可以追踪加速度，甚至是急动
卡尔曼滤波器如何计算隐藏变量的估计

在最后一章中，我们证明了位置和速度是相关的。
但是对于一只狗来说，它们有多大的相关性呢？我不知道。正如我们将看到的，过滤器会为我们计算这个，
所以我将协方差初始化为零(非对角线上的)。当然，如果你知道协方差，你应该使用它们

卡尔曼滤波器不只是过滤数据，它允许我们将机器人和飞机等系统的控制输入结合起来

协方差矩阵

东方耀

协方差矩阵P 告诉我们滤波器的理论性能
标准偏差是方差的平方根，大约68%的高斯分布发生在一个标准偏差内
我们可以用这段代码实现非常复杂的多维过滤器，只需改变过滤器变量的赋值即可。也许我们想追踪金融模型中的100个维度
或者我们有一架带有GPS、INS、塔康、雷达高度计、气压高度计和空速指示器的飞机，
我们希望将所有这些传感器集成到一个模型中，以预测3D空间中的位置、速度和加速度