import numpy as np import os from bisect import bisect from tqdm import tqdm from cffi import FFI TEMPLATE_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'templates')) GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'generated')) def write_code(name, code, header): if not os.path.exists(GENERATED_DIR): os.mkdir(GENERATED_DIR) open(os.path.join(GENERATED_DIR, f"{name}.cpp"), 'w').write(code) open(os.path.join(GENERATED_DIR, f"{name}.h"), 'w').write(header) def load_code(name): shared_fn = os.path.join(GENERATED_DIR, f"lib{name}.so") header_fn = os.path.join(GENERATED_DIR, f"{name}.h") header = open(header_fn).read() ffi = FFI() ffi.cdef(header) return (ffi, ffi.dlopen(shared_fn)) class KalmanError(Exception): pass class ObservationKind(): UNKNOWN = 0 NO_OBSERVATION = 1 GPS_NED = 2 ODOMETRIC_SPEED = 3 PHONE_GYRO = 4 GPS_VEL = 5 PSEUDORANGE_GPS = 6 PSEUDORANGE_RATE_GPS = 7 SPEED = 8 NO_ROT = 9 PHONE_ACCEL = 10 ORB_POINT = 11 ECEF_POS = 12 CAMERA_ODO_TRANSLATION = 13 CAMERA_ODO_ROTATION = 14 ORB_FEATURES = 15 MSCKF_TEST = 16 FEATURE_TRACK_TEST = 17 LANE_PT = 18 IMU_FRAME = 19 PSEUDORANGE_GLONASS = 20 PSEUDORANGE_RATE_GLONASS = 21 PSEUDORANGE = 22 PSEUDORANGE_RATE = 23 ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s] ROAD_FRAME_YAW_RATE = 25 # [rad/s] STEER_ANGLE = 26 # [rad] ANGLE_OFFSET_FAST = 27 # [rad] STIFFNESS = 28 # [-] STEER_RATIO = 29 # [-] ROAD_FRAME_X_SPEED = 30 # (x) [m/s] names = [ 'Unknown', 'No observation', 'GPS NED', 'Odometric speed', 'Phone gyro', 'GPS velocity', 'GPS pseudorange', 'GPS pseudorange rate', 'Speed', 'No rotation', 'Phone acceleration', 'ORB point', 'ECEF pos', 'camera odometric translation', 'camera odometric rotation', 'ORB features', 'MSCKF test', 'Feature track test', 'Lane ecef point', 'imu frame eulers', 'GLONASS pseudorange', 'GLONASS pseudorange rate', 'Road Frame x,y speed', 'Road Frame yaw rate', 'Steer Angle', 'Fast Angle Offset', 'Stiffness', 'Steer Ratio', ] @classmethod def to_string(cls, kind): return cls.names[kind] SAT_OBS = [ObservationKind.PSEUDORANGE_GPS, ObservationKind.PSEUDORANGE_RATE_GPS, ObservationKind.PSEUDORANGE_GLONASS, ObservationKind.PSEUDORANGE_RATE_GLONASS] def run_car_ekf_offline(kf, observations_by_kind): from laika.raw_gnss import GNSSMeasurement observations = [] # create list of observations with element format: [kind, time, data] for kind in observations_by_kind: for t, data in zip(observations_by_kind[kind][0], observations_by_kind[kind][1]): observations.append([t, kind, data]) observations.sort(key=lambda obs: obs[0]) times, estimates = run_observations_through_filter(kf, observations) forward_states = np.stack(e[1] for e in estimates) forward_covs = np.stack(e[3] for e in estimates) smoothed_states, smoothed_covs = kf.rts_smooth(estimates) observations_dict = {} # TODO assuming observations and estimates # are same length may not work with VO for e in estimates: t = e[4] kind = str(int(e[5])) res = e[6] z = e[7] ea = e[8] if len(z) == 0: continue if kind not in observations_dict: observations_dict[kind] = {} observations_dict[kind]['t'] = np.array(len(z)*[t]) observations_dict[kind]['z'] = np.array(z) observations_dict[kind]['ea'] = np.array(ea) observations_dict[kind]['residual'] = np.array(res) else: observations_dict[kind]['t'] = np.append(observations_dict[kind]['t'], np.array(len(z)*[t])) observations_dict[kind]['z'] = np.vstack((observations_dict[kind]['z'], np.array(z))) observations_dict[kind]['ea'] = np.vstack((observations_dict[kind]['ea'], np.array(ea))) observations_dict[kind]['residual'] = np.vstack((observations_dict[kind]['residual'], np.array(res))) # add svIds to gnss data for kind in map(str, SAT_OBS): if int(kind) in observations_by_kind and kind in observations_dict: observations_dict[kind]['svIds'] = np.array([]) observations_dict[kind]['CNO'] = np.array([]) observations_dict[kind]['std'] = np.array([]) for obs in observations_by_kind[int(kind)][1]: observations_dict[kind]['svIds'] = np.append(observations_dict[kind]['svIds'], np.array([obs[:,GNSSMeasurement.PRN]])) observations_dict[kind]['std'] = np.append(observations_dict[kind]['std'], np.array([obs[:,GNSSMeasurement.PR_STD]])) return smoothed_states, smoothed_covs, forward_states, forward_covs, times, observations_dict def run_observations_through_filter(kf, observations, filter_time=None): estimates = [] for obs in tqdm(observations): t = obs[0] kind = obs[1] data = obs[2] estimates.append(kf.predict_and_observe(t, kind, data)) times = [x[4] for x in estimates] return times, estimates def save_residuals_plot(obs, save_path, data_name): import matplotlib.pyplot as plt import mpld3 # pylint: disable=import-error fig = plt.figure(figsize=(10,20)) fig.suptitle('Residuals of ' + data_name, fontsize=24) n = len(list(obs.keys())) start_times = [obs[kind]['t'][0] for kind in obs] start_time = min(start_times) xlims = [start_time + 3, start_time + 60] for i, kind in enumerate(obs): ax = fig.add_subplot(n, 1, i+1) ax.set_xlim(xlims) t = obs[kind]['t'] res = obs[kind]['residual'] start_idx = bisect(t, xlims[0]) if len(res) == start_idx: continue ylim = max(np.linalg.norm(res[start_idx:], axis=1)) ax.set_ylim([-ylim, ylim]) if int(kind) in SAT_OBS: svIds = obs[kind]['svIds'] for svId in set(svIds): svId_idx = (svIds == svId) t = obs[kind]['t'][svId_idx] res = obs[kind]['residual'][svId_idx] ax.plot(t, res, label='SV ' + str(int(svId))) ax.legend(loc='right') else: ax.plot(t, res) plt.title('Residual of kind ' + ObservationKind.to_string(int(kind)), fontsize=20) plt.tight_layout() os.makedirs(save_path) mpld3.save_html(fig, save_path + 'residuals_plot.html')