def test_predict():
assert KalmanFilter.predict(63, 8, 119, 2) == (182, 10)
assert KalmanFilter.predict(77, 1, 106, 3) == (183, 4)
assert KalmanFilter.predict(91, 10, 80, 5) == (171, 15)
assert KalmanFilter.predict(51, 1, 52, 4) == (103, 5)
assert KalmanFilter.predict(9, 6, 59, 5) == (68, 11)
assert KalmanFilter.predict(38, 3, 113, 5) == (151, 8)
assert KalmanFilter.predict(51, 2, 87, 2) == (138, 4)
assert KalmanFilter.predict(38, 8, 96, 3) == (134, 11)
assert KalmanFilter.predict(71, 4, 53, 5) == (124, 9)
assert KalmanFilter.predict(9, 10, 69, 2) == (78, 12)
def test_kalman_filter_linear_data(plot=False):
barometer = Barometer()
PREDICTION_BUFFER = 3
PREDICTION_SIGMA = 3
mu = 0
sigma = 1000
prev_mu = mu
# For plotting
true_altitudes = []
pred_altitudes = []
gps_altitudes = []
barometer_altitudes = []
lr = LinearRegression()
with open(str(DATA / "linear_flight.jsonl"), "r") as f:
for line in f:
sample = json.loads(line)
if "gps_altitude" in sample:
gps_altitude = sample["gps_altitude"]
gps_sigma = sample["gps_variance"]
mu, sigma = KalmanFilter.update(mu, sigma, gps_altitude,
gps_sigma)
barometer_altitude = barometer.altitude(sample["pressure"])
mu, sigma = KalmanFilter.update(mu, sigma, barometer_altitude,
barometer.variance)
print(
f"Update: [{mu}, {sigma}], true altitude {sample['altitude']}")
xx = np.array(list(range(PREDICTION_BUFFER))).reshape(-1, 1)
if len(pred_altitudes) <= PREDICTION_BUFFER * 4:
delta = mu - prev_mu
else:
yy = np.array(copy(
pred_altitudes[-PREDICTION_BUFFER:])).reshape(-1, 1)
lr.fit(xx, yy)
new_mu = lr.predict([[PREDICTION_BUFFER - 1]])
print(f"new: {new_mu}")
delta = new_mu - prev_mu
pred_mu, pred_sigma = KalmanFilter.predict(mu, sigma, delta,
PREDICTION_SIGMA)
prev_mu = mu
mu = pred_mu
sigma = pred_sigma
pred_altitudes.append(mu)
if plot:
true_altitudes.append(sample["altitude"])
if "gps_altitude" in sample:
gps_altitudes.append(gps_altitude)
else:
gps_altitudes.append(None)
barometer_altitudes.append(barometer_altitude)
final_altitude = sample["altitude"]
# print the final, resultant mu, sig
print("\n")
print(f"Final result: [{mu}, {sigma}], true altitude {final_altitude}")
if plot:
import matplotlib.pyplot as plt
pred_altitudes = pred_altitudes[1:]
smooth_pred_altitudes = smooth(np.array(pred_altitudes),
window_len=3,
window="flat")
plt.figure()
plt.plot(gps_altitudes, "k+", label="gps measurements")
plt.plot(barometer_altitudes, "r+", label="barometer measurements")
plt.plot(pred_altitudes, "b-", label="predicted altitudes")
plt.plot(smooth_pred_altitudes,
"m-",
label="smoothed predicted altitudes")
plt.plot(true_altitudes, "g-", label="true altitudes")
plt.title("Estimated Altitude", fontweight="bold")
plt.xlabel("Time Step")
plt.ylabel("Altitude (m)")
plt.legend()
plt.show()
# assert that final prediction is within 5% of true altitude
assert mu - final_altitude < final_altitude * 0.05
def predict(self):
_, delta = self.motion_model.predict(self.prev_altitudes)
mu, sigma = KalmanFilter.predict(self.mu, self.sigma, delta,
self.motion_model.variance)
self.prev_altitudes.append(mu)
self.prev_variances.append(sigma)