diff --git a/components/pid/kalman_eval.py b/components/pid/kalman_eval.py new file mode 100644 index 0000000..f350530 --- /dev/null +++ b/components/pid/kalman_eval.py @@ -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()