- added

2020-12-13 15:53:46 +01:00
parent 752bb18ef5
commit c401885967
1 changed files with 45 additions and 0 deletions
@@ -0,0 +1,45 @@
+import numpy as np
+from matplotlib.pyplot import plot, figure, subplot, legend, grid, show
+
+
+T_true = 72
+T_est = 68
+E_est = 2
+E_mea = 0.5
+k_var = 0.5
+
+_t = np.empty(0)
+_T_true = np.empty(0)
+_KG = np.empty(0)
+_T_est = np.empty(0)
+_E_est = np.empty(0)
+
+for t in range(0, 1000):
+	_t = np.append(_t, t)
+	_T_true = np.append(_T_true, T_true)
+
+	T_meas = T_true + k_var*np.random.randn()
+
+	# 1: Calculate Kalman gain KG
+	KG = E_est / (E_est + E_mea)
+
+	# 2: Calculate estimation T_est
+	T_est = T_est + KG*(T_meas - T_est)
+
+	# 3: Calulate error in estimate E_est
+	E_est = (1-KG)*E_est
+
+	_KG = np.append(_KG, KG)
+	_T_est = np.append(_T_est, T_est)
+	_E_est = np.append(_E_est, E_est)
+
+figure(1)
+subplot(2, 1, 1)
+plot(_t, _T_est, 'b-', _t, _T_true, '-r', linewidth=1)
+legend(["T_est", "T_true"])
+grid(True)
+subplot(2, 1, 2)
+plot(_t, _E_est, 'b-', _t, _KG, '-r', linewidth=1)
+legend(["E_est", "KG"])
+grid(True)
+show()