49 lines
948 B
Python
49 lines
948 B
Python
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()
|