print("flight len:", len(flight_interp))
# load camera rotation rate data (derived from feature matching video
# frames)
feat_data = FeatureData()
feat_data.load(video_rates)
feat_data.smooth(smooth_cutoff_hz)
feat_data.make_interp()
if args.plot:
feat_data.plot()
feat_interp = feat_data.resample(args.resample_hz)
# find the time correlation of video vs flight data
time_shift = \
correlate.sync_gyros(flight_interp, feat_interp, feat_data.span_sec,
hz=hz, cam_mount=args.cam_mount,
force_time_shift=args.time_shift, plot=args.plot)
# quick estimate ground elevation
sum = 0.0
count = 0
for f in data['filter']:
air = interp.query(f['time'], 'air')
if air['airspeed'] < 5.0:
sum += f['alt']
count += 1
if count > 0:
ground_m = sum / float(count)
else:
ground_m = data['filter'][0].alt
print("ground est:", ground_m)
bounds_error=False,
fill_value=0.0)
r_interp = interpolate.interp1d(imu['time'],
imu['r'],
bounds_error=False,
fill_value=0.0)
alt_interp = interp.group['filter'].interp['alt']
for x in np.linspace(imu_min, imu_max, int(round(flight_len * hz))):
flight_interp.append([x, p_interp(x), q_interp(x), r_interp(x)])
print("flight len:", len(flight_interp))
# find the time correlation of video vs flight data
time_shift = \
correlate.sync_gyros(flight_interp, video_interp, horiz_len,
hz=hz, cam_mount=args.cam_mount,
force_time_shift=args.time_shift, plot=args.plot)
# optimizer stuffs
from scipy.optimize import least_squares
# Scan altitude range so we can match the portion of the flight that
# is up and away. This means the EKF will have had a better chance to
# converge, and the horizon detection should be getting a clear view.
min_alt = None
max_alt = None
for filt in flight_data['filter']:
alt = filt['alt']
if min_alt is None or alt < min_alt:
min_alt = alt
if max_alt is None or alt > max_alt: