def gps_kf(self, dimension=1, init_data=0):
"""
Invokes Kalman filter for 1D and 2D and plots graphs.
"""
abs_error = 10.0
try:
sensor = Sensor()
kf = KalmanFilter(error=abs_error)
# generate GPS observations.
obs, time = sensor.measure(init_data,
abs_error=abs_error,
unit='position')
# estimate position using Kalman filter
est = kf.estimate(obs)
# print out results and plot all.
self.mean, self.median, est_error = self.print_out(
obs, est, init_data)
self.plot(obs=obs,
est=est,
est_error=est_error,
time=time,
init_data=init_data,
dim=dimension,
sensor='gps')
except Exception as e:
print('Exception: '.upper(), e)
traceback.print_tb(e.__traceback__)
def imu_kf(self, dimension=1, init_data=0):
"""
Invokes Kalman filter for 1D and 2D and plots graphs.
"""
dt_gps = 1.0
dt_speed = 1.0
dt_accel = 1.0
gps_error = 50.0
try:
sensor = Sensor()
kf = KalmanFilter(error=gps_error)
# generate GPS observations.
gps_obs, time = sensor.measure(init_data,
abs_error=gps_error,
unit='position',
dt=dt_gps)
# estimate position using GPS data
est_gps = kf.estimate(observations=gps_obs)
# generate speedometer and accelerometer observations.
speed_obs, time = sensor.measure(init_data,
abs_error=0.1,
unit='velocity',
dt=dt_speed)
accel_obs, time = sensor.measure(init_data,
abs_error=0.01,
unit='acceleration',
dt=dt_accel)
# correct postion observations
gps_rows = len(est_gps[:, 0])
speed_rows = len(speed_obs[:, 0])
accel_rows = len(accel_obs[:, 0])
corrected_position = np.ones((accel_obs.shape))
k = 0
for i in range(gps_rows):
for j in range(int(accel_rows / gps_rows)):
corrected_position[k] = est_gps[i]
k += 1
k = 0
for i in range(speed_rows):
for j in range(int(accel_rows / speed_rows)):
corrected_position[k] += speed_obs[i] * dt_speed
k += 1
for i in range(accel_rows):
corrected_position[i] += accel_obs[i] * dt_accel**2 / 2
# estimate corrected position data
est = kf.estimate(observations=corrected_position)
# print out results.
self.mean, self.median, est_error = self.print_out(
corrected_position, est, init_data, dt_accel)
# plot results
self.plot(obs=corrected_position,
est=est,
est_error=est_error,
time=time,
init_data=init_data,
dim=dimension,
sensor='multisensor')
except Exception as e:
print('Exception: '.upper(), e)
traceback.print_tb(e.__traceback__)
