import numpy as np
import random
import matplotlib.pyplot as plt
#config plt
plt.rc('font', size=10) # controls default text sizes
plt.rc('axes', titlesize=30) # fontsize of the axes title
plt.rc('axes', labelsize=25) # fontsize of the x and y labels
plt.rc('xtick', labelsize=18) # fontsize of the tick labels
plt.rc('ytick', labelsize=18) # fontsize of the tick labels
plt.rc('legend', fontsize=20) # legend fontsize
plt.rc('figure', titlesize=35) # fontsize of the figure title
T = 1
N = 200
cov_mat = np.array(([10000, 0], [0, 10000]))
cov_Q = np.zeros((200, 2))
acc = 0.2**2
var = 20**2
Fi = np.array([[1, T], [0, 1]])
G = np.array((T**2 / 2, T))
z = np.zeros(N)
H = np.array([[1, 0]])
cov_Q = G[:,np.newaxis].dot(G[np.newaxis,:]) * acc
def generating_true_traj(N, Fi, G, init, rand_acc, plot = False, q = None):
X = np.zeros((2,N))
X[:, 0] = init
for i in range(1, N):
if q is not None:
acc = random.gauss(0, np.sqrt(rand_acc)) + q
else:
acc = random.gauss(0, np.sqrt(rand_acc))
X[:, i] = Fi.dot(X[:, i - 1]) + acc * G
if plot:
figure, ax = plt.subplots(figsize = (16,9))
ax.set_title(f'True trajectory of {N} points')
ax.plot(X[0], linewidth=3, color = 'red', label = 'coordinate')
ax.plot(X[1], linewidth=3, color = 'blue', label = 'velocity')
ax.set_xlabel('number of points')
ax.set_ylabel('data values')
ax.grid()
ax.legend()
plt.show()
return X
def generating_measurments (true_traj, H = np.array([[1,0]]), var_noise=0, plot = False ) :
Ni = true_traj.shape[1]
z = np.zeros(Ni)
for i in range(Ni):
z[i] = H.dot(true_traj[:, i]) + random.gauss(0, np.sqrt(var_noise))
if plot:
figure, ax = plt.subplots(figsize = (16,9))
ax.set_title(f'Measurements of {N} points')
ax.plot(z, linewidth=3, color = 'orange')
ax.set_xlabel('number of points')
ax.set_ylabel('measurements of coordinate')
ax.grid()
plt.show()
return z
class Kalman():
def __init__( self, Fi, H, cov_Q, cov_r ):
self.Fi = Fi
self.H = H
self.cov_Q = cov_Q
self.cov_r = cov_r
def calc_Kalman(self, meas, k_con = None,
X_f0 = None, P_f0=None, cov_h = None,
ret = False, plot = False, k_flag = False, true = None, retpf = None):
# X_p - Prediction X matrix
# X_f - filtered X matrix
# P_p - Prediction error covariance matrix
# P_f - Filtration error covariance matrix
# Finish doc
# Do Recalculation of P matrixes !!!
# Do recalculations of X matrixes !!!
N = len(meas)
Fi = self.Fi
H = self.H
cov_Q = self.cov_Q
cov_r = self.cov_r
X_p = np.zeros((N, 2, 1))
P_f = np.zeros((N, 2, 2))
if P_f0 is not None:
P_f[0] = P_f0
else:
P_f[0] = [[10000, 0], [0, 10000]]
P_p = np.zeros((N, 2, 2))
X_f = np.zeros((N, 2, 1))
K = np.zeros((N, 2, 1))
Pf_x = np.zeros(N)
if X_f0 is not None:
X_f[0] = X_f0
else:
X_f[0] = [[2], [0]]
for i in range(1, N):
X_p[i] = Fi.dot(X_f[i - 1]) #+ G.dot([0.2, 0])
P_p[i] = (Fi.dot(P_f[i - 1])).dot(Fi.T) + cov_Q
K[i] = (P_p[i].dot(H.T)).dot(np.linalg.inv((H.dot(P_p[i])).dot(H.T) + cov_r))
X_f[i] = X_p[i] + K[i].dot(float(meas[i] - H.dot(X_p[i])))
P_f[i] = (np.eye(2) - K[i].dot(H)).dot(P_p[i])
#print(P_f[i][0][0])
Pf_x[i] = np.sqrt(abs(P_f[i][0][0]))
# Update P,X and other matrixes of Class
self.X_p = X_p
self.X_f = X_f
self.P_p = P_p
self.P_f = P_f
self.Pf_x = Pf_x
self.K = K
if plot:
figur, ax = plt.subplots(figsize = (16,9))
ax.set_title('Kalman filtration')
ax.plot(X_f[:, 0], linewidth=3, color = 'red', label = 'filtered')
ax.plot(meas, linewidth=3, color = 'blue', label = 'measurements')
ax.plot(true[0], linewidth=3, color = 'green', label = 'true trajectory')
ax.set_xlim([140, 190])
ax.set_ylim([2600, 3700])
ax.set_xlabel('number of points')
ax.set_ylabel('data')
ax.grid()
ax.legend()
plt.show()
if k_con:
graph, ax = plt.subplots(figsize = (16,9))
ax.set_title('Kalman filtration')
ax.set_xlabel('number of points')
ax.set_ylabel('data')
ax.plot(K[:, 1], linewidth=3, color = 'red', label = 'filter gain K')
arr = np.zeros(200)
for i in range(200):
arr[i] = P_f[i][0][0] ** 0.5
ax.plot(arr, linewidth=3, color = 'green', label = 'error cavariance matrix')
ax.grid()
ax.legend()
plt.show()
if ret:
return X_f
if retpf == 1:
return P_f
def plot_errors(num_of_points, X, KalmanObj, ret = None, q = None, plot = None):
run_error = np.zeros((num_of_points, len(X[0])))
final_error = np.zeros(len(X[0]) - 3)
for i in range(num_of_points):
if q is not None:
true_data = generating_true_traj(N, Fi, G, [5, 1], acc, q = 0.2)
else:
true_data = generating_true_traj(N, Fi, G, [5, 1], acc)
data = generating_measurments(true_data, var_noise = var)
Xf = KalmanObj.calc_Kalman(data, ret = 1)
for j in range(3, len(X[0])):
run_error[i][j-3] = (true_data[0][j] - Xf[j][0])**2
for i in range(0, len(X[0]) - 3):
final_error[i] = np.sqrt((1 / (num_of_points - 1)) * np.sum(run_error[:, i]))
if plot == 1:
graph, ax = plt.subplots(figsize = (16,9))
ax.set_title('Final error plot')
ax.plot(final_error, linewidth=3, color = 'red')
ax.set_xlabel('number of points')
ax.set_ylabel('Final error')
ax.grid()
plt.show()
print("Mean-sqared error ", ((X[0] - Xf)**2).mean())
print("Standart deviation ", np.std(Xf))
if ret is not None:
return final_error
true_data = generating_true_traj(N, Fi, G, [5, 1], acc, q = 0.2, plot = 1)
data = generating_measurments(true_data, var_noise = var, plot = 1)
KKK = Kalman(Fi, H, cov_Q, var)
XXX = KKK.calc_Kalman(data, plot = True, ret = 1, true = true_data)
data_err = plot_errors(500, true_data, KKK, q = 1, ret = 1)
data_norm = plot_errors(500, true_data, KKK, ret = 1)
Pf = KKK.calc_Kalman(data, retpf = 1, true = true_data)
k = Pf[:, 0, 0] ** 0.5
graph, ax = plt.subplots(figsize = (16,9))
ax.set_title('Final error plot')
ax.plot(k, linewidth=3, color = 'blue', label = 'Estimated average error')
ax.plot(data_err, linewidth=3, color = 'red', label = 'True average error (biased)')
ax.plot(data_norm, linewidth=3, color = 'green', label = 'True average error (unbiased)')
ax.set_xlabel('number of points')
ax.set_ylabel('Final error')
ax.legend()
ax.grid()
plt.show()
data_norm = plot_errors(500, true_data, KKK, ret = 1)
Pf = KKK.calc_Kalman(data, retpf = 1, true = true_data)
k = Pf[:, 0, 0] ** 0.5
graph, ax = plt.subplots(figsize = (16,9))
ax.set_title('Final error plot')
ax.plot(k, linewidth=3, color = 'blue', label = 'Estimated average error')
ax.plot(data_norm, linewidth=3, color = 'green', label = 'True average error (unbiased)')
ax.set_xlabel('number of points')
ax.set_ylabel('Final error')
ax.legend()
ax.grid()
plt.show()