209 lines
6.2 KiB
Python
209 lines
6.2 KiB
Python
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')
|