a_2 = np.hstack((np.zeros(eye.shape), eye, eye * h))
a_3 = np.hstack((np.zeros(eye.shape), np.zeros(eye.shape), eye))
a = np.vstack((a_1, a_2, a_3))
# Get measurements
r_pv, q_pv, c_pv, y_pv = dataset.get_pos_vel_gaussian(0, 10, 0, 0.2)
r_g, q_g, c_g, y_g = dataset.get_position_gaussian(0, 10)
r_u, q_u, c_u, y_u = dataset.get_position_uniform(10)
# Instantiate Kalman Filters
kalman_gaussian = KalmanFilter(r_g, q_g, y_g, x0, p0, a, b, c_g)
kalman_uniform = KalmanFilter(r_u, q_u, y_u, x0, p0, a, b, c_u)
kalman_velocity = KalmanFilter(r_pv, q_pv, y_pv, x0, p0, a, b, c_pv)
# Get predictions, covariance matrices and Kalman gains
x_g, p_g, kn_g = kalman_gaussian.get_prediction()
x_u, p_u, kn_u = kalman_uniform.get_prediction()
x_pv, p_pv, kn_pv = kalman_velocity.get_prediction()
# MSE calculation for different scenarios
mse = MSE()
mse_gaussian_position = np.zeros((9, 100))
mse_uniform = np.zeros((9, 100))
mse_gaussian_velocity = np.zeros((9, 100))
for i in range(100):
r_pv, q_pv, c_pv, y_pv = dataset.get_pos_vel_gaussian(0, 10, 0, 0.2)
r_g, q_g, c_g, y_g = dataset.get_position_gaussian(0, 10)
r_u, q_u, c_u, y_u = dataset.get_position_uniform(10)
kalman_gaussian = KalmanFilter(r_g, q_g, y_g, x0, p0, a, b, c_g)
