├── Figure_1.png ├── README.md └── main.py /Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/liuchangji/2D-Kalman-Filter-Example_Dr_CAN_in_python/6d2bd0c3f794d203c10b879ccda37224538ab04a/Figure_1.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 2D-Kalman-Filter-Example_Dr_CAN_in_python 2 | 3 | Dr Can卡尔曼滤波例子的python实现 4 | 例子的简单实现 5 | 6 | 视频地址：https://www.bilibili.com/video/BV1dV411B7ME/?spm_id_from=333.788.recommend_more_video.1 7 | 8 | 9 | ![image](https://github.com/liuchangji/2D-Kalman-Filter-Example_Dr_CAN_in_python/blob/main/Figure_1.png) 10 | 11 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | # https://zhuanlan.zhihu.com/p/48876718 2 | # https://www.bilibili.com/video/BV1dV411B7ME?share_source=copy_web 3 | # DR_CAN例子的python实现 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | 7 | 8 | def gaussian_distribution_generator(var): 9 | return np.random.normal(loc=0.0, scale=var, size=None) 10 | 11 | 12 | # 状态转移矩阵，上一时刻的状态转移到当前时刻 13 | A = np.array([[1, 1], 14 | [0, 1]]) 15 | 16 | # 过程噪声协方差矩阵Q，p(w)~N(0,Q)，噪声来自真实世界中的不确定性 17 | Q = np.array([[0.1, 0], 18 | [0, 0.1]]) 19 | 20 | # 观测噪声协方差矩阵R，p(v)~N(0,R) 21 | R = np.array([[1, 0], 22 | [0, 1]]) 23 | 24 | # 状态观测矩阵 25 | H = np.array([[1, 0], 26 | [0, 1]]) 27 | 28 | # 控制输入矩阵B 29 | B = None 30 | # 初始位置与速度 31 | X0 = np.array([[0], 32 | [1]]) 33 | 34 | # 状态估计协方差矩阵P初始化 35 | P = np.array([[1, 0], 36 | [0, 1]]) 37 | 38 | if __name__ == "__main__": 39 | # ---------------------------初始化------------------------- 40 | X_true = np.array(X0) # 真实状态初始化 41 | X_posterior = np.array(X0) 42 | P_posterior = np.array(P) 43 | 44 | speed_true = [] 45 | position_true = [] 46 | 47 | speed_measure = [] 48 | position_measure = [] 49 | 50 | speed_prior_est = [] 51 | position_prior_est = [] 52 | 53 | speed_posterior_est = [] 54 | position_posterior_est = [] 55 | 56 | for i in range(30): 57 | # -----------------------生成真实值---------------------- 58 | # 生成过程噪声 59 | w = np.array([[gaussian_distribution_generator(Q[0, 0])], 60 | [gaussian_distribution_generator(Q[1, 1])]]) 61 | X_true = np.dot(A, X_true) + w # 得到当前时刻状态 62 | speed_true.append(X_true[1, 0]) 63 | position_true.append(X_true[0, 0]) 64 | # -----------------------生成观测值---------------------- 65 | # 生成观测噪声 66 | v = np.array([[gaussian_distribution_generator(R[0, 0])], 67 | [gaussian_distribution_generator(R[1, 1])]]) 68 | 69 | Z_measure = np.dot(H, X_true) + v # 生成观测值,H为单位阵E 70 | position_measure.append(Z_measure[0, 0]) 71 | speed_measure.append(Z_measure[1, 0]) 72 | # ----------------------进行先验估计--------------------- 73 | X_prior = np.dot(A, X_posterior) 74 | position_prior_est.append(X_prior[0, 0]) 75 | speed_prior_est.append(X_prior[1, 0]) 76 | # 计算状态估计协方差矩阵P 77 | P_prior_1 = np.dot(A, P_posterior) 78 | P_prior = np.dot(P_prior_1, A.T) + Q 79 | # ----------------------计算卡尔曼增益,用numpy一步一步计算Prior and posterior 80 | k1 = np.dot(P_prior, H.T) 81 | k2 = np.dot(np.dot(H, P_prior), H.T) + R 82 | K = np.dot(k1, np.linalg.inv(k2)) 83 | # ---------------------后验估计------------ 84 | X_posterior_1 = Z_measure - np.dot(H, X_prior) 85 | X_posterior = X_prior + np.dot(K, X_posterior_1) 86 | position_posterior_est.append(X_posterior[0, 0]) 87 | speed_posterior_est.append(X_posterior[1, 0]) 88 | # 更新状态估计协方差矩阵P 89 | P_posterior_1 = np.eye(2) - np.dot(K, H) 90 | P_posterior = np.dot(P_posterior_1, P_prior) 91 | 92 | 93 | 94 | # 可视化显示 95 | if True: 96 | fig, axs = plt.subplots(1,2) 97 | axs[0].plot(speed_true, "-", label="speed_true", linewidth=1) # Plot some data on the axes. 98 | axs[0].plot(speed_measure, "-", label="speed_measure", linewidth=1) # Plot some data on the axes. 99 | axs[0].plot(speed_prior_est, "-", label="speed_prior_est", linewidth=1) # Plot some data on the axes. 100 | axs[0].plot(speed_posterior_est, "-", label="speed_posterior_est", linewidth=1) # Plot some data on the axes. 101 | axs[0].set_title("speed") 102 | axs[0].set_xlabel('k') # Add an x-label to the axes. 103 | axs[0].legend() # Add a legend. 104 | 105 | axs[1].plot(position_true, "-", label="position_true", linewidth=1) # Plot some data on the axes. 106 | axs[1].plot(position_measure, "-", label="position_measure", linewidth=1) # Plot some data on the axes. 107 | axs[1].plot(position_prior_est, "-", label="position_prior_est", linewidth=1) # Plot some data on the axes. 108 | axs[1].plot(position_posterior_est, "-", label="position_posterior_est", linewidth=1) # Plot some data on the axes. 109 | axs[1].set_title("position") 110 | axs[1].set_xlabel('k') # Add an x-label to the axes. 111 | axs[1].legend() # Add a legend. 112 | 113 | plt.show() 114 | --------------------------------------------------------------------------------