import numpy as np
from matplotlib.pyplot import plot, figure, subplot, legend, grid, show


T_true = 72
T_est = 68
E_est = 4
E_mea = 2

_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, 10000):
	_t = np.append(_t, t)
	_T_true = np.append(_T_true, T_true)

	T_meas = T_true + E_mea*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: Calculate 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(3, 1, 1)
plot(_t, _T_est, 'b-', _t, _T_true, '-r', linewidth=1)
legend(["T_est", "T_true"])
grid(True)
subplot(3, 1, 2)
plot(_t, _E_est, 'b-', linewidth=1)
legend(["E_est"])
grid(True)
subplot(3, 1, 3)
plot(_t, _KG, '-r', linewidth=1)
legend(["KG"])
grid(True)
show()