def add_gps_measurement(self, altitude, variance, time=None):
if time is None or time >= len(self.prev_altitudes):
mu, sigma = KalmanFilter.update(self.mu, self.sigma, altitude,
variance)
self.prev_altitudes.append(mu)
self.prev_variances.append(sigma)
self.predict()
else:
self.prev_altitudes[time], self.prev_variances[
time] = KalmanFilter.update(self.prev_altitudes[time],
self.prev_variances[time],
altitude, variance)
def test_update():
assert KalmanFilter.update(14, 2, 95, 4) == (41.0, 1.3333333333333333)
assert KalmanFilter.update(8, 3, 99, 3) == (53.5, 1.5)
assert KalmanFilter.update(23, 6, 65, 2) == (54.5, 1.5)
assert KalmanFilter.update(77, 6, 101, 4) == (91.4, 2.4000000000000004)
assert KalmanFilter.update(57, 9, 119,
2) == (107.72727272727273, 1.6363636363636362)
assert KalmanFilter.update(46, 1, 108, 3) == (61.5, 0.75)
assert KalmanFilter.update(20, 9, 120,
5) == (84.28571428571429, 3.2142857142857144)
assert KalmanFilter.update(69, 5, 92, 3) == (83.375, 1.875)
assert KalmanFilter.update(56, 2, 111, 3) == (78.0, 1.2000000000000002)
assert KalmanFilter.update(95, 9, 59,
4) == (70.07692307692308, 2.769230769230769)
def add_barometer_measurement(self, pressure, time=None):
barometer_altitude = self.barometer.altitude(pressure)
if time is None or time >= len(self.prev_altitudes):
mu, sigma = KalmanFilter.update(self.mu, self.sigma,
barometer_altitude,
self.barometer.variance)
self.prev_altitudes.append(mu)
self.prev_variances.append(sigma)
self.predict()
else:
self.prev_altitudes[time], self.prev_variances[
time] = KalmanFilter.update(self.prev_altitudes[time],
self.prev_variances[time],
barometer_altitude,
self.barometer.variance)
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