fake_timestamp = way_points[0].time.timestamp()
fake_time_start = fake_timestamp
start_real_time = time.time()
prev_time = start_real_time
video_kalman.initialize(
np.array([utm_data[0], utm_data[1], 0, 0]).T,
np.array([4, 4, 3, 3]).T, start_real_time)
index = 1
video.set(1, 100)
current_image = ""
while index < len(way_points):
if fake_timestamp >= way_points[index].time.timestamp():
if way_points[index].latitude > 0:
utm_data = utm.from_latlon(way_points[index].latitude,
way_points[index].longitude)
video_kalman.update_estimations(np.array(utm_data[:2]).T)
print(way_points[index])
index += 1
success, image = video.read()
if not success:
print("Error in reading video!", file=sys.stderr)
break
cv.imshow("Good Weather", image)
video_kalman.predict_values()
zone_num, zone_letter = utm_data[2], utm_data[3]
lat, long = utm.to_latlon(video_kalman.predicted_data[0],
video_kalman.predicted_data[1], zone_num,
zone_letter)
nearest_data = geographic_db.retrieve_nearest_point(long, lat)
if nearest_data is not None:
def update_using_kalman(kalman: KalmanFilter, metadata):
output_data = []
last_point = metadata[0]
last_utm = utm.from_latlon(last_point.get("latitude", None),
last_point.get("longitude", None))
start_time = last_point['time']
similar_data_count = 1
for data in metadata[1:]:
if data["time"] != last_point["time"]:
data_utm = utm.from_latlon(data.get("latitude", None),
data.get("longitude", None))
if data["speed"] is not None:
speed_vec = normalize_vec(
np.array([data_utm[0], data_utm[1]]) -
np.array(last_utm)) * data["speed"]
new_data = np.array(
[data_utm[0], data_utm[1], speed_vec[0], speed_vec[1]]).T
else:
new_data = np.array([data_utm[0], data_utm[1]]).T
last_utm = data_utm[:2]
if data["accuracy"] is not None:
noise_vec = [data["accuracy"] * 0.71, data["accuracy"] * 0.71]
else:
noise_vec = None
kalman.update_estimations(new_data=new_data,
measurement_noise=noise_vec,
estimate_time=data["time"])
last_point = data
print(last_point['time'] - start_time, similar_data_count,
data.get("latitude", None), data.get("longitude", None))
similar_data_count = 1
predicted_data = kalman.predicted_data
predicted_utm = predicted_data[:2]
predicted_lat_lng = utm.to_latlon(predicted_utm[0],
predicted_utm[1], data_utm[2],
data_utm[3])
output_data.append({
'latitude': predicted_lat_lng[0],
'longitude': predicted_lat_lng[1],
'path': data['path']
})
else:
new_time = data['time'] + 0.25 * similar_data_count
data_utm = utm.from_latlon(data.get("latitude", None),
data.get("longitude", None))
last_utm = data_utm[:2]
kalman.predict_values(new_time)
predicted_data = kalman.predicted_data
predicted_utm = predicted_data[:2]
predicted_lat_lng = utm.to_latlon(predicted_utm[0],
predicted_utm[1], data_utm[2],
data_utm[3])
output_data.append({
'latitude': predicted_lat_lng[0],
'longitude': predicted_lat_lng[1],
'path': data['path']
})
similar_data_count += 1
return output_data