一维线性kalman滤波算法实例_改进R变化

东方耀

一维线性kalman滤波算法实例_改进R变化
思考步骤：
1、状态变量 x1=距离/位置(米)   先写出一个系统的离散的状态空间方程表达
    x1还是应该为 速度
2、线性kalman对数据要求很严格 要求整段数据的方差是固定的  Q R矩阵的取值问题
    后验估计是：谁方差小 就更信任谁  但是这个问题的数据是 刚开始方差小 中间方差大 后面方差又小
    改变：能不能让Q R 矩阵的方差 随这数据 在变  可以的    测量噪声:协方差矩阵R 可以根据测量结果改变
    由于得不到真实的值 所以Q的方差 不好变 只能固定一个经验值
    发现：初值很关键 很敏感啊

1. # -*- coding: utf-8 -*-
   
2. __author__ = u'东方耀 微信：dfy_88888'
   
3. __date__ = '2022/2/23 上午11:45'
   
4. __product__ = 'PyCharm'
   
5. __filename__ = 'j04_kalman_filter_状态变量(动目标的距离)'
   
6. 
   
7. import numpy as np
   
8. import matplotlib.pyplot as plt
   
9. import matplotlib.ticker as mticker
   
10. from matplotlib.font_manager import FontProperties
    
11. 
    
12. font_fname = "/usr/share/fonts/wps-office/simfang.ttf"
    
13. # font_fname = "C:\Windows\Fonts\simfang.ttf"
    
14. font = FontProperties(fname=font_fname)
    
15. # plt.rcParams['font.sans-serif']=['simfang']#设置作图中文显示
    
16. 
    
17. '''
    
18. 一维线性kalman滤波算法实例_改进R变化
    
19. 思考步骤：
    
20. 1、状态变量 x1=距离/位置(米)   先写出一个系统的离散的状态空间方程表达
    
21.     x1还是应该为 速度
    
22. 2、线性kalman对数据要求很严格 要求整段数据的方差是固定的  Q R矩阵的取值问题
    
23.     后验估计是：谁方差小 就更信任谁  但是这个问题的数据是 刚开始方差小 中间方差大 后面方差又小
    
24.     改变：能不能让Q R 矩阵的方差 随这数据 在变  可以的    测量噪声:协方差矩阵R 可以根据测量结果改变
    
25.     由于得不到真实的值 所以Q的方差 不好变 只能固定一个经验值
    
26.     发现：初值很关键 很敏感啊
    
27. '''
    
28. 
    
29. save_distance_csv_file = "/media/jiang/AI_Data/jjj_matlab_works/ZTR/actual_used_data/python/multi_frame_147_distance.csv"
    
30. distance = np.loadtxt(save_distance_csv_file, delimiter=',')
    
31. 
    
32. prt = 450 * 700e-6
    
33. 
    
34. # 状态转移矩阵，上一时刻的状态转移到当前时刻
    
35. # 数学模型中得到的 这是固定的
    
36. A = np.array([[1]])
    
37. 
    
38. # 测量矩阵  从测量或传感器的方程得到 也是固定的
    
39. H = np.array([[1]])
    
40. 
    
41. # 过程噪声:协方差矩阵Q， p(w)~N(0,Q)，噪声来自真实世界中的不确定性
    
42. # 位置与速度是相互独立的 所以只有对角线有值
    
43. Q = np.array([[0.08]])
    
44. 
    
45. # 测量噪声:协方差矩阵R， p(v)~N(0,R)
    
46. R = np.array([[100.0]])
    
47. 
    
48. len_counter_R_var = 5
    
49. 
    
50. # 初始速度 0 行吗？ 改为1呢？
    
51. X0 = np.array([[1.0]])
    
52. # 初始 误差的协方差矩阵
    
53. P = np.array([[300.0]])
    
54. 
    
55. # ---------------------------初始化-------------------------
    
56. X_posterior_est = np.array(X0)
    
57. P_posterior = np.array(P)  # 不停在更新 在变
    
58. 
    
59. # 真实值
    
60. # speed_true = []  # 不可能知道
    
61. 
    
62. speed_measure = []  # 测量值
    
63. 
    
64. speed_prior_est = []  # prior 先验估计值
    
65. 
    
66. speed_posterior_est = []  # posterior 后验估计值
    
67. 
    
68. distance_kalman = distance.copy()
    
69. 
    
70. distance_kalman[0] = 0
    
71. distance_kalman[1] = distance[1]
    
72. 
    
73. for i in range(2, len(distance)):
    
74.     # abs(distance[i] - distance[i-1])
    
75.     # w = np.array([[gaussian_distribution_generator(Q[0, 0])]])
    
76.     # X_true = np.dot(A, X_true) + w
    
77.     # speed_true.append(X_true[0, 0])
    
78.     Z_measure = np.array([[np.abs(distance[i] - distance[i - 1]) / prt]])
    
79.     speed_measure.append(Z_measure[0, 0])
    
80. 
    
81.     # assert 0 == 1, "停"
    
82.     # ----------------------进行先验估计  真正的kalman算法---------------------
    
83.     X_prior_est = np.dot(A, X_posterior_est)  # X_posterior_est需要初值
    
84.     speed_prior_est.append(X_prior_est[0, 0])
    
85.     if (len(speed_measure) >= len_counter_R_var):
    
86.         print("(第%d次循环)测量系统的方差：" % i, np.var(np.array(speed_measure[-len_counter_R_var:])))
    
87.         # Q[0, 0] = np.var(np.array(speed_prior_est[-len_counter_R_var:]))   这样改不行 是真实值的方差
    
88.         R[0, 0] = np.var(np.array(speed_measure[-len_counter_R_var:]))
    
89. 
    
90.     # 计算先验误差的协方差矩阵P
    
91.     # print("第%d次使用的误差的协方差矩阵P(几次后会稳定下来)=\n" % i, P_posterior)
    
92.     P_prior_1 = np.dot(A, P_posterior)  # P_posterior 需要初值
    
93.     P_prior = np.dot(P_prior_1, A.T) + Q
    
94.     # ----------------------计算卡尔曼增益,用numpy一步一步计算Prior and posterior
    
95.     k1 = np.dot(P_prior, H.T)
    
96.     k2 = np.dot(np.dot(H, P_prior), H.T) + R
    
97.     K = np.dot(k1, np.linalg.inv(k2))  # np.linalg.inv()：矩阵求逆
    
98.     # print("第%d次计算得到的卡尔曼增益K(几次后会稳定下来)=\n" % i, K)
    
99.     # ---------------------后验估计------------
    
100.     X_posterior_est_1 = Z_measure - np.dot(H, X_prior_est)
     
101.     # X的后验估计 = X的先验估计 + 系数 * (测量值 - H * X的先验估计)
     
102.     X_posterior_est = X_prior_est + np.dot(K, X_posterior_est_1)
     
103.     print("第%d次循环的kalman滤波结果(Q=%.2f,R=%.2f)：先验估计的速度=%.1f,测量的速度=%.1f,后验估计的速度=%.2f" % (i, Q[0, 0], R[0, 0], X_prior_est[0, 0], Z_measure[0, 0], X_posterior_est[0, 0]))
     
104.     # X_posterior_est 真正要的结果
     
105.     if (distance[i] - distance[i - 1] >= 0):
     
106.         distance_kalman[i] = distance_kalman[i - 1] + X_posterior_est[0, 0] * prt
     
107.     else:
     
108.         distance_kalman[i] = distance_kalman[i - 1] - X_posterior_est[0, 0] * prt
     
109. 
     
110.     speed_posterior_est.append(X_posterior_est[0, 0])
     
111.     # 更新(误差)协方差矩阵P
     
112.     P_posterior_1 = np.eye(1) - np.dot(K, H)
     
113.     P_posterior = np.dot(P_posterior_1, P_prior)
     
114. 
     
115. # print("测量出来的速度：", speed_measure, np.var(np.array(speed_measure)))
     
116. 
     
117. 
     
118. 
     
119. plt.figure(figsize=(14, 12))
     
120. plt.figure(1)
     
121. 
     
122. plt.plot(speed_measure, color='black', marker='o', markersize=6, label="speed_measure")
     
123. plt.plot(speed_posterior_est, color='red', marker='o', markersize=6, label="speed_posterior_est")
     
124. plt.plot(speed_prior_est, color='blue', marker='o', markersize=6, label="speed_prior_est")
     
125. plt.xlabel("状态变化次数(积累帧数-1)", fontproperties=font, fontsize=15)
     
126. plt.ylabel("动目标速度", fontproperties=font, fontsize=15)
     
127. plt.title('卡尔曼滤波的1d实例_动目标速度(kalman状态变量)', fontproperties=font, fontsize=16)
     
128. plt.legend()
     
129. 
     
130. 
     
131. plt.figure(figsize=(14, 12))
     
132. plt.figure(2)
     
133. plt.xlabel("帧索引(slow_time)", fontproperties=font, fontsize=15)
     
134. plt.ylabel("距离(米)", fontproperties=font, fontsize=15)
     
135. plt.title('目标轨迹(径向距离变化_argmax算法)_kalman速度转距离', fontproperties=font, fontsize=16)
     
136. plt.plot(distance, color='black', marker='o', markersize=6, label="distance_measure")
     
137. plt.plot(distance_kalman, color='red', marker='o', markersize=6, label="distance_kalman")
     
138. plt.legend()
     
139. 
     
140. plt.show()
     

复制代码

ljdxls111

fasfsdfsdfsdafasdfas

ljdxls111

eafawfawefwfwafawfawf

ljdxls111

dsfdsfdsfdsfdsfdsfdsfsdfds

ljdxls111

dfasdfsdfdsfasdfdsafdasf

ljdxls111

dssdfdsafdsafdsafsadfdsafdasfdsaf

ljdxls111

sdfdsfdsfdsfdsdsfds