mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-30 19:33:45 +08:00
openpilot v0.7.6 release
This commit is contained in:
Vehicle Researcher
@@ -1,9 +1,9 @@
|
||||
Import('env', 'common', 'messaging')
|
||||
Import('env', 'common', 'cereal', 'messaging')
|
||||
loc_objs = [
|
||||
"locationd_yawrate.cc",
|
||||
"params_learner.cc",
|
||||
"paramsd.cc"]
|
||||
loc_libs = [messaging, 'zmq', common, 'capnp', 'kj', 'json11', 'pthread']
|
||||
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'json11', 'pthread']
|
||||
|
||||
env.Program("paramsd", loc_objs, LIBS=loc_libs)
|
||||
env.SharedLibrary("locationd", loc_objs, LIBS=loc_libs)
|
||||
@@ -19,4 +19,3 @@ env.Program("ubloxd_test", [
|
||||
"ublox_msg.cc",
|
||||
"ubloxd_main.cc"],
|
||||
LIBS=loc_libs)
|
||||
|
||||
|
||||
@@ -8,4 +8,3 @@ class Calibration:
|
||||
UNCALIBRATED = 0
|
||||
CALIBRATED = 1
|
||||
INVALID = 2
|
||||
|
||||
|
||||
@@ -5,6 +5,7 @@ import copy
|
||||
import json
|
||||
import numpy as np
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.locationd.calibration_helpers import Calibration
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from common.params import Params, put_nonblocking
|
||||
@@ -12,8 +13,7 @@ from common.transformations.model import model_height
|
||||
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
|
||||
get_calib_from_vp, vp_from_rpy, H, W, FOCAL
|
||||
|
||||
MPH_TO_MS = 0.44704
|
||||
MIN_SPEED_FILTER = 15 * MPH_TO_MS
|
||||
MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
|
||||
MAX_VEL_ANGLE_STD = np.radians(0.25)
|
||||
MAX_YAW_RATE_FILTER = np.radians(2) # per second
|
||||
|
||||
@@ -24,22 +24,21 @@ INPUTS_WANTED = 50 # We want a little bit more than we need for stability
|
||||
WRITE_CYCLES = 10 # write every 1000 cycles
|
||||
VP_INIT = np.array([W/2., H/2.])
|
||||
|
||||
# These validity corners were chosen by looking at 1000
|
||||
# and taking most extreme cases with some margin.
|
||||
VP_VALIDITY_CORNERS = np.array([[W//2 - 120, 300], [W//2 + 120, 520]])
|
||||
# These values are needed to accomodate biggest modelframe
|
||||
VP_VALIDITY_CORNERS = np.array([[W//2 - 63, 300], [W//2 + 63, 520]])
|
||||
DEBUG = os.getenv("DEBUG") is not None
|
||||
|
||||
|
||||
def is_calibration_valid(vp):
|
||||
return vp[0] > VP_VALIDITY_CORNERS[0,0] and vp[0] < VP_VALIDITY_CORNERS[1,0] and \
|
||||
vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1]
|
||||
return vp[0] > VP_VALIDITY_CORNERS[0, 0] and vp[0] < VP_VALIDITY_CORNERS[1, 0] and \
|
||||
vp[1] > VP_VALIDITY_CORNERS[0, 1] and vp[1] < VP_VALIDITY_CORNERS[1, 1]
|
||||
|
||||
|
||||
def sanity_clip(vp):
|
||||
if np.isnan(vp).any():
|
||||
vp = VP_INIT
|
||||
return np.array([np.clip(vp[0], VP_VALIDITY_CORNERS[0,0] - 20, VP_VALIDITY_CORNERS[1,0] + 20),
|
||||
np.clip(vp[1], VP_VALIDITY_CORNERS[0,1] - 20, VP_VALIDITY_CORNERS[1,1] + 20)])
|
||||
return np.array([np.clip(vp[0], VP_VALIDITY_CORNERS[0, 0] - 5, VP_VALIDITY_CORNERS[1, 0] + 5),
|
||||
np.clip(vp[1], VP_VALIDITY_CORNERS[0, 1] - 5, VP_VALIDITY_CORNERS[1, 1] + 5)])
|
||||
|
||||
|
||||
def intrinsics_from_vp(vp):
|
||||
@@ -69,10 +68,7 @@ class Calibrator():
|
||||
if calibration_params:
|
||||
try:
|
||||
calibration_params = json.loads(calibration_params)
|
||||
if 'calib_radians' in calibration_params:
|
||||
self.vp = vp_from_rpy(calibration_params["calib_radians"])
|
||||
else:
|
||||
self.vp = np.array(calibration_params["vanishing_point"])
|
||||
self.vp = vp_from_rpy(calibration_params["calib_radians"])
|
||||
if not np.isfinite(self.vp).all():
|
||||
self.vp = copy.copy(VP_INIT)
|
||||
self.vps = np.tile(self.vp, (INPUTS_WANTED, 1))
|
||||
|
||||
@@ -1,52 +0,0 @@
|
||||
# Kalman filter library
|
||||
|
||||
## Introduction
|
||||
The kalman filter framework described here is an incredibly powerful tool for any optimization problem,
|
||||
but particularly for visual odometry, sensor fusion localization or SLAM. It is designed to provide very
|
||||
accurate results, work online or offline, be fairly computationally efficient, be easy to design filters with in
|
||||
python.
|
||||
|
||||
## Feature walkthrough
|
||||
|
||||
### Extended Kalman Filter with symbolic Jacobian computation
|
||||
Most dynamic systems can be described as a Hidden Markov Process. To estimate the state of such a system with noisy
|
||||
measurements one can use a Recursive Bayesian estimator. For a linear Markov Process a regular linear Kalman filter is optimal.
|
||||
Unfortunately, a lot of systems are non-linear. Extended Kalman Filters can model systems by linearizing the non-linear
|
||||
system at every step, this provides a close to optimal estimator when the linearization is good enough. If the linearization
|
||||
introduces too much noise, one can use an Iterated Extended Kalman Filter, Unscented Kalman Filter or a Particle Filter. For
|
||||
most applications those estimators are overkill and introduce too much complexity and require a lot of additional compute.
|
||||
|
||||
Conventionally Extended Kalman Filters are implemented by writing the system's dynamic equations and then manually symbolically
|
||||
calculating the Jacobians for the linearization. For complex systems this is time consuming and very prone to calculation errors.
|
||||
This library symbolically computes the Jacobians using sympy to simplify the system's definition and remove the possiblity of introducing calculation errors.
|
||||
|
||||
### Error State Kalman Filter
|
||||
3D localization algorithms ussually also require estimating orientation of an object in 3D. Orientation is generally represented
|
||||
with euler angles or quaternions.
|
||||
|
||||
Euler angles have several problems, there are mulitple ways to represent the same orientation,
|
||||
gimbal lock can cause the loss of a degree of freedom and lastly their behaviour is very non-linear when errors are large.
|
||||
Quaternions with one strictly positive dimension don't suffer from these issues, but have another set of problems.
|
||||
Quaternions need to be normalized otherwise they will grow unbounded, this is cannot be cleanly enforced in a kalman filter.
|
||||
Most importantly though a quaternion has 4 dimensions, but only represents 3 degrees of freedom, so there is one redundant dimension.
|
||||
|
||||
Kalman filters are designed to minimize the error of the system's state. It is possible to have a kalman filter where state and the error of the state are represented in a different space. As long as there is an error function that can compute the error based on the true state and estimated state. It is problematic to have redundant dimensions in the error of the kalman filter, but not in the state. A good compromise then, is to use the quaternion to represent the system's attitude state and use euler angles to describe the error in attitude. This library supports and defining an arbitrary error that is in a different space than the state. [Joan Solà](https://arxiv.org/abs/1711.02508) has written a comprehensive description of using ESKFs for robust 3D orientation estimation.
|
||||
|
||||
### Multi-State Constraint Kalman Filter
|
||||
How do you integrate feature-based visual odometry with a Kalman filter? The problem is that one cannot write an observation equation for 2D feature observations in image space for a localization kalman filter. One needs to give the feature observation a depth so it has a 3D position, then one can write an obvervation equation in the kalman filter. This is possible by tracking the feature across frames and then estimating the depth. However, the solution is not that simple, the depth estimated by tracking the feature across frames depends on the location of the camera at those frames, and thus the state of the kalman filter. This creates a positive feedback loop where the kalman filter wrongly gains confidence in it's position because the feature position updates reinforce it.
|
||||
|
||||
The solution is to use an [MSCKF](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf), which this library fully supports.
|
||||
|
||||
### Rauch–Tung–Striebel smoothing
|
||||
When doing offline estimation with a kalman filter there can be an initialization period where states are badly estimated.
|
||||
Global estimators don't suffer from this, to make our kalman filter competitive with global optimizers we can run the filter
|
||||
backwards using an RTS smoother. Those combined with potentially multiple forward and backwards passes of the data should make
|
||||
performance very close to global optimization.
|
||||
|
||||
### Mahalanobis distance outlier rejector
|
||||
A lot of measurements do not come from a Gaussian distribution and as such have outliers that do not fit the statistical model
|
||||
of the Kalman filter. This can cause a lot of performance issues if not dealt with. This library allows the use of a mahalanobis
|
||||
distance statistical test on the incoming measurements to deal with this. Note that good initialization is critical to prevent
|
||||
good measurements from being rejected.
|
||||
|
||||
|
||||
@@ -1,37 +0,0 @@
|
||||
Import('env', 'arch')
|
||||
|
||||
templates = Glob('templates/*')
|
||||
sympy_helpers = "helpers/sympy_helpers.py"
|
||||
ekf_sym = "helpers/ekf_sym.py"
|
||||
|
||||
to_build = {
|
||||
'car': 'models/car_kf.py',
|
||||
'live': 'models/live_kf.py',
|
||||
}
|
||||
|
||||
|
||||
if arch != "aarch64":
|
||||
to_build.update({
|
||||
'lane': '#xx/pipeline/lib/ekf/lane_kf.py',
|
||||
'pos_computer_4': 'helpers/lst_sq_computer.py',
|
||||
'pos_computer_5': 'helpers/lst_sq_computer.py',
|
||||
'feature_handler_5': 'helpers/feature_handler.py',
|
||||
'loc_4': 'models/loc_kf.py',
|
||||
'gnss': 'models/gnss_kf.py',
|
||||
})
|
||||
|
||||
found = {}
|
||||
|
||||
for target, command in to_build.items():
|
||||
if File(command).exists():
|
||||
found[target] = command
|
||||
|
||||
for target, command in found.items():
|
||||
target_files = File([f'generated/{target}.cpp', f'generated/{target}.h'])
|
||||
command_file = File(command)
|
||||
env.Command(target_files,
|
||||
[templates, command_file, sympy_helpers, ekf_sym],
|
||||
command_file.get_abspath()+" "+target
|
||||
)
|
||||
|
||||
env.SharedLibrary('generated/' + target, target_files[0])
|
||||
@@ -1,208 +0,0 @@
|
||||
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')
|
||||
@@ -1,22 +0,0 @@
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def gen_chi2_ppf_lookup(max_dim=200):
|
||||
from scipy.stats import chi2
|
||||
table = np.zeros((max_dim, 98))
|
||||
for dim in range(1, max_dim):
|
||||
table[dim] = chi2.ppf(np.arange(.01, .99, .01), dim)
|
||||
|
||||
np.save('chi2_lookup_table', table)
|
||||
|
||||
|
||||
def chi2_ppf(p, dim):
|
||||
table = np.load(os.path.dirname(os.path.realpath(__file__)) + '/chi2_lookup_table.npy')
|
||||
result = np.interp(p, np.arange(.01, .99, .01), table[dim])
|
||||
return result
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
gen_chi2_ppf_lookup()
|
||||
Binary file not shown.
@@ -1,602 +0,0 @@
|
||||
import os
|
||||
from bisect import bisect_right
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
from numpy import dot
|
||||
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import sympy_into_c
|
||||
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
|
||||
write_code)
|
||||
|
||||
from selfdrive.locationd.kalman.helpers.chi2_lookup import chi2_ppf
|
||||
|
||||
|
||||
def solve(a, b):
|
||||
if a.shape[0] == 1 and a.shape[1] == 1:
|
||||
return b / a[0][0]
|
||||
else:
|
||||
return np.linalg.solve(a, b)
|
||||
|
||||
|
||||
def null(H, eps=1e-12):
|
||||
u, s, vh = np.linalg.svd(H)
|
||||
padding = max(0, np.shape(H)[1] - np.shape(s)[0])
|
||||
null_mask = np.concatenate(((s <= eps), np.ones((padding,), dtype=bool)), axis=0)
|
||||
null_space = np.compress(null_mask, vh, axis=0)
|
||||
return np.transpose(null_space)
|
||||
|
||||
|
||||
def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[], global_vars=None):
|
||||
# optional state transition matrix, H modifier
|
||||
# and err_function if an error-state kalman filter (ESKF)
|
||||
# is desired. Best described in "Quaternion kinematics
|
||||
# for the error-state Kalman filter" by Joan Sola
|
||||
|
||||
if eskf_params:
|
||||
err_eqs = eskf_params[0]
|
||||
inv_err_eqs = eskf_params[1]
|
||||
H_mod_sym = eskf_params[2]
|
||||
f_err_sym = eskf_params[3]
|
||||
x_err_sym = eskf_params[4]
|
||||
else:
|
||||
nom_x = sp.MatrixSymbol('nom_x', dim_x, 1)
|
||||
true_x = sp.MatrixSymbol('true_x', dim_x, 1)
|
||||
delta_x = sp.MatrixSymbol('delta_x', dim_x, 1)
|
||||
err_function_sym = sp.Matrix(nom_x + delta_x)
|
||||
inv_err_function_sym = sp.Matrix(true_x - nom_x)
|
||||
err_eqs = [err_function_sym, nom_x, delta_x]
|
||||
inv_err_eqs = [inv_err_function_sym, nom_x, true_x]
|
||||
|
||||
H_mod_sym = sp.Matrix(np.eye(dim_x))
|
||||
f_err_sym = f_sym
|
||||
x_err_sym = x_sym
|
||||
|
||||
# This configures the multi-state augmentation
|
||||
# needed for EKF-SLAM with MSCKF (Mourikis et al 2007)
|
||||
if msckf_params:
|
||||
msckf = True
|
||||
dim_main = msckf_params[0] # size of the main state
|
||||
dim_augment = msckf_params[1] # size of one augment state chunk
|
||||
dim_main_err = msckf_params[2]
|
||||
dim_augment_err = msckf_params[3]
|
||||
N = msckf_params[4]
|
||||
feature_track_kinds = msckf_params[5]
|
||||
assert dim_main + dim_augment * N == dim_x
|
||||
assert dim_main_err + dim_augment_err * N == dim_err
|
||||
else:
|
||||
msckf = False
|
||||
dim_main = dim_x
|
||||
dim_augment = 0
|
||||
dim_main_err = dim_err
|
||||
dim_augment_err = 0
|
||||
N = 0
|
||||
|
||||
# linearize with jacobians
|
||||
F_sym = f_err_sym.jacobian(x_err_sym)
|
||||
|
||||
if eskf_params:
|
||||
for sym in x_err_sym:
|
||||
F_sym = F_sym.subs(sym, 0)
|
||||
|
||||
assert dt_sym in F_sym.free_symbols
|
||||
|
||||
for i in range(len(obs_eqs)):
|
||||
obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym))
|
||||
if msckf and obs_eqs[i][1] in feature_track_kinds:
|
||||
obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2]))
|
||||
else:
|
||||
obs_eqs[i].append(None)
|
||||
|
||||
# collect sympy functions
|
||||
sympy_functions = []
|
||||
|
||||
# error functions
|
||||
sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]]))
|
||||
sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]]))
|
||||
|
||||
# H modifier for ESKF updates
|
||||
sympy_functions.append(('H_mod_fun', H_mod_sym, [x_sym]))
|
||||
|
||||
# state propagation function
|
||||
sympy_functions.append(('f_fun', f_sym, [x_sym, dt_sym]))
|
||||
sympy_functions.append(('F_fun', F_sym, [x_sym, dt_sym]))
|
||||
|
||||
# observation functions
|
||||
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
|
||||
sympy_functions.append(('h_%d' % kind, h_sym, [x_sym, ea_sym]))
|
||||
sympy_functions.append(('H_%d' % kind, H_sym, [x_sym, ea_sym]))
|
||||
if msckf and kind in feature_track_kinds:
|
||||
sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym]))
|
||||
|
||||
# Generate and wrap all th c code
|
||||
header, code = sympy_into_c(sympy_functions, global_vars)
|
||||
extra_header = "#define DIM %d\n" % dim_x
|
||||
extra_header += "#define EDIM %d\n" % dim_err
|
||||
extra_header += "#define MEDIM %d\n" % dim_main_err
|
||||
extra_header += "typedef void (*Hfun)(double *, double *, double *);\n"
|
||||
|
||||
extra_header += "\nvoid predict(double *x, double *P, double *Q, double dt);"
|
||||
|
||||
extra_post = ""
|
||||
|
||||
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
|
||||
if msckf and kind in feature_track_kinds:
|
||||
He_str = 'He_%d' % kind
|
||||
# ea_dim = ea_sym.shape[0]
|
||||
else:
|
||||
He_str = 'NULL'
|
||||
# ea_dim = 1 # not really dim of ea but makes c function work
|
||||
maha_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
|
||||
maha_test = kind in maha_test_kinds
|
||||
extra_post += """
|
||||
void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
|
||||
update<%d,%d,%d>(in_x, in_P, h_%d, H_%d, %s, in_z, in_R, in_ea, MAHA_THRESH_%d);
|
||||
}
|
||||
""" % (kind, h_sym.shape[0], 3, maha_test, kind, kind, He_str, kind)
|
||||
extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % (kind, maha_thresh)
|
||||
extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind
|
||||
|
||||
code += '\nextern "C"{\n' + extra_header + "\n}\n"
|
||||
code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read()
|
||||
code += '\nextern "C"{\n' + extra_post + "\n}\n"
|
||||
|
||||
if global_vars is not None:
|
||||
global_code = '\nextern "C"{\n'
|
||||
for var in global_vars:
|
||||
global_code += f"\ndouble {var.name};\n"
|
||||
global_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n"
|
||||
extra_header += f"\nvoid set_{var.name}(double x);\n"
|
||||
|
||||
global_code += '\n}\n'
|
||||
code = global_code + code
|
||||
|
||||
header += "\n" + extra_header
|
||||
|
||||
write_code(name, code, header)
|
||||
|
||||
|
||||
class EKF_sym():
|
||||
def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err,
|
||||
N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], global_vars=None):
|
||||
"""Generates process function and all observation functions for the kalman filter."""
|
||||
self.msckf = N > 0
|
||||
self.N = N
|
||||
self.dim_augment = dim_augment
|
||||
self.dim_augment_err = dim_augment_err
|
||||
self.dim_main = dim_main
|
||||
self.dim_main_err = dim_main_err
|
||||
|
||||
# state
|
||||
x_initial = x_initial.reshape((-1, 1))
|
||||
self.dim_x = x_initial.shape[0]
|
||||
self.dim_err = P_initial.shape[0]
|
||||
assert dim_main + dim_augment * N == self.dim_x
|
||||
assert dim_main_err + dim_augment_err * N == self.dim_err
|
||||
assert Q.shape == P_initial.shape
|
||||
|
||||
# kinds that should get mahalanobis distance
|
||||
# tested for outlier rejection
|
||||
self.maha_test_kinds = maha_test_kinds
|
||||
|
||||
self.global_vars = global_vars
|
||||
|
||||
# process noise
|
||||
self.Q = Q
|
||||
|
||||
# rewind stuff
|
||||
self.rewind_t = []
|
||||
self.rewind_states = []
|
||||
self.rewind_obscache = []
|
||||
self.init_state(x_initial, P_initial, None)
|
||||
|
||||
ffi, lib = load_code(name)
|
||||
kinds, self.feature_track_kinds = [], []
|
||||
for func in dir(lib):
|
||||
if func[:2] == 'h_':
|
||||
kinds.append(int(func[2:]))
|
||||
if func[:3] == 'He_':
|
||||
self.feature_track_kinds.append(int(func[3:]))
|
||||
|
||||
# wrap all the sympy functions
|
||||
def wrap_1lists(name):
|
||||
func = eval("lib.%s" % name, {"lib": lib})
|
||||
|
||||
def ret(lst1, out):
|
||||
func(ffi.cast("double *", lst1.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return ret
|
||||
|
||||
def wrap_2lists(name):
|
||||
func = eval("lib.%s" % name, {"lib": lib})
|
||||
|
||||
def ret(lst1, lst2, out):
|
||||
func(ffi.cast("double *", lst1.ctypes.data),
|
||||
ffi.cast("double *", lst2.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return ret
|
||||
|
||||
def wrap_1list_1float(name):
|
||||
func = eval("lib.%s" % name, {"lib": lib})
|
||||
|
||||
def ret(lst1, fl, out):
|
||||
func(ffi.cast("double *", lst1.ctypes.data),
|
||||
ffi.cast("double", fl),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return ret
|
||||
|
||||
self.f = wrap_1list_1float("f_fun")
|
||||
self.F = wrap_1list_1float("F_fun")
|
||||
|
||||
self.err_function = wrap_2lists("err_fun")
|
||||
self.inv_err_function = wrap_2lists("inv_err_fun")
|
||||
self.H_mod = wrap_1lists("H_mod_fun")
|
||||
|
||||
self.hs, self.Hs, self.Hes = {}, {}, {}
|
||||
for kind in kinds:
|
||||
self.hs[kind] = wrap_2lists("h_%d" % kind)
|
||||
self.Hs[kind] = wrap_2lists("H_%d" % kind)
|
||||
if self.msckf and kind in self.feature_track_kinds:
|
||||
self.Hes[kind] = wrap_2lists("He_%d" % kind)
|
||||
|
||||
if self.global_vars is not None:
|
||||
for var in self.global_vars:
|
||||
fun_name = f"set_{var.name}"
|
||||
setattr(self, fun_name, getattr(lib, fun_name))
|
||||
|
||||
# wrap the C++ predict function
|
||||
def _predict_blas(x, P, dt):
|
||||
lib.predict(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", P.ctypes.data),
|
||||
ffi.cast("double *", self.Q.ctypes.data),
|
||||
ffi.cast("double", dt))
|
||||
return x, P
|
||||
|
||||
# wrap the C++ update function
|
||||
def fun_wrapper(f, kind):
|
||||
f = eval("lib.%s" % f, {"lib": lib})
|
||||
|
||||
def _update_inner_blas(x, P, z, R, extra_args):
|
||||
f(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", P.ctypes.data),
|
||||
ffi.cast("double *", z.ctypes.data),
|
||||
ffi.cast("double *", R.ctypes.data),
|
||||
ffi.cast("double *", extra_args.ctypes.data))
|
||||
if self.msckf and kind in self.feature_track_kinds:
|
||||
y = z[:-len(extra_args)]
|
||||
else:
|
||||
y = z
|
||||
return x, P, y
|
||||
return _update_inner_blas
|
||||
|
||||
self._updates = {}
|
||||
for kind in kinds:
|
||||
self._updates[kind] = fun_wrapper("update_%d" % kind, kind)
|
||||
|
||||
def _update_blas(x, P, kind, z, R, extra_args=[]):
|
||||
return self._updates[kind](x, P, z, R, extra_args)
|
||||
|
||||
# assign the functions
|
||||
self._predict = _predict_blas
|
||||
# self._predict = self._predict_python
|
||||
self._update = _update_blas
|
||||
# self._update = self._update_python
|
||||
|
||||
def init_state(self, state, covs, filter_time):
|
||||
self.x = np.array(state.reshape((-1, 1))).astype(np.float64)
|
||||
self.P = np.array(covs).astype(np.float64)
|
||||
self.filter_time = filter_time
|
||||
self.augment_times = [0] * self.N
|
||||
self.rewind_obscache = []
|
||||
self.rewind_t = []
|
||||
self.rewind_states = []
|
||||
|
||||
def reset_rewind(self):
|
||||
self.rewind_obscache = []
|
||||
self.rewind_t = []
|
||||
self.rewind_states = []
|
||||
|
||||
def augment(self):
|
||||
# TODO this is not a generalized way of doing this and implies that the augmented states
|
||||
# are simply the first (dim_augment_state) elements of the main state.
|
||||
assert self.msckf
|
||||
d1 = self.dim_main
|
||||
d2 = self.dim_main_err
|
||||
d3 = self.dim_augment
|
||||
d4 = self.dim_augment_err
|
||||
|
||||
# push through augmented states
|
||||
self.x[d1:-d3] = self.x[d1 + d3:]
|
||||
self.x[-d3:] = self.x[:d3]
|
||||
assert self.x.shape == (self.dim_x, 1)
|
||||
|
||||
# push through augmented covs
|
||||
assert self.P.shape == (self.dim_err, self.dim_err)
|
||||
P_reduced = self.P
|
||||
P_reduced = np.delete(P_reduced, np.s_[d2:d2 + d4], axis=1)
|
||||
P_reduced = np.delete(P_reduced, np.s_[d2:d2 + d4], axis=0)
|
||||
assert P_reduced.shape == (self.dim_err - d4, self.dim_err - d4)
|
||||
to_mult = np.zeros((self.dim_err, self.dim_err - d4))
|
||||
to_mult[:-d4, :] = np.eye(self.dim_err - d4)
|
||||
to_mult[-d4:, :d4] = np.eye(d4)
|
||||
self.P = to_mult.dot(P_reduced.dot(to_mult.T))
|
||||
self.augment_times = self.augment_times[1:]
|
||||
self.augment_times.append(self.filter_time)
|
||||
assert self.P.shape == (self.dim_err, self.dim_err)
|
||||
|
||||
def state(self):
|
||||
return np.array(self.x).flatten()
|
||||
|
||||
def covs(self):
|
||||
return self.P
|
||||
|
||||
def rewind(self, t):
|
||||
# find where we are rewinding to
|
||||
idx = bisect_right(self.rewind_t, t)
|
||||
assert self.rewind_t[idx - 1] <= t
|
||||
assert self.rewind_t[idx] > t # must be true, or rewind wouldn't be called
|
||||
|
||||
# set the state to the time right before that
|
||||
self.filter_time = self.rewind_t[idx - 1]
|
||||
self.x[:] = self.rewind_states[idx - 1][0]
|
||||
self.P[:] = self.rewind_states[idx - 1][1]
|
||||
|
||||
# return the observations we rewound over for fast forwarding
|
||||
ret = self.rewind_obscache[idx:]
|
||||
|
||||
# throw away the old future
|
||||
# TODO: is this making a copy?
|
||||
self.rewind_t = self.rewind_t[:idx]
|
||||
self.rewind_states = self.rewind_states[:idx]
|
||||
self.rewind_obscache = self.rewind_obscache[:idx]
|
||||
|
||||
return ret
|
||||
|
||||
def checkpoint(self, obs):
|
||||
# push to rewinder
|
||||
self.rewind_t.append(self.filter_time)
|
||||
self.rewind_states.append((np.copy(self.x), np.copy(self.P)))
|
||||
self.rewind_obscache.append(obs)
|
||||
|
||||
# only keep a certain number around
|
||||
REWIND_TO_KEEP = 512
|
||||
self.rewind_t = self.rewind_t[-REWIND_TO_KEEP:]
|
||||
self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:]
|
||||
self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:]
|
||||
|
||||
def predict(self, t):
|
||||
# initialize time
|
||||
if self.filter_time is None:
|
||||
self.filter_time = t
|
||||
|
||||
# predict
|
||||
dt = t - self.filter_time
|
||||
assert dt >= 0
|
||||
self.x, self.P = self._predict(self.x, self.P, dt)
|
||||
self.filter_time = t
|
||||
|
||||
def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False):
|
||||
# TODO handle rewinding at this level"
|
||||
|
||||
# rewind
|
||||
if self.filter_time is not None and t < self.filter_time:
|
||||
if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] - 1.0:
|
||||
print("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time))
|
||||
return None
|
||||
rewound = self.rewind(t)
|
||||
else:
|
||||
rewound = []
|
||||
|
||||
ret = self._predict_and_update_batch(t, kind, z, R, extra_args, augment)
|
||||
|
||||
# optional fast forward
|
||||
for r in rewound:
|
||||
self._predict_and_update_batch(*r)
|
||||
|
||||
return ret
|
||||
|
||||
def _predict_and_update_batch(self, t, kind, z, R, extra_args, augment=False):
|
||||
"""The main kalman filter function
|
||||
Predicts the state and then updates a batch of observations
|
||||
|
||||
dim_x: dimensionality of the state space
|
||||
dim_z: dimensionality of the observation and depends on kind
|
||||
n: number of observations
|
||||
|
||||
Args:
|
||||
t (float): Time of observation
|
||||
kind (int): Type of observation
|
||||
z (vec [n,dim_z]): Measurements
|
||||
R (mat [n,dim_z, dim_z]): Measurement Noise
|
||||
extra_args (list, [n]): Values used in H computations
|
||||
"""
|
||||
# initialize time
|
||||
if self.filter_time is None:
|
||||
self.filter_time = t
|
||||
|
||||
# predict
|
||||
dt = t - self.filter_time
|
||||
assert dt >= 0
|
||||
self.x, self.P = self._predict(self.x, self.P, dt)
|
||||
self.filter_time = t
|
||||
xk_km1, Pk_km1 = np.copy(self.x).flatten(), np.copy(self.P)
|
||||
|
||||
# update batch
|
||||
y = []
|
||||
for i in range(len(z)):
|
||||
# these are from the user, so we canonicalize them
|
||||
z_i = np.array(z[i], dtype=np.float64, order='F')
|
||||
R_i = np.array(R[i], dtype=np.float64, order='F')
|
||||
extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F')
|
||||
# update
|
||||
self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i)
|
||||
y.append(y_i)
|
||||
xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P)
|
||||
|
||||
if augment:
|
||||
self.augment()
|
||||
|
||||
# checkpoint
|
||||
self.checkpoint((t, kind, z, R, extra_args))
|
||||
|
||||
return xk_km1, xk_k, Pk_km1, Pk_k, t, kind, y, z, extra_args
|
||||
|
||||
def _predict_python(self, x, P, dt):
|
||||
x_new = np.zeros(x.shape, dtype=np.float64)
|
||||
self.f(x, dt, x_new)
|
||||
|
||||
F = np.zeros(P.shape, dtype=np.float64)
|
||||
self.F(x, dt, F)
|
||||
|
||||
if not self.msckf:
|
||||
P = dot(dot(F, P), F.T)
|
||||
else:
|
||||
# Update the predicted state covariance:
|
||||
# Pk+1|k = |F*Pii*FT + Q*dt F*Pij |
|
||||
# |PijT*FT Pjj |
|
||||
# Where F is the jacobian of the main state
|
||||
# predict function, Pii is the main state's
|
||||
# covariance and Q its process noise. Pij
|
||||
# is the covariance between the augmented
|
||||
# states and the main state.
|
||||
#
|
||||
d2 = self.dim_main_err # known at compile time
|
||||
F_curr = F[:d2, :d2]
|
||||
P[:d2, :d2] = (F_curr.dot(P[:d2, :d2])).dot(F_curr.T)
|
||||
P[:d2, d2:] = F_curr.dot(P[:d2, d2:])
|
||||
P[d2:, :d2] = P[d2:, :d2].dot(F_curr.T)
|
||||
|
||||
P += dt * self.Q
|
||||
return x_new, P
|
||||
|
||||
def _update_python(self, x, P, kind, z, R, extra_args=[]):
|
||||
# init vars
|
||||
z = z.reshape((-1, 1))
|
||||
h = np.zeros(z.shape, dtype=np.float64)
|
||||
H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
|
||||
|
||||
# C functions
|
||||
self.hs[kind](x, extra_args, h)
|
||||
self.Hs[kind](x, extra_args, H)
|
||||
|
||||
# y is the "loss"
|
||||
y = z - h
|
||||
|
||||
# *** same above this line ***
|
||||
|
||||
if self.msckf and kind in self.Hes:
|
||||
# Do some algebraic magic to decorrelate
|
||||
He = np.zeros((z.shape[0], len(extra_args)), dtype=np.float64)
|
||||
self.Hes[kind](x, extra_args, He)
|
||||
|
||||
# TODO: Don't call a function here, do projection locally
|
||||
A = null(He.T)
|
||||
|
||||
y = A.T.dot(y)
|
||||
H = A.T.dot(H)
|
||||
R = A.T.dot(R.dot(A))
|
||||
|
||||
# TODO If nullspace isn't the dimension we want
|
||||
if A.shape[1] + He.shape[1] != A.shape[0]:
|
||||
print('Warning: null space projection failed, measurement ignored')
|
||||
return x, P, np.zeros(A.shape[0] - He.shape[1])
|
||||
|
||||
# if using eskf
|
||||
H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
|
||||
self.H_mod(x, H_mod)
|
||||
H = H.dot(H_mod)
|
||||
|
||||
# Do mahalobis distance test
|
||||
# currently just runs on msckf observations
|
||||
# could run on anything if needed
|
||||
if self.msckf and kind in self.maha_test_kinds:
|
||||
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
|
||||
maha_dist = y.T.dot(a.dot(y))
|
||||
if maha_dist > chi2_ppf(0.95, y.shape[0]):
|
||||
R = 10e16 * R
|
||||
|
||||
# *** same below this line ***
|
||||
|
||||
# Outlier resilient weighting as described in:
|
||||
# "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..."
|
||||
weight = 1 # (1.5)/(1 + np.sum(y**2)/np.sum(R))
|
||||
|
||||
S = dot(dot(H, P), H.T) + R / weight
|
||||
K = solve(S, dot(H, P.T)).T
|
||||
I_KH = np.eye(P.shape[0]) - dot(K, H)
|
||||
|
||||
# update actual state
|
||||
delta_x = dot(K, y)
|
||||
P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T)
|
||||
|
||||
# inject observed error into state
|
||||
x_new = np.zeros(x.shape, dtype=np.float64)
|
||||
self.err_function(x, delta_x, x_new)
|
||||
return x_new, P, y.flatten()
|
||||
|
||||
def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95):
|
||||
# init vars
|
||||
z = z.reshape((-1, 1))
|
||||
h = np.zeros(z.shape, dtype=np.float64)
|
||||
H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
|
||||
|
||||
# C functions
|
||||
self.hs[kind](x, extra_args, h)
|
||||
self.Hs[kind](x, extra_args, H)
|
||||
|
||||
# y is the "loss"
|
||||
y = z - h
|
||||
|
||||
# if using eskf
|
||||
H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
|
||||
self.H_mod(x, H_mod)
|
||||
H = H.dot(H_mod)
|
||||
|
||||
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
|
||||
maha_dist = y.T.dot(a.dot(y))
|
||||
if maha_dist > chi2_ppf(maha_thresh, y.shape[0]):
|
||||
return False
|
||||
else:
|
||||
return True
|
||||
|
||||
def rts_smooth(self, estimates, norm_quats=False):
|
||||
'''
|
||||
Returns rts smoothed results of
|
||||
kalman filter estimates
|
||||
|
||||
If the kalman state is augmented with
|
||||
old states only the main state is smoothed
|
||||
'''
|
||||
xk_n = estimates[-1][0]
|
||||
Pk_n = estimates[-1][2]
|
||||
Fk_1 = np.zeros(Pk_n.shape, dtype=np.float64)
|
||||
|
||||
states_smoothed = [xk_n]
|
||||
covs_smoothed = [Pk_n]
|
||||
for k in range(len(estimates) - 2, -1, -1):
|
||||
xk1_n = xk_n
|
||||
if norm_quats:
|
||||
xk1_n[3:7] /= np.linalg.norm(xk1_n[3:7])
|
||||
Pk1_n = Pk_n
|
||||
|
||||
xk1_k, _, Pk1_k, _, t2, _, _, _, _ = estimates[k + 1]
|
||||
_, xk_k, _, Pk_k, t1, _, _, _, _ = estimates[k]
|
||||
dt = t2 - t1
|
||||
self.F(xk_k, dt, Fk_1)
|
||||
|
||||
d1 = self.dim_main
|
||||
d2 = self.dim_main_err
|
||||
Ck = np.linalg.solve(Pk1_k[:d2, :d2], Fk_1[:d2, :d2].dot(Pk_k[:d2, :d2].T)).T
|
||||
xk_n = xk_k
|
||||
delta_x = np.zeros((Pk_n.shape[0], 1), dtype=np.float64)
|
||||
self.inv_err_function(xk1_k, xk1_n, delta_x)
|
||||
delta_x[:d2] = Ck.dot(delta_x[:d2])
|
||||
x_new = np.zeros((xk_n.shape[0], 1), dtype=np.float64)
|
||||
self.err_function(xk_k, delta_x, x_new)
|
||||
xk_n[:d1] = x_new[:d1, 0]
|
||||
Pk_n = Pk_k
|
||||
Pk_n[:d2, :d2] = Pk_k[:d2, :d2] + Ck.dot(Pk1_n[:d2, :d2] - Pk1_k[:d2, :d2]).dot(Ck.T)
|
||||
states_smoothed.append(xk_n)
|
||||
covs_smoothed.append(Pk_n)
|
||||
|
||||
return np.flipud(np.vstack(states_smoothed)), np.stack(covs_smoothed, 0)[::-1]
|
||||
@@ -1,158 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
|
||||
import common.transformations.orientation as orient
|
||||
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
|
||||
write_code)
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import quat_matrix_l
|
||||
|
||||
|
||||
def sane(track):
|
||||
img_pos = track[1:, 2:4]
|
||||
diffs_x = abs(img_pos[1:, 0] - img_pos[:-1, 0])
|
||||
diffs_y = abs(img_pos[1:, 1] - img_pos[:-1, 1])
|
||||
for i in range(1, len(diffs_x)):
|
||||
if ((diffs_x[i] > 0.05 or diffs_x[i - 1] > 0.05) and
|
||||
(diffs_x[i] > 2 * diffs_x[i - 1] or
|
||||
diffs_x[i] < .5 * diffs_x[i - 1])) or \
|
||||
((diffs_y[i] > 0.05 or diffs_y[i - 1] > 0.05) and
|
||||
(diffs_y[i] > 2 * diffs_y[i - 1] or
|
||||
diffs_y[i] < .5 * diffs_y[i - 1])):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
class FeatureHandler():
|
||||
name = 'feature_handler'
|
||||
|
||||
@staticmethod
|
||||
def generate_code(K=5):
|
||||
# Wrap c code for slow matching
|
||||
c_header = "\nvoid merge_features(double *tracks, double *features, long long *empty_idxs);"
|
||||
|
||||
c_code = "#include <math.h>\n"
|
||||
c_code += "#include <string.h>\n"
|
||||
c_code += "#define K %d\n" % K
|
||||
c_code += "\n" + open(os.path.join(TEMPLATE_DIR, "feature_handler.c")).read()
|
||||
|
||||
filename = f"{FeatureHandler.name}_{K}"
|
||||
write_code(filename, c_code, c_header)
|
||||
|
||||
def __init__(self, K=5):
|
||||
self.MAX_TRACKS = 6000
|
||||
self.K = K
|
||||
|
||||
# Array of tracks, each track has K 5D features preceded
|
||||
# by 5 params that inidicate [f_idx, last_idx, updated, complete, valid]
|
||||
# f_idx: idx of current last feature in track
|
||||
# idx of of last feature in frame
|
||||
# bool for whether this track has been update
|
||||
# bool for whether this track is complete
|
||||
# bool for whether this track is valid
|
||||
self.tracks = np.zeros((self.MAX_TRACKS, K + 1, 5))
|
||||
self.tracks[:] = np.nan
|
||||
|
||||
name = f"{FeatureHandler.name}_{K}"
|
||||
ffi, lib = load_code(name)
|
||||
|
||||
def merge_features_c(tracks, features, empty_idxs):
|
||||
lib.merge_features(ffi.cast("double *", tracks.ctypes.data),
|
||||
ffi.cast("double *", features.ctypes.data),
|
||||
ffi.cast("long long *", empty_idxs.ctypes.data))
|
||||
|
||||
# self.merge_features = self.merge_features_python
|
||||
self.merge_features = merge_features_c
|
||||
|
||||
def reset(self):
|
||||
self.tracks[:] = np.nan
|
||||
|
||||
def merge_features_python(self, tracks, features, empty_idxs):
|
||||
empty_idx = 0
|
||||
for f in features:
|
||||
match_idx = int(f[4])
|
||||
if tracks[match_idx, 0, 1] == match_idx and tracks[match_idx, 0, 2] == 0:
|
||||
tracks[match_idx, 0, 0] += 1
|
||||
tracks[match_idx, 0, 1] = f[1]
|
||||
tracks[match_idx, 0, 2] = 1
|
||||
tracks[match_idx, int(tracks[match_idx, 0, 0])] = f
|
||||
if tracks[match_idx, 0, 0] == self.K:
|
||||
tracks[match_idx, 0, 3] = 1
|
||||
if sane(tracks[match_idx]):
|
||||
tracks[match_idx, 0, 4] = 1
|
||||
else:
|
||||
if empty_idx == len(empty_idxs):
|
||||
print('need more empty space')
|
||||
continue
|
||||
tracks[empty_idxs[empty_idx], 0, 0] = 1
|
||||
tracks[empty_idxs[empty_idx], 0, 1] = f[1]
|
||||
tracks[empty_idxs[empty_idx], 0, 2] = 1
|
||||
tracks[empty_idxs[empty_idx], 1] = f
|
||||
empty_idx += 1
|
||||
|
||||
def update_tracks(self, features):
|
||||
last_idxs = np.copy(self.tracks[:, 0, 1])
|
||||
real = np.isfinite(last_idxs)
|
||||
self.tracks[last_idxs[real].astype(int)] = self.tracks[real]
|
||||
|
||||
mask = np.ones(self.MAX_TRACKS, np.bool)
|
||||
mask[last_idxs[real].astype(int)] = 0
|
||||
empty_idxs = np.arange(self.MAX_TRACKS)[mask]
|
||||
|
||||
self.tracks[empty_idxs] = np.nan
|
||||
self.tracks[:, 0, 2] = 0
|
||||
self.merge_features(self.tracks, features, empty_idxs)
|
||||
|
||||
def handle_features(self, features):
|
||||
self.update_tracks(features)
|
||||
valid_idxs = self.tracks[:, 0, 4] == 1
|
||||
complete_idxs = self.tracks[:, 0, 3] == 1
|
||||
stale_idxs = self.tracks[:, 0, 2] == 0
|
||||
valid_tracks = self.tracks[valid_idxs]
|
||||
self.tracks[complete_idxs] = np.nan
|
||||
self.tracks[stale_idxs] = np.nan
|
||||
return valid_tracks[:, 1:, :4].reshape((len(valid_tracks), self.K * 4))
|
||||
|
||||
|
||||
def generate_orient_error_jac(K):
|
||||
import sympy as sp
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import quat_rotate
|
||||
|
||||
x_sym = sp.MatrixSymbol('abr', 3, 1)
|
||||
dtheta = sp.MatrixSymbol('dtheta', 3, 1)
|
||||
delta_quat = sp.Matrix(np.ones(4))
|
||||
delta_quat[1:, :] = sp.Matrix(0.5 * dtheta[0:3, :])
|
||||
poses_sym = sp.MatrixSymbol('poses', 7 * K, 1)
|
||||
img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1)
|
||||
alpha, beta, rho = x_sym
|
||||
to_c = sp.Matrix(orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0))
|
||||
pos_0 = sp.Matrix(np.array(poses_sym[K * 7 - 7:K * 7 - 4])[:, 0])
|
||||
q = quat_matrix_l(poses_sym[K * 7 - 4:K * 7]) * delta_quat
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_0 = to_c * quat_rot.T
|
||||
rows = []
|
||||
for i in range(K):
|
||||
pos_i = sp.Matrix(np.array(poses_sym[i * 7:i * 7 + 3])[:, 0])
|
||||
q = quat_matrix_l(poses_sym[7 * i + 3:7 * i + 7]) * delta_quat
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_i = to_c * quat_rot.T
|
||||
rot_0_to_i = rot_g_to_i * (rot_g_to_0.T)
|
||||
trans_0_to_i = rot_g_to_i * (pos_0 - pos_i)
|
||||
funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1]) + rho * trans_0_to_i
|
||||
h1, h2, h3 = funct_vec
|
||||
rows.append(h1 / h3 - img_pos_sym[i * 2 + 0])
|
||||
rows.append(h2 / h3 - img_pos_sym[i * 2 + 1])
|
||||
img_pos_residual_sym = sp.Matrix(rows)
|
||||
|
||||
# sympy into c
|
||||
sympy_functions = []
|
||||
sympy_functions.append(('orient_error_jac', img_pos_residual_sym.jacobian(dtheta), [x_sym, poses_sym, img_pos_sym, dtheta]))
|
||||
|
||||
return sympy_functions
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# TODO: get K from argparse
|
||||
FeatureHandler.generate_code()
|
||||
@@ -1,176 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
import common.transformations.orientation as orient
|
||||
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
|
||||
write_code)
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import (quat_rotate,
|
||||
sympy_into_c)
|
||||
|
||||
|
||||
def generate_residual(K):
|
||||
x_sym = sp.MatrixSymbol('abr', 3, 1)
|
||||
poses_sym = sp.MatrixSymbol('poses', 7 * K, 1)
|
||||
img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1)
|
||||
alpha, beta, rho = x_sym
|
||||
to_c = sp.Matrix(orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0))
|
||||
pos_0 = sp.Matrix(np.array(poses_sym[K * 7 - 7:K * 7 - 4])[:, 0])
|
||||
q = poses_sym[K * 7 - 4:K * 7]
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_0 = to_c * quat_rot.T
|
||||
rows = []
|
||||
|
||||
for i in range(K):
|
||||
pos_i = sp.Matrix(np.array(poses_sym[i * 7:i * 7 + 3])[:, 0])
|
||||
q = poses_sym[7 * i + 3:7 * i + 7]
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_i = to_c * quat_rot.T
|
||||
rot_0_to_i = rot_g_to_i * rot_g_to_0.T
|
||||
trans_0_to_i = rot_g_to_i * (pos_0 - pos_i)
|
||||
funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1]) + rho * trans_0_to_i
|
||||
h1, h2, h3 = funct_vec
|
||||
rows.append(h1 / h3 - img_pos_sym[i * 2 + 0])
|
||||
rows.append(h2 / h3 - img_pos_sym[i * 2 + 1])
|
||||
img_pos_residual_sym = sp.Matrix(rows)
|
||||
|
||||
# sympy into c
|
||||
sympy_functions = []
|
||||
sympy_functions.append(('res_fun', img_pos_residual_sym, [x_sym, poses_sym, img_pos_sym]))
|
||||
sympy_functions.append(('jac_fun', img_pos_residual_sym.jacobian(x_sym), [x_sym, poses_sym, img_pos_sym]))
|
||||
|
||||
return sympy_functions
|
||||
|
||||
|
||||
class LstSqComputer():
|
||||
name = 'pos_computer'
|
||||
|
||||
@staticmethod
|
||||
def generate_code(K=4):
|
||||
sympy_functions = generate_residual(K)
|
||||
header, code = sympy_into_c(sympy_functions)
|
||||
|
||||
code += "\n#define KDIM %d\n" % K
|
||||
code += "\n" + open(os.path.join(TEMPLATE_DIR, "compute_pos.c")).read()
|
||||
|
||||
header += """
|
||||
void compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);
|
||||
"""
|
||||
|
||||
filename = f"{LstSqComputer.name}_{K}"
|
||||
write_code(filename, code, header)
|
||||
|
||||
def __init__(self, K=4, MIN_DEPTH=2, MAX_DEPTH=500):
|
||||
self.to_c = orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0)
|
||||
self.MAX_DEPTH = MAX_DEPTH
|
||||
self.MIN_DEPTH = MIN_DEPTH
|
||||
|
||||
name = f"{LstSqComputer.name}_{K}"
|
||||
ffi, lib = load_code(name)
|
||||
|
||||
# wrap c functions
|
||||
def residual_jac(x, poses, img_positions):
|
||||
out = np.zeros(((K * 2, 3)), dtype=np.float64)
|
||||
lib.jac_fun(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return out
|
||||
self.residual_jac = residual_jac
|
||||
|
||||
def residual(x, poses, img_positions):
|
||||
out = np.zeros((K * 2), dtype=np.float64)
|
||||
lib.res_fun(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return out
|
||||
self.residual = residual
|
||||
|
||||
def compute_pos_c(poses, img_positions):
|
||||
pos = np.zeros(3, dtype=np.float64)
|
||||
param = np.zeros(3, dtype=np.float64)
|
||||
# Can't be a view for the ctype
|
||||
img_positions = np.copy(img_positions)
|
||||
lib.compute_pos(ffi.cast("double *", self.to_c.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", param.ctypes.data),
|
||||
ffi.cast("double *", pos.ctypes.data))
|
||||
return pos, param
|
||||
self.compute_pos_c = compute_pos_c
|
||||
|
||||
def compute_pos(self, poses, img_positions, debug=False):
|
||||
pos, param = self.compute_pos_c(poses, img_positions)
|
||||
# pos, param = self.compute_pos_python(poses, img_positions)
|
||||
|
||||
depth = 1 / param[2]
|
||||
if debug:
|
||||
# orient_err_jac = self.orient_error_jac(param, poses, img_positions, np.zeros(3)).reshape((-1,2,3))
|
||||
jac = self.residual_jac(param, poses, img_positions).reshape((-1, 2, 3))
|
||||
res = self.residual(param, poses, img_positions).reshape((-1, 2))
|
||||
return pos, param, res, jac # , orient_err_jac
|
||||
elif (self.MIN_DEPTH < depth < self.MAX_DEPTH):
|
||||
return pos
|
||||
else:
|
||||
return None
|
||||
|
||||
def gauss_newton(self, fun, jac, x, args):
|
||||
poses, img_positions = args
|
||||
delta = 1
|
||||
counter = 0
|
||||
while abs(np.linalg.norm(delta)) > 1e-4 and counter < 30:
|
||||
delta = np.linalg.pinv(jac(x, poses, img_positions)).dot(fun(x, poses, img_positions))
|
||||
x = x - delta
|
||||
counter += 1
|
||||
return [x]
|
||||
|
||||
def compute_pos_python(self, poses, img_positions, check_quality=False):
|
||||
import scipy.optimize as opt
|
||||
|
||||
# This procedure is also described
|
||||
# in the MSCKF paper (Mourikis et al. 2007)
|
||||
x = np.array([img_positions[-1][0],
|
||||
img_positions[-1][1], 0.1])
|
||||
res = opt.leastsq(self.residual, x, Dfun=self.residual_jac, args=(poses, img_positions)) # scipy opt
|
||||
# res = self.gauss_newton(self.residual, self.residual_jac, x, (poses, img_positions)) # diy gauss_newton
|
||||
|
||||
alpha, beta, rho = res[0]
|
||||
rot_0_to_g = (orient.rotations_from_quats(poses[-1, 3:])).dot(self.to_c.T)
|
||||
return (rot_0_to_g.dot(np.array([alpha, beta, 1]))) / rho + poses[-1, :3]
|
||||
|
||||
|
||||
# EXPERIMENTAL CODE
|
||||
def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos):
|
||||
# only speed correction for now
|
||||
t_roll = 0.016 # 16ms rolling shutter?
|
||||
vroll, vpitch, vyaw = rot_rates
|
||||
A = 0.5 * np.array([[-1, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A.dot(poses[-1][3:7])
|
||||
v = np.append(v, q_dot)
|
||||
v = np.array([v[0], v[1], v[2], 0, 0, 0, 0])
|
||||
current_pose = poses[-1] + v * 0.05
|
||||
poses = np.vstack((current_pose, poses))
|
||||
dt = -img_positions[:, 1] * t_roll / 0.48
|
||||
errs = project(poses, ecef_pos) - project(poses + np.atleast_2d(dt).T.dot(np.atleast_2d(v)), ecef_pos)
|
||||
return img_positions - errs
|
||||
|
||||
|
||||
def project(poses, ecef_pos):
|
||||
img_positions = np.zeros((len(poses), 2))
|
||||
for i, p in enumerate(poses):
|
||||
cam_frame = orient.rotations_from_quats(p[3:]).T.dot(ecef_pos - p[:3])
|
||||
img_positions[i] = np.array([cam_frame[1] / cam_frame[0], cam_frame[2] / cam_frame[0]])
|
||||
return img_positions
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
K = int(sys.argv[1].split("_")[-1])
|
||||
LstSqComputer.generate_code(K=K)
|
||||
|
||||
@@ -1,90 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import sympy as sp
|
||||
import numpy as np
|
||||
|
||||
|
||||
def cross(x):
|
||||
ret = sp.Matrix(np.zeros((3, 3)))
|
||||
ret[0, 1], ret[0, 2] = -x[2], x[1]
|
||||
ret[1, 0], ret[1, 2] = x[2], -x[0]
|
||||
ret[2, 0], ret[2, 1] = -x[1], x[0]
|
||||
return ret
|
||||
|
||||
|
||||
def euler_rotate(roll, pitch, yaw):
|
||||
# make symbolic rotation matrix from eulers
|
||||
matrix_roll = sp.Matrix([[1, 0, 0],
|
||||
[0, sp.cos(roll), -sp.sin(roll)],
|
||||
[0, sp.sin(roll), sp.cos(roll)]])
|
||||
matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)],
|
||||
[0, 1, 0],
|
||||
[-sp.sin(pitch), 0, sp.cos(pitch)]])
|
||||
matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0],
|
||||
[sp.sin(yaw), sp.cos(yaw), 0],
|
||||
[0, 0, 1]])
|
||||
return matrix_yaw * matrix_pitch * matrix_roll
|
||||
|
||||
|
||||
def quat_rotate(q0, q1, q2, q3):
|
||||
# make symbolic rotation matrix from quat
|
||||
return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 + q0 * q3), 2 * (q1 * q3 - q0 * q2)],
|
||||
[2 * (q1 * q2 - q0 * q3), q0**2 - q1**2 + q2**2 - q3**2, 2 * (q2 * q3 + q0 * q1)],
|
||||
[2 * (q1 * q3 + q0 * q2), 2 * (q2 * q3 - q0 * q1), q0**2 - q1**2 - q2**2 + q3**2]]).T
|
||||
|
||||
|
||||
def quat_matrix_l(p):
|
||||
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
|
||||
[p[1], p[0], -p[3], p[2]],
|
||||
[p[2], p[3], p[0], -p[1]],
|
||||
[p[3], -p[2], p[1], p[0]]])
|
||||
|
||||
|
||||
def quat_matrix_r(p):
|
||||
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
|
||||
[p[1], p[0], p[3], -p[2]],
|
||||
[p[2], -p[3], p[0], p[1]],
|
||||
[p[3], p[2], -p[1], p[0]]])
|
||||
|
||||
|
||||
def sympy_into_c(sympy_functions, global_vars=None):
|
||||
from sympy.utilities import codegen
|
||||
routines = []
|
||||
for name, expr, args in sympy_functions:
|
||||
r = codegen.make_routine(name, expr, language="C99", global_vars=global_vars)
|
||||
|
||||
# argument ordering input to sympy is broken with function with output arguments
|
||||
nargs = []
|
||||
|
||||
# reorder the input arguments
|
||||
for aa in args:
|
||||
if aa is None:
|
||||
nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1, 1]))
|
||||
continue
|
||||
found = False
|
||||
for a in r.arguments:
|
||||
if str(aa.name) == str(a.name):
|
||||
nargs.append(a)
|
||||
found = True
|
||||
break
|
||||
if not found:
|
||||
# [1,1] is a hack for Matrices
|
||||
nargs.append(codegen.InputArgument(aa, dimensions=[1, 1]))
|
||||
|
||||
# add the output arguments
|
||||
for a in r.arguments:
|
||||
if type(a) == codegen.OutputArgument:
|
||||
nargs.append(a)
|
||||
|
||||
# assert len(r.arguments) == len(args)+1
|
||||
r.arguments = nargs
|
||||
|
||||
# add routine to list
|
||||
routines.append(r)
|
||||
|
||||
[(c_name, c_code), (h_name, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf")
|
||||
c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
|
||||
|
||||
c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#')
|
||||
c_code = 'extern "C" {\n#include <math.h>\n' + c_code + "\n}\n"
|
||||
|
||||
return c_header, c_code
|
||||
@@ -1,54 +0,0 @@
|
||||
#include <Eigen/QR>
|
||||
#include <Eigen/Dense>
|
||||
#include <iostream>
|
||||
|
||||
typedef Eigen::Matrix<double, KDIM*2, 3, Eigen::RowMajor> R3M;
|
||||
typedef Eigen::Matrix<double, KDIM*2, 1> R1M;
|
||||
typedef Eigen::Matrix<double, 3, 1> O1M;
|
||||
typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> M3D;
|
||||
|
||||
extern "C" {
|
||||
void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) {
|
||||
|
||||
double res[KDIM*2] = {0};
|
||||
double jac[KDIM*6] = {0};
|
||||
|
||||
O1M x(in_x);
|
||||
O1M delta;
|
||||
int counter = 0;
|
||||
while ((delta.squaredNorm() > 0.0001 and counter < 30) or counter == 0){
|
||||
res_fun(in_x, in_poses, in_img_positions, res);
|
||||
jac_fun(in_x, in_poses, in_img_positions, jac);
|
||||
R1M E(res); R3M J(jac);
|
||||
delta = (J.transpose()*J).inverse() * J.transpose() * E;
|
||||
x = x - delta;
|
||||
memcpy(in_x, x.data(), 3 * sizeof(double));
|
||||
counter = counter + 1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void compute_pos(double *to_c, double *poses, double *img_positions, double *param, double *pos) {
|
||||
param[0] = img_positions[KDIM*2-2];
|
||||
param[1] = img_positions[KDIM*2-1];
|
||||
param[2] = 0.1;
|
||||
gauss_newton(param, poses, img_positions);
|
||||
|
||||
Eigen::Quaterniond q;
|
||||
q.w() = poses[KDIM*7-4];
|
||||
q.x() = poses[KDIM*7-3];
|
||||
q.y() = poses[KDIM*7-2];
|
||||
q.z() = poses[KDIM*7-1];
|
||||
M3D RC(to_c);
|
||||
Eigen::Matrix3d R = q.normalized().toRotationMatrix();
|
||||
Eigen::Matrix3d rot = R * RC.transpose();
|
||||
|
||||
pos[0] = param[0]/param[2];
|
||||
pos[1] = param[1]/param[2];
|
||||
pos[2] = 1.0/param[2];
|
||||
O1M ecef_offset(poses + KDIM*7-7);
|
||||
O1M ecef_output(pos);
|
||||
ecef_output = rot*ecef_output + ecef_offset;
|
||||
memcpy(pos, ecef_output.data(), 3 * sizeof(double));
|
||||
}
|
||||
}
|
||||
@@ -1,123 +0,0 @@
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
|
||||
typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM;
|
||||
typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM;
|
||||
typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM;
|
||||
|
||||
void predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> RRM;
|
||||
|
||||
double nx[DIM] = {0};
|
||||
double in_F[EDIM*EDIM] = {0};
|
||||
|
||||
// functions from sympy
|
||||
f_fun(in_x, dt, nx);
|
||||
F_fun(in_x, dt, in_F);
|
||||
|
||||
|
||||
EEM F(in_F);
|
||||
EEM P(in_P);
|
||||
EEM Q(in_Q);
|
||||
|
||||
RRM F_main = F.topLeftCorner(MEDIM, MEDIM);
|
||||
P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose();
|
||||
P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM);
|
||||
P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose();
|
||||
|
||||
P = P + dt*Q;
|
||||
|
||||
// copy out state
|
||||
memcpy(in_x, nx, DIM * sizeof(double));
|
||||
memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
|
||||
}
|
||||
|
||||
// note: extra_args dim only correct when null space projecting
|
||||
// otherwise 1
|
||||
template <int ZDIM, int EADIM, bool MAHA_TEST>
|
||||
void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) {
|
||||
typedef Eigen::Matrix<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM;
|
||||
typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM;
|
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM;
|
||||
//typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM;
|
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M;
|
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXM;
|
||||
|
||||
double in_hx[ZDIM] = {0};
|
||||
double in_H[ZDIM * DIM] = {0};
|
||||
double in_H_mod[EDIM * DIM] = {0};
|
||||
double delta_x[EDIM] = {0};
|
||||
double x_new[DIM] = {0};
|
||||
|
||||
|
||||
// state x, P
|
||||
Eigen::Matrix<double, ZDIM, 1> z(in_z);
|
||||
EEM P(in_P);
|
||||
ZZM pre_R(in_R);
|
||||
|
||||
// functions from sympy
|
||||
h_fun(in_x, in_ea, in_hx);
|
||||
H_fun(in_x, in_ea, in_H);
|
||||
ZDM pre_H(in_H);
|
||||
|
||||
// get y (y = z - hx)
|
||||
Eigen::Matrix<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y;
|
||||
X1M y; XXM H; XXM R;
|
||||
if (Hea_fun){
|
||||
typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> ZAM;
|
||||
double in_Hea[ZDIM * EADIM] = {0};
|
||||
Hea_fun(in_x, in_ea, in_Hea);
|
||||
ZAM Hea(in_Hea);
|
||||
XXM A = Hea.transpose().fullPivLu().kernel();
|
||||
|
||||
|
||||
y = A.transpose() * pre_y;
|
||||
H = A.transpose() * pre_H;
|
||||
R = A.transpose() * pre_R * A;
|
||||
} else {
|
||||
y = pre_y;
|
||||
H = pre_H;
|
||||
R = pre_R;
|
||||
}
|
||||
// get modified H
|
||||
H_mod_fun(in_x, in_H_mod);
|
||||
DEM H_mod(in_H_mod);
|
||||
XEM H_err = H * H_mod;
|
||||
|
||||
// Do mahalobis distance test
|
||||
if (MAHA_TEST){
|
||||
XXM a = (H_err * P * H_err.transpose() + R).inverse();
|
||||
double maha_dist = y.transpose() * a * y;
|
||||
if (maha_dist > MAHA_THRESHOLD){
|
||||
R = 1.0e16 * R;
|
||||
}
|
||||
}
|
||||
|
||||
// Outlier resilient weighting
|
||||
double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());
|
||||
|
||||
// kalman gains and I_KH
|
||||
XXM S = ((H_err * P) * H_err.transpose()) + R/weight;
|
||||
XEM KT = S.fullPivLu().solve(H_err * P.transpose());
|
||||
//EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
|
||||
//EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
|
||||
//std::cout << "Here is the matrix rot:\n" << K << std::endl;
|
||||
EEM I_KH = Eigen::Matrix<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err);
|
||||
|
||||
// update state by injecting dx
|
||||
Eigen::Matrix<double, EDIM, 1> dx(delta_x);
|
||||
dx = (KT.transpose() * y);
|
||||
memcpy(delta_x, dx.data(), EDIM * sizeof(double));
|
||||
err_fun(in_x, delta_x, x_new);
|
||||
Eigen::Matrix<double, DIM, 1> x(x_new);
|
||||
|
||||
// update cov
|
||||
P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT);
|
||||
|
||||
// copy out state
|
||||
memcpy(in_x, x.data(), DIM * sizeof(double));
|
||||
memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
|
||||
memcpy(in_z, y.data(), y.rows() * sizeof(double));
|
||||
}
|
||||
|
||||
|
||||
@@ -1,58 +0,0 @@
|
||||
extern "C"{
|
||||
bool sane(double track [K + 1][5]) {
|
||||
double diffs_x [K-1];
|
||||
double diffs_y [K-1];
|
||||
int i;
|
||||
for (i = 0; i < K-1; i++) {
|
||||
diffs_x[i] = fabs(track[i+2][2] - track[i+1][2]);
|
||||
diffs_y[i] = fabs(track[i+2][3] - track[i+1][3]);
|
||||
}
|
||||
for (i = 1; i < K-1; i++) {
|
||||
if (((diffs_x[i] > 0.05 or diffs_x[i-1] > 0.05) and
|
||||
(diffs_x[i] > 2*diffs_x[i-1] or
|
||||
diffs_x[i] < .5*diffs_x[i-1])) or
|
||||
((diffs_y[i] > 0.05 or diffs_y[i-1] > 0.05) and
|
||||
(diffs_y[i] > 2*diffs_y[i-1] or
|
||||
diffs_y[i] < .5*diffs_y[i-1]))){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void merge_features(double *tracks, double *features, long long *empty_idxs) {
|
||||
double feature_arr [3000][5];
|
||||
memcpy(feature_arr, features, 3000 * 5 * sizeof(double));
|
||||
double track_arr [6000][K + 1][5];
|
||||
memcpy(track_arr, tracks, (K+1) * 6000 * 5 * sizeof(double));
|
||||
int match;
|
||||
int empty_idx = 0;
|
||||
int idx;
|
||||
for (int i = 0; i < 3000; i++) {
|
||||
match = feature_arr[i][4];
|
||||
if (track_arr[match][0][1] == match and track_arr[match][0][2] == 0){
|
||||
track_arr[match][0][0] = track_arr[match][0][0] + 1;
|
||||
track_arr[match][0][1] = feature_arr[i][1];
|
||||
track_arr[match][0][2] = 1;
|
||||
idx = track_arr[match][0][0];
|
||||
memcpy(track_arr[match][idx], feature_arr[i], 5 * sizeof(double));
|
||||
if (idx == K){
|
||||
// label complete
|
||||
track_arr[match][0][3] = 1;
|
||||
if (sane(track_arr[match])){
|
||||
// label valid
|
||||
track_arr[match][0][4] = 1;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// gen new track with this feature
|
||||
track_arr[empty_idxs[empty_idx]][0][0] = 1;
|
||||
track_arr[empty_idxs[empty_idx]][0][1] = feature_arr[i][1];
|
||||
track_arr[empty_idxs[empty_idx]][0][2] = 1;
|
||||
memcpy(track_arr[empty_idxs[empty_idx]][1], feature_arr[i], 5 * sizeof(double));
|
||||
empty_idx = empty_idx + 1;
|
||||
}
|
||||
}
|
||||
memcpy(tracks, track_arr, (K+1) * 6000 * 5 * sizeof(double));
|
||||
}
|
||||
}
|
||||
Regular → Executable
+92
-68
@@ -1,26 +1,27 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
import cereal.messaging as messaging
|
||||
import common.transformations.coordinates as coord
|
||||
from common.transformations.orientation import (ecef_euler_from_ned,
|
||||
euler_from_quat,
|
||||
ned_euler_from_ecef,
|
||||
quat_from_euler,
|
||||
rot_from_quat, rot_from_euler)
|
||||
from selfdrive.locationd.kalman.helpers import ObservationKind, KalmanError
|
||||
from selfdrive.locationd.kalman.models.live_kf import LiveKalman, States
|
||||
from common.transformations.orientation import ecef_euler_from_ned, \
|
||||
euler_from_quat, \
|
||||
ned_euler_from_ecef, \
|
||||
quat_from_euler, \
|
||||
rot_from_quat, rot_from_euler
|
||||
from rednose.helpers import KalmanError
|
||||
from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind
|
||||
from selfdrive.locationd.models.constants import GENERATED_DIR
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
#from datetime import datetime
|
||||
#from laika.gps_time import GPSTime
|
||||
|
||||
from sympy.utilities.lambdify import lambdify
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import euler_rotate
|
||||
from rednose.helpers.sympy_helpers import euler_rotate
|
||||
|
||||
|
||||
OUTPUT_DECIMATION = 2
|
||||
VISION_DECIMATION = 2
|
||||
SENSOR_DECIMATION = 10
|
||||
|
||||
@@ -47,7 +48,7 @@ def get_H():
|
||||
|
||||
class Localizer():
|
||||
def __init__(self, disabled_logs=[], dog=None):
|
||||
self.kf = LiveKalman()
|
||||
self.kf = LiveKalman(GENERATED_DIR)
|
||||
self.reset_kalman()
|
||||
self.max_age = .2 # seconds
|
||||
self.disabled_logs = disabled_logs
|
||||
@@ -57,6 +58,15 @@ class Localizer():
|
||||
self.calibrated = 0
|
||||
self.H = get_H()
|
||||
|
||||
self.posenet_invalid_count = 0
|
||||
self.posenet_speed = 0
|
||||
self.car_speed = 0
|
||||
|
||||
self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS])
|
||||
|
||||
self.unix_timestamp_millis = 0
|
||||
self.last_gps_fix = 0
|
||||
|
||||
@staticmethod
|
||||
def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov):
|
||||
predicted_std = np.sqrt(np.diagonal(predicted_cov))
|
||||
@@ -83,7 +93,7 @@ class Localizer():
|
||||
vel_device = device_from_ecef.dot(vel_ecef)
|
||||
device_from_ecef_eul = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]).T
|
||||
idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + list(range(States.ECEF_VELOCITY_ERR.start, States.ECEF_VELOCITY_ERR.stop))
|
||||
condensed_cov = predicted_cov[idxs][:,idxs]
|
||||
condensed_cov = predicted_cov[idxs][:, idxs]
|
||||
HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef])))
|
||||
vel_device_cov = HH.dot(condensed_cov).dot(HH.T)
|
||||
vel_device_std = np.sqrt(np.diagonal(vel_device_cov))
|
||||
@@ -141,25 +151,30 @@ class Localizer():
|
||||
def liveLocationMsg(self, time):
|
||||
fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P)
|
||||
|
||||
if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0):
|
||||
self.posenet_invalid_count += 1
|
||||
else:
|
||||
self.posenet_invalid_count = 0
|
||||
fix.posenetOK = self.posenet_invalid_count < 4
|
||||
|
||||
#fix.gpsWeek = self.time.week
|
||||
#fix.gpsTimeOfWeek = self.time.tow
|
||||
fix.unixTimestampMillis = self.unix_timestamp_millis
|
||||
|
||||
if self.filter_ready and self.calibrated:
|
||||
if np.linalg.norm(fix.positionECEF.std) < 50 and self.calibrated:
|
||||
fix.status = 'valid'
|
||||
elif self.filter_ready:
|
||||
elif np.linalg.norm(fix.positionECEF.std) < 50:
|
||||
fix.status = 'uncalibrated'
|
||||
else:
|
||||
fix.status = 'uninitialized'
|
||||
return fix
|
||||
|
||||
def update_kalman(self, time, kind, meas):
|
||||
if self.filter_ready:
|
||||
try:
|
||||
self.kf.predict_and_observe(time, kind, meas)
|
||||
except KalmanError:
|
||||
cloudlog.error("Error in predict and observe, kalman reset")
|
||||
self.reset_kalman()
|
||||
def update_kalman(self, time, kind, meas, R=None):
|
||||
try:
|
||||
self.kf.predict_and_observe(time, kind, meas, R=R)
|
||||
except KalmanError:
|
||||
cloudlog.error("Error in predict and observe, kalman reset")
|
||||
self.reset_kalman()
|
||||
#idx = bisect_right([x[0] for x in self.observation_buffer], time)
|
||||
#self.observation_buffer.insert(idx, (time, kind, meas))
|
||||
#while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
|
||||
@@ -167,48 +182,47 @@ class Localizer():
|
||||
# self.observation_buffer.pop(0)
|
||||
|
||||
def handle_gps(self, current_time, log):
|
||||
# ignore the message if the fix is invalid
|
||||
if log.flags % 2 == 0:
|
||||
return
|
||||
|
||||
self.last_gps_fix = current_time
|
||||
|
||||
self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
|
||||
fix_ecef = self.converter.ned2ecef([0, 0, 0])
|
||||
ecef_pos = self.converter.ned2ecef([0, 0, 0])
|
||||
ecef_vel = self.converter.ned2ecef_matrix.dot(np.array(log.vNED))
|
||||
ecef_pos_R = np.diag([(3*log.verticalAccuracy)**2]*3)
|
||||
ecef_vel_R = np.diag([(log.speedAccuracy)**2]*3)
|
||||
|
||||
|
||||
#self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3))
|
||||
self.unix_timestamp_millis = log.timestamp
|
||||
gps_est_error = np.sqrt((self.kf.x[0] - ecef_pos[0])**2 +
|
||||
(self.kf.x[1] - ecef_pos[1])**2 +
|
||||
(self.kf.x[2] - ecef_pos[2])**2)
|
||||
|
||||
# TODO initing with bad bearing not allowed, maybe not bad?
|
||||
if not self.filter_ready and log.speed > 5:
|
||||
self.filter_ready = True
|
||||
initial_ecef = fix_ecef
|
||||
gps_bearing = math.radians(log.bearing)
|
||||
initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing])
|
||||
initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef)
|
||||
gps_speed = log.speed
|
||||
quat_uncertainty = 0.2**2
|
||||
orientation_ecef = euler_from_quat(self.kf.x[States.ECEF_ORIENTATION])
|
||||
orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef)
|
||||
orientation_ned_gps = np.array([0, 0, np.radians(log.bearing)])
|
||||
orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi
|
||||
if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1:
|
||||
cloudlog.error("Locationd vs ubloxLocation orientation difference too large, kalman reset")
|
||||
initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
|
||||
self.reset_kalman(init_orient=initial_pose_ecef_quat)
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_ORIENTATION_FROM_GPS, initial_pose_ecef_quat)
|
||||
elif gps_est_error > 50:
|
||||
cloudlog.error("Locationd vs ubloxLocation position difference too large, kalman reset")
|
||||
self.reset_kalman()
|
||||
|
||||
initial_state = LiveKalman.initial_x
|
||||
initial_covs_diag = LiveKalman.initial_P_diag
|
||||
|
||||
initial_state[States.ECEF_POS] = initial_ecef
|
||||
initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat
|
||||
initial_state[States.ECEF_VELOCITY] = rot_from_quat(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))
|
||||
|
||||
initial_covs_diag[States.ECEF_POS_ERR] = 10**2
|
||||
initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty
|
||||
initial_covs_diag[States.ECEF_VELOCITY_ERR] = 1**2
|
||||
self.kf.init_state(initial_state, covs=np.diag(initial_covs_diag), filter_time=current_time)
|
||||
cloudlog.info("Filter initialized")
|
||||
elif self.filter_ready:
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef)
|
||||
gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 +
|
||||
(self.kf.x[1] - fix_ecef[1])**2 +
|
||||
(self.kf.x[2] - fix_ecef[2])**2)
|
||||
if gps_est_error > 50:
|
||||
cloudlog.error("Locationd vs ubloxLocation difference too large, kalman reset")
|
||||
self.reset_kalman()
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R)
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R)
|
||||
|
||||
def handle_car_state(self, current_time, log):
|
||||
self.speed_counter += 1
|
||||
|
||||
if self.speed_counter % SENSOR_DECIMATION == 0:
|
||||
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, [log.vEgo])
|
||||
self.car_speed = abs(log.vEgo)
|
||||
if log.vEgo == 0:
|
||||
self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0])
|
||||
|
||||
@@ -220,12 +234,13 @@ class Localizer():
|
||||
rot_device_std = self.device_from_calib.dot(log.rotStd)
|
||||
self.update_kalman(current_time,
|
||||
ObservationKind.CAMERA_ODO_ROTATION,
|
||||
np.concatenate([rot_device, rot_device_std]))
|
||||
np.concatenate([rot_device, 10*rot_device_std]))
|
||||
trans_device = self.device_from_calib.dot(log.trans)
|
||||
trans_device_std = self.device_from_calib.dot(log.transStd)
|
||||
self.posenet_speed = np.linalg.norm(trans_device)
|
||||
self.update_kalman(current_time,
|
||||
ObservationKind.CAMERA_ODO_TRANSLATION,
|
||||
np.concatenate([trans_device, trans_device_std]))
|
||||
np.concatenate([trans_device, 10*trans_device_std]))
|
||||
|
||||
def handle_sensors(self, current_time, log):
|
||||
# TODO does not yet account for double sensor readings in the log
|
||||
@@ -234,10 +249,6 @@ class Localizer():
|
||||
if sensor_reading.sensor == 5 and sensor_reading.type == 16:
|
||||
self.gyro_counter += 1
|
||||
if self.gyro_counter % SENSOR_DECIMATION == 0:
|
||||
if max(abs(self.kf.x[States.IMU_OFFSET])) > 0.07:
|
||||
cloudlog.info('imu frame angles exceeded, correcting')
|
||||
self.update_kalman(current_time, ObservationKind.IMU_FRAME, [0, 0, 0])
|
||||
|
||||
v = sensor_reading.gyroUncalibrated.v
|
||||
self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]])
|
||||
|
||||
@@ -254,9 +265,14 @@ class Localizer():
|
||||
self.calib_from_device = self.device_from_calib.T
|
||||
self.calibrated = log.calStatus == 1
|
||||
|
||||
def reset_kalman(self):
|
||||
self.filter_time = None
|
||||
self.filter_ready = False
|
||||
def reset_kalman(self, current_time=None, init_orient=None):
|
||||
self.filter_time = current_time
|
||||
init_x = LiveKalman.initial_x
|
||||
# too nonlinear to init on completely wrong
|
||||
if init_orient is not None:
|
||||
init_x[3:7] = init_orient
|
||||
self.kf.init_state(init_x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time)
|
||||
|
||||
self.observation_buffer = []
|
||||
|
||||
self.gyro_counter = 0
|
||||
@@ -264,14 +280,15 @@ class Localizer():
|
||||
self.speed_counter = 0
|
||||
self.cam_counter = 0
|
||||
|
||||
|
||||
def locationd_thread(sm, pm, disabled_logs=[]):
|
||||
if sm is None:
|
||||
sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration'])
|
||||
socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState']
|
||||
sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal'])
|
||||
if pm is None:
|
||||
pm = messaging.PubMaster(['liveLocationKalman'])
|
||||
|
||||
localizer = Localizer(disabled_logs=disabled_logs)
|
||||
camera_odometry_cnt = 0
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
@@ -290,13 +307,20 @@ def locationd_thread(sm, pm, disabled_logs=[]):
|
||||
elif sock == "liveCalibration":
|
||||
localizer.handle_live_calib(t, sm[sock])
|
||||
|
||||
if localizer.filter_ready and sm.updated['gpsLocationExternal']:
|
||||
t = sm.logMonoTime['gpsLocationExternal']
|
||||
msg = messaging.new_message('liveLocationKalman')
|
||||
msg.logMonoTime = t
|
||||
if sm.updated['cameraOdometry']:
|
||||
camera_odometry_cnt += 1
|
||||
|
||||
msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
|
||||
pm.send('liveLocationKalman', msg)
|
||||
if camera_odometry_cnt % OUTPUT_DECIMATION == 0:
|
||||
t = sm.logMonoTime['cameraOdometry']
|
||||
msg = messaging.new_message('liveLocationKalman')
|
||||
msg.logMonoTime = t
|
||||
|
||||
msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
|
||||
msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
|
||||
|
||||
gps_age = (t / 1e9) - localizer.last_gps_fix
|
||||
msg.liveLocationKalman.gpsOK = gps_age < 1.0
|
||||
pm.send('liveLocationKalman', msg)
|
||||
|
||||
|
||||
def main(sm=None, pm=None):
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
|
||||
#include <capnp/message.h>
|
||||
#include <capnp/serialize-packed.h>
|
||||
#include <Eigen/Dense>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
@@ -32,7 +32,6 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double
|
||||
void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
|
||||
for (cereal::SensorEventData::Reader sensor_event : sensor_events){
|
||||
if (sensor_event.getSensor() == 5 && sensor_event.getType() == 16) {
|
||||
sensor_data_time = current_time;
|
||||
double meas = -sensor_event.getGyroUncalibrated().getV()[0];
|
||||
update_state(C_gyro, R_gyro, current_time, meas);
|
||||
}
|
||||
@@ -43,16 +42,11 @@ void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odo
|
||||
double R = pow(5 * camera_odometry.getRotStd()[2], 2);
|
||||
double meas = camera_odometry.getRot()[2];
|
||||
update_state(C_posenet, R, current_time, meas);
|
||||
|
||||
auto trans = camera_odometry.getTrans();
|
||||
posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]);
|
||||
camera_odometry_time = current_time;
|
||||
}
|
||||
|
||||
void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
|
||||
steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS;
|
||||
car_speed = controls_state.getVEgo();
|
||||
controls_state_time = current_time;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#define DEGREES_TO_RADIANS 0.017453292519943295
|
||||
@@ -25,11 +25,7 @@ public:
|
||||
Eigen::Matrix2d P;
|
||||
double steering_angle = 0;
|
||||
double car_speed = 0;
|
||||
double posenet_speed = 0;
|
||||
double prev_update_time = -1;
|
||||
double controls_state_time = -1;
|
||||
double sensor_data_time = -1;
|
||||
double camera_odometry_time = -1;
|
||||
|
||||
Localizer();
|
||||
void handle_log(cereal::Event::Reader event);
|
||||
|
||||
@@ -0,0 +1,37 @@
|
||||
Import('env', 'arch')
|
||||
|
||||
templates = Glob('#rednose/templates/*')
|
||||
|
||||
sympy_helpers = "#rednose/helpers/sympy_helpers.py"
|
||||
ekf_sym = "#rednose/helpers/ekf_sym.py"
|
||||
|
||||
to_build = {
|
||||
'live': ('live_kf.py', 'generated'),
|
||||
'car': ('car_kf.py', 'generated'),
|
||||
}
|
||||
|
||||
if arch != "aarch64":
|
||||
to_build.update({
|
||||
'gnss': ('gnss_kf.py', 'generated'),
|
||||
'loc_4': ('loc_kf.py', 'generated'),
|
||||
'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', 'generated'),
|
||||
'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', 'generated'),
|
||||
'feature_handler_5': ('#rednose/helpers/feature_handler.py', 'generated'),
|
||||
'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', 'generated'),
|
||||
})
|
||||
|
||||
found = {}
|
||||
|
||||
for target, (command, generated_folder) in to_build.items():
|
||||
if File(command).exists():
|
||||
found[target] = (command, generated_folder)
|
||||
|
||||
for target, (command, generated_folder) in found.items():
|
||||
target_files = File([f'{generated_folder}/{target}.cpp', f'{generated_folder}/{target}.h'])
|
||||
command_file = File(command)
|
||||
|
||||
env.Command(target_files,
|
||||
[templates, command_file, sympy_helpers, ekf_sym],
|
||||
command_file.get_abspath() + " " + target + " " + Dir(generated_folder).get_abspath())
|
||||
|
||||
env.SharedLibrary(f'{generated_folder}/' + target, target_files[0])
|
||||
+22
-60
@@ -1,15 +1,17 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import math
|
||||
import sys
|
||||
from typing import Any, Dict
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
from selfdrive.locationd.kalman.helpers import ObservationKind
|
||||
from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code
|
||||
from rednose import KalmanFilter
|
||||
from rednose.helpers.ekf_sym import EKF_sym, gen_code
|
||||
from selfdrive.locationd.models.constants import ObservationKind
|
||||
|
||||
i = 0
|
||||
|
||||
|
||||
def _slice(n):
|
||||
global i
|
||||
s = slice(i, i + n)
|
||||
@@ -30,10 +32,10 @@ class States():
|
||||
STEER_ANGLE = _slice(1) # [rad]
|
||||
|
||||
|
||||
class CarKalman():
|
||||
class CarKalman(KalmanFilter):
|
||||
name = 'car'
|
||||
|
||||
x_initial = np.array([
|
||||
initial_x = np.array([
|
||||
1.0,
|
||||
15.0,
|
||||
0.0,
|
||||
@@ -48,8 +50,8 @@ class CarKalman():
|
||||
Q = np.diag([
|
||||
(.05/100)**2,
|
||||
.01**2,
|
||||
math.radians(0.002)**2,
|
||||
math.radians(0.1)**2,
|
||||
math.radians(0.02)**2,
|
||||
math.radians(0.25)**2,
|
||||
|
||||
.1**2, .01**2,
|
||||
math.radians(0.1)**2,
|
||||
@@ -57,15 +59,14 @@ class CarKalman():
|
||||
])
|
||||
P_initial = Q.copy()
|
||||
|
||||
obs_noise = {
|
||||
obs_noise : Dict[int, Any] = {
|
||||
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2),
|
||||
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2),
|
||||
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
|
||||
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
|
||||
ObservationKind.STIFFNESS: np.atleast_2d(5.0**2),
|
||||
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
|
||||
}
|
||||
|
||||
maha_test_kinds = [] # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED]
|
||||
global_vars = [
|
||||
sp.Symbol('mass'),
|
||||
sp.Symbol('rotational_inertia'),
|
||||
@@ -76,10 +77,9 @@ class CarKalman():
|
||||
]
|
||||
|
||||
@staticmethod
|
||||
def generate_code():
|
||||
dim_state = CarKalman.x_initial.shape[0]
|
||||
def generate_code(generated_dir):
|
||||
dim_state = CarKalman.initial_x.shape[0]
|
||||
name = CarKalman.name
|
||||
maha_test_kinds = CarKalman.maha_test_kinds
|
||||
|
||||
# globals
|
||||
m, j, aF, aR, cF_orig, cR_orig = CarKalman.global_vars
|
||||
@@ -136,58 +136,20 @@ class CarKalman():
|
||||
[sp.Matrix([x]), ObservationKind.STIFFNESS, None],
|
||||
]
|
||||
|
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds, global_vars=CarKalman.global_vars)
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=CarKalman.global_vars)
|
||||
|
||||
def __init__(self, steer_ratio=15, stiffness_factor=1, angle_offset=0):
|
||||
self.dim_state = self.x_initial.shape[0]
|
||||
x_init = self.x_initial
|
||||
def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0):
|
||||
dim_state = self.initial_x.shape[0]
|
||||
dim_state_err = self.P_initial.shape[0]
|
||||
x_init = self.initial_x
|
||||
x_init[States.STEER_RATIO] = steer_ratio
|
||||
x_init[States.STIFFNESS] = stiffness_factor
|
||||
x_init[States.ANGLE_OFFSET] = angle_offset
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(self.name, self.Q, self.x_initial, self.P_initial, self.dim_state, self.dim_state, maha_test_kinds=self.maha_test_kinds, global_vars=self.global_vars)
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
return self.filter.state()
|
||||
|
||||
@property
|
||||
def P(self):
|
||||
return self.filter.covs()
|
||||
|
||||
def predict(self, t):
|
||||
return self.filter.predict(t)
|
||||
|
||||
def rts_smooth(self, estimates):
|
||||
return self.filter.rts_smooth(estimates, norm_quats=False)
|
||||
|
||||
def get_R(self, kind, n):
|
||||
obs_noise = self.obs_noise[kind]
|
||||
dim = obs_noise.shape[0]
|
||||
R = np.zeros((n, dim, dim))
|
||||
for i in range(n):
|
||||
R[i, :, :] = obs_noise
|
||||
return R
|
||||
|
||||
def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
|
||||
if covs_diag is not None:
|
||||
P = np.diag(covs_diag)
|
||||
elif covs is not None:
|
||||
P = covs
|
||||
else:
|
||||
P = self.filter.covs()
|
||||
self.filter.init_state(state, P, filter_time)
|
||||
|
||||
def predict_and_observe(self, t, kind, data, R=None):
|
||||
if len(data) > 0:
|
||||
data = np.atleast_2d(data)
|
||||
|
||||
if R is None:
|
||||
R = self.get_R(kind, len(data))
|
||||
|
||||
self.filter.predict_and_update_batch(t, kind, data, R)
|
||||
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
CarKalman.generate_code()
|
||||
generated_dir = sys.argv[2]
|
||||
CarKalman.generate_code(generated_dir)
|
||||
@@ -0,0 +1,82 @@
|
||||
import os
|
||||
|
||||
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
|
||||
|
||||
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
|
||||
ECEF_VEL = 31
|
||||
ECEF_ORIENTATION_FROM_GPS = 32
|
||||
|
||||
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]
|
||||
+40
-35
@@ -1,13 +1,13 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
from selfdrive.locationd.kalman.helpers import KalmanError, ObservationKind
|
||||
from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import (euler_rotate,
|
||||
quat_matrix_r,
|
||||
quat_rotate)
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.locationd.models.constants import ObservationKind
|
||||
from rednose.helpers.ekf_sym import EKF_sym, gen_code
|
||||
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
|
||||
|
||||
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
|
||||
|
||||
@@ -46,9 +46,9 @@ class LiveKalman():
|
||||
0, 0, 0])
|
||||
|
||||
# state covariance
|
||||
initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
|
||||
10**2, 10**2, 10**2,
|
||||
10**2, 10**2, 10**2,
|
||||
initial_P_diag = np.array([1e16, 1e16, 1e16,
|
||||
1e6, 1e6, 1e6,
|
||||
1e4, 1e4, 1e4,
|
||||
1**2, 1**2, 1**2,
|
||||
0.05**2, 0.05**2, 0.05**2,
|
||||
0.02**2,
|
||||
@@ -57,8 +57,8 @@ class LiveKalman():
|
||||
|
||||
# process noise
|
||||
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.001**2, 0.001*2, 0.001**2,
|
||||
0.01**2, 0.01**2, 0.01**2,
|
||||
0.1**2, 0.1**2, 0.1**2,
|
||||
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
|
||||
(0.02 / 100)**2,
|
||||
@@ -66,7 +66,7 @@ class LiveKalman():
|
||||
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
|
||||
|
||||
@staticmethod
|
||||
def generate_code():
|
||||
def generate_code(generated_dir):
|
||||
name = LiveKalman.name
|
||||
dim_state = LiveKalman.initial_x.shape[0]
|
||||
dim_state_err = LiveKalman.initial_P_diag.shape[0]
|
||||
@@ -80,7 +80,7 @@ class LiveKalman():
|
||||
omega = state[States.ANGULAR_VELOCITY, :]
|
||||
vroll, vpitch, vyaw = omega
|
||||
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
|
||||
odo_scale = state[States.ODO_SCALE, :][0,:]
|
||||
odo_scale = state[States.ODO_SCALE, :][0, :]
|
||||
acceleration = state[States.ACCELERATION, :]
|
||||
imu_angles = state[States.IMU_OFFSET, :]
|
||||
|
||||
@@ -158,20 +158,22 @@ class LiveKalman():
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
imu_rot = euler_rotate(*imu_angles)
|
||||
h_gyro_sym = imu_rot * sp.Matrix([vroll + roll_bias,
|
||||
#imu_rot = euler_rotate(*imu_angles)
|
||||
h_gyro_sym = sp.Matrix([vroll + roll_bias,
|
||||
vpitch + pitch_bias,
|
||||
vyaw + yaw_bias])
|
||||
|
||||
pos = sp.Matrix([x, y, z])
|
||||
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
|
||||
h_acc_sym = imu_rot * (gravity + acceleration)
|
||||
h_acc_sym = (gravity + acceleration)
|
||||
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
|
||||
|
||||
speed = sp.sqrt(vx**2 + vy**2 + vz**2)
|
||||
speed = sp.sqrt(vx**2 + vy**2 + vz**2 + 1e-6)
|
||||
h_speed_sym = sp.Matrix([speed * odo_scale])
|
||||
|
||||
h_pos_sym = sp.Matrix([x, y, z])
|
||||
h_vel_sym = sp.Matrix([vx, vy, vz])
|
||||
h_orientation_sym = q
|
||||
h_imu_frame_sym = sp.Matrix(imu_angles)
|
||||
|
||||
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
||||
@@ -181,13 +183,15 @@ class LiveKalman():
|
||||
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
|
||||
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
||||
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
||||
[h_vel_sym, ObservationKind.ECEF_VEL, None],
|
||||
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
|
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
|
||||
|
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
|
||||
|
||||
def __init__(self):
|
||||
def __init__(self, generated_dir):
|
||||
self.dim_state = self.initial_x.shape[0]
|
||||
self.dim_state_err = self.initial_P_diag.shape[0]
|
||||
|
||||
@@ -197,10 +201,12 @@ class LiveKalman():
|
||||
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
|
||||
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
|
||||
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]),
|
||||
ObservationKind.ECEF_VEL: np.diag([.5**2, .5**2, .5**2]),
|
||||
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])}
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
|
||||
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
@@ -226,26 +232,24 @@ class LiveKalman():
|
||||
P = self.filter.covs()
|
||||
self.filter.init_state(state, P, filter_time)
|
||||
|
||||
def predict_and_observe(self, t, kind, data):
|
||||
if len(data) > 0:
|
||||
data = np.atleast_2d(data)
|
||||
def predict_and_observe(self, t, kind, meas, R=None):
|
||||
if len(meas) > 0:
|
||||
meas = np.atleast_2d(meas)
|
||||
if kind == ObservationKind.CAMERA_ODO_TRANSLATION:
|
||||
r = self.predict_and_update_odo_trans(data, t, kind)
|
||||
r = self.predict_and_update_odo_trans(meas, t, kind)
|
||||
elif kind == ObservationKind.CAMERA_ODO_ROTATION:
|
||||
r = self.predict_and_update_odo_rot(data, t, kind)
|
||||
r = self.predict_and_update_odo_rot(meas, t, kind)
|
||||
elif kind == ObservationKind.ODOMETRIC_SPEED:
|
||||
r = self.predict_and_update_odo_speed(data, t, kind)
|
||||
r = self.predict_and_update_odo_speed(meas, t, kind)
|
||||
else:
|
||||
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
|
||||
if R is None:
|
||||
R = self.get_R(kind, len(meas))
|
||||
elif len(R.shape) == 2:
|
||||
R = R[None]
|
||||
r = self.filter.predict_and_update_batch(t, kind, meas, R)
|
||||
|
||||
# Normalize quats
|
||||
quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
|
||||
|
||||
# Should not continue if the quats behave this weirdly
|
||||
if not (0.1 < quat_norm < 10):
|
||||
cloudlog.error("Kalman filter quaternions unstable")
|
||||
raise KalmanError
|
||||
|
||||
self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
|
||||
|
||||
return r
|
||||
@@ -281,4 +285,5 @@ class LiveKalman():
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
LiveKalman.generate_code()
|
||||
generated_dir = sys.argv[2]
|
||||
LiveKalman.generate_code(generated_dir)
|
||||
@@ -4,12 +4,8 @@
|
||||
#include <csignal>
|
||||
#include <unistd.h>
|
||||
|
||||
|
||||
#include <capnp/message.h>
|
||||
#include <capnp/serialize-packed.h>
|
||||
|
||||
#include "json11.hpp"
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/messaging.h"
|
||||
@@ -30,18 +26,8 @@ void sigpipe_handler(int sig) {
|
||||
int main(int argc, char *argv[]) {
|
||||
signal(SIGPIPE, (sighandler_t)sigpipe_handler);
|
||||
|
||||
Context * c = Context::create();
|
||||
SubSocket * controls_state_sock = SubSocket::create(c, "controlsState");
|
||||
SubSocket * sensor_events_sock = SubSocket::create(c, "sensorEvents");
|
||||
SubSocket * camera_odometry_sock = SubSocket::create(c, "cameraOdometry");
|
||||
PubSocket * live_parameters_sock = PubSocket::create(c, "liveParameters");
|
||||
|
||||
assert(controls_state_sock != NULL);
|
||||
assert(sensor_events_sock != NULL);
|
||||
assert(camera_odometry_sock != NULL);
|
||||
assert(live_parameters_sock != NULL);
|
||||
|
||||
Poller * poller = Poller::create({controls_state_sock, sensor_events_sock, camera_odometry_sock});
|
||||
SubMaster sm({"controlsState", "sensorEvents", "cameraOdometry"});
|
||||
PubMaster pm({"liveParameters"});
|
||||
|
||||
Localizer localizer;
|
||||
|
||||
@@ -104,84 +90,55 @@ int main(int argc, char *argv[]) {
|
||||
// Main loop
|
||||
int save_counter = 0;
|
||||
while (true){
|
||||
for (auto s : poller->poll(100)){
|
||||
Message * msg = s->receive();
|
||||
if (sm.update(100) == 0) continue;
|
||||
|
||||
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), msg->getData(), msg->getSize());
|
||||
if (sm.updated("controlsState")){
|
||||
localizer.handle_log(sm["controlsState"]);
|
||||
save_counter++;
|
||||
|
||||
capnp::FlatArrayMessageReader capnp_msg(amsg);
|
||||
cereal::Event::Reader event = capnp_msg.getRoot<cereal::Event>();
|
||||
double yaw_rate = -localizer.x[0];
|
||||
bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
|
||||
|
||||
localizer.handle_log(event);
|
||||
double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
|
||||
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
|
||||
|
||||
auto which = event.which();
|
||||
// Throw vision failure if posenet and odometric speed too different
|
||||
if (which == cereal::Event::CAMERA_ODOMETRY){
|
||||
if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.4 * localizer.car_speed, 5.0)) {
|
||||
posenet_invalid_count++;
|
||||
} else {
|
||||
posenet_invalid_count = 0;
|
||||
}
|
||||
} else if (which == cereal::Event::CONTROLS_STATE){
|
||||
save_counter++;
|
||||
capnp::MallocMessageBuilder msg;
|
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||
event.setLogMonoTime(nanos_since_boot());
|
||||
auto live_params = event.initLiveParameters();
|
||||
live_params.setValid(valid);
|
||||
live_params.setYawRate(localizer.x[0]);
|
||||
live_params.setGyroBias(localizer.x[1]);
|
||||
live_params.setAngleOffset(angle_offset_degrees);
|
||||
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
|
||||
live_params.setStiffnessFactor(learner.x);
|
||||
live_params.setSteerRatio(learner.sR);
|
||||
|
||||
double yaw_rate = -localizer.x[0];
|
||||
bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
|
||||
pm.send("liveParameters", msg);
|
||||
|
||||
// TODO: Fix in replay
|
||||
double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
|
||||
double camera_odometry_age = localizer.controls_state_time - localizer.camera_odometry_time;
|
||||
// Save parameters every minute
|
||||
if (save_counter % 6000 == 0) {
|
||||
json11::Json json = json11::Json::object {
|
||||
{"carVin", vin},
|
||||
{"carFingerprint", fingerprint},
|
||||
{"steerRatio", learner.sR},
|
||||
{"stiffnessFactor", learner.x},
|
||||
{"angleOffsetAverage", angle_offset_average_degrees},
|
||||
};
|
||||
|
||||
double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
|
||||
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
|
||||
|
||||
capnp::MallocMessageBuilder msg;
|
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||
event.setLogMonoTime(nanos_since_boot());
|
||||
auto live_params = event.initLiveParameters();
|
||||
live_params.setValid(valid);
|
||||
live_params.setYawRate(localizer.x[0]);
|
||||
live_params.setGyroBias(localizer.x[1]);
|
||||
live_params.setSensorValid(sensor_data_age < 5.0);
|
||||
live_params.setAngleOffset(angle_offset_degrees);
|
||||
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
|
||||
live_params.setStiffnessFactor(learner.x);
|
||||
live_params.setSteerRatio(learner.sR);
|
||||
live_params.setPosenetSpeed(localizer.posenet_speed);
|
||||
live_params.setPosenetValid((posenet_invalid_count < 4) && (camera_odometry_age < 5.0));
|
||||
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
live_parameters_sock->send((char*)bytes.begin(), bytes.size());
|
||||
|
||||
// Save parameters every minute
|
||||
if (save_counter % 6000 == 0) {
|
||||
json11::Json json = json11::Json::object {
|
||||
{"carVin", vin},
|
||||
{"carFingerprint", fingerprint},
|
||||
{"steerRatio", learner.sR},
|
||||
{"stiffnessFactor", learner.x},
|
||||
{"angleOffsetAverage", angle_offset_average_degrees},
|
||||
};
|
||||
|
||||
std::string out = json.dump();
|
||||
std::async(std::launch::async,
|
||||
[out]{
|
||||
write_db_value("LiveParameters", out.c_str(), out.length());
|
||||
});
|
||||
}
|
||||
std::string out = json.dump();
|
||||
std::async(std::launch::async,
|
||||
[out]{
|
||||
write_db_value("LiveParameters", out.c_str(), out.length());
|
||||
});
|
||||
}
|
||||
delete msg;
|
||||
}
|
||||
if (sm.updated("sensorEvents")){
|
||||
localizer.handle_log(sm["sensorEvents"]);
|
||||
}
|
||||
if (sm.updated("cameraOdometry")){
|
||||
localizer.handle_log(sm["cameraOdometry"]);
|
||||
}
|
||||
}
|
||||
|
||||
delete live_parameters_sock;
|
||||
delete controls_state_sock;
|
||||
delete camera_odometry_sock;
|
||||
delete sensor_events_sock;
|
||||
delete poller;
|
||||
delete c;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -5,18 +5,20 @@ import json
|
||||
import numpy as np
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import car
|
||||
from cereal import car, log
|
||||
from common.params import Params, put_nonblocking
|
||||
from selfdrive.locationd.kalman.models.car_kf import (CarKalman,
|
||||
ObservationKind, States)
|
||||
from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
|
||||
from selfdrive.locationd.models.constants import GENERATED_DIR
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
KalmanStatus = log.LiveLocationKalman.Status
|
||||
|
||||
CARSTATE_DECIMATION = 5
|
||||
|
||||
|
||||
class ParamsLearner:
|
||||
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
|
||||
self.kf = CarKalman(steer_ratio, stiffness_factor, angle_offset)
|
||||
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset)
|
||||
|
||||
self.kf.filter.set_mass(CP.mass) # pylint: disable=no-member
|
||||
self.kf.filter.set_rotational_inertia(CP.rotationalInertia) # pylint: disable=no-member
|
||||
@@ -32,6 +34,8 @@ class ParamsLearner:
|
||||
self.steering_angle = 0
|
||||
self.carstate_counter = 0
|
||||
|
||||
self.valid = True
|
||||
|
||||
def handle_log(self, t, which, msg):
|
||||
if which == 'liveLocationKalman':
|
||||
|
||||
@@ -39,21 +43,13 @@ class ParamsLearner:
|
||||
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
|
||||
|
||||
if self.active:
|
||||
self.kf.predict_and_observe(t,
|
||||
ObservationKind.ROAD_FRAME_YAW_RATE,
|
||||
np.array([[[-yaw_rate]]]),
|
||||
np.array([np.atleast_2d(yaw_rate_std**2)]))
|
||||
|
||||
if msg.inputsOK and msg.posenetOK and msg.status == KalmanStatus.valid:
|
||||
self.kf.predict_and_observe(t,
|
||||
ObservationKind.ROAD_FRAME_YAW_RATE,
|
||||
np.array([[[-yaw_rate]]]),
|
||||
np.array([np.atleast_2d(yaw_rate_std**2)]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
|
||||
|
||||
# Clamp values
|
||||
# x = self.kf.x
|
||||
# if not (10 < x[States.STEER_RATIO] < 25):
|
||||
# self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[[15.0]]]))
|
||||
|
||||
# if not (0.5 < x[States.STIFFNESS] < 3.0):
|
||||
# self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[[1.0]]]))
|
||||
|
||||
elif which == 'carState':
|
||||
self.carstate_counter += 1
|
||||
if self.carstate_counter % CARSTATE_DECIMATION == 0:
|
||||
@@ -105,6 +101,7 @@ def main(sm=None, pm=None):
|
||||
cloudlog.info("Parameter learner resetting to default values")
|
||||
|
||||
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage']))
|
||||
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
|
||||
|
||||
i = 0
|
||||
while True:
|
||||
@@ -132,6 +129,12 @@ def main(sm=None, pm=None):
|
||||
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
|
||||
msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET])
|
||||
msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(x[States.ANGLE_OFFSET_FAST])
|
||||
msg.liveParameters.valid = all((
|
||||
abs(msg.liveParameters.angleOffsetAverage) < 10.0,
|
||||
abs(msg.liveParameters.angleOffset) < 10.0,
|
||||
0.5 <= msg.liveParameters.stiffnessFactor <= 2.0,
|
||||
min_sr <= msg.liveParameters.steerRatio <= max_sr,
|
||||
))
|
||||
|
||||
i += 1
|
||||
if i % 6000 == 0: # once a minute
|
||||
@@ -143,13 +146,6 @@ def main(sm=None, pm=None):
|
||||
}
|
||||
put_nonblocking("LiveParameters", json.dumps(params))
|
||||
|
||||
# P = learner.kf.P
|
||||
# print()
|
||||
# print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5)
|
||||
# print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5)
|
||||
# print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5)
|
||||
# print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5)
|
||||
|
||||
pm.send('liveParameters', msg)
|
||||
|
||||
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#!/usr/bin/env python3
|
||||
# type: ignore
|
||||
import subprocess
|
||||
import os
|
||||
import sys
|
||||
|
||||
@@ -133,5 +133,3 @@ on of this parser
|
||||
self.ionoAlpha = []
|
||||
self.ionoBeta = []
|
||||
self.ionoCoeffsValid = False
|
||||
|
||||
|
||||
|
||||
@@ -12,7 +12,8 @@ for ublox version 8, not all functions may work.
|
||||
|
||||
|
||||
import struct
|
||||
import time, os
|
||||
import os
|
||||
import time
|
||||
|
||||
# protocol constants
|
||||
PREAMBLE1 = 0xb5
|
||||
@@ -291,7 +292,7 @@ class UBloxDescriptor:
|
||||
fields = self.fields[:]
|
||||
for f in fields:
|
||||
(fieldname, alen) = ArrayParse(f)
|
||||
if not fieldname in msg._fields:
|
||||
if fieldname not in msg._fields:
|
||||
break
|
||||
if alen == -1:
|
||||
f1.append(msg._fields[fieldname])
|
||||
@@ -327,7 +328,7 @@ class UBloxDescriptor:
|
||||
ret = self.name + ': '
|
||||
for f in self.fields:
|
||||
(fieldname, alen) = ArrayParse(f)
|
||||
if not fieldname in msg._fields:
|
||||
if fieldname not in msg._fields:
|
||||
continue
|
||||
v = msg._fields[fieldname]
|
||||
if isinstance(v, list):
|
||||
@@ -591,7 +592,7 @@ class UBloxMessage:
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if not type in msg_types:
|
||||
if type not in msg_types:
|
||||
raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
|
||||
msg_types[type].unpack(self)
|
||||
return self._fields, self._recs
|
||||
@@ -601,7 +602,7 @@ class UBloxMessage:
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if not type in msg_types:
|
||||
if type not in msg_types:
|
||||
raise UBloxError('Unknown message %s' % str(type))
|
||||
msg_types[type].pack(self)
|
||||
|
||||
@@ -610,7 +611,7 @@ class UBloxMessage:
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if not type in msg_types:
|
||||
if type not in msg_types:
|
||||
raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
|
||||
return msg_types[type].name
|
||||
|
||||
@@ -716,7 +717,7 @@ class UBlox:
|
||||
self.dev = PandaSerial(self.panda, 1, self.baudrate)
|
||||
|
||||
self.baudrate = 460800
|
||||
print("upping baud:",self.baudrate)
|
||||
print("upping baud:", self.baudrate)
|
||||
self.send_nmea("$PUBX,41,1,0007,0003,%u,0" % self.baudrate)
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#!/usr/bin/env python3
|
||||
# type: ignore
|
||||
|
||||
import os
|
||||
import serial
|
||||
@@ -20,7 +21,7 @@ print_dB = os.getenv("PRINT_DB") is not None # print antenna dB
|
||||
timeout = 1
|
||||
dyn_model = 4 # auto model
|
||||
baudrate = 460800
|
||||
ports = ["/dev/ttyACM0","/dev/ttyACM1"]
|
||||
ports = ["/dev/ttyACM0", "/dev/ttyACM1"]
|
||||
rate = 100 # send new data every 100ms
|
||||
|
||||
# which SV IDs we have seen and when we got iono
|
||||
@@ -82,7 +83,7 @@ def configure_ublox(dev):
|
||||
|
||||
def int_to_bool_list(num):
|
||||
# for parsing bool bytes
|
||||
return [bool(num & (1<<n)) for n in range(8)]
|
||||
return [bool(num & (1 << n)) for n in range(8)]
|
||||
|
||||
|
||||
def gen_ephemeris(ephem_data):
|
||||
@@ -133,9 +134,9 @@ def gen_solution(msg):
|
||||
msg_data['day'],
|
||||
msg_data['hour'],
|
||||
msg_data['min'],
|
||||
msg_data['sec'])
|
||||
- datetime.datetime(1970,1,1)).total_seconds())*1e+03
|
||||
+ msg_data['nano']*1e-06)
|
||||
msg_data['sec']) -
|
||||
datetime.datetime(1970, 1, 1)).total_seconds())*1e+03 +
|
||||
msg_data['nano']*1e-06)
|
||||
gps_fix = {'bearing': msg_data['headMot']*1e-05, # heading of motion in degrees
|
||||
'altitude': msg_data['height']*1e-03, # altitude above ellipsoid
|
||||
'latitude': msg_data['lat']*1e-07, # latitude in degrees
|
||||
@@ -162,9 +163,9 @@ def gen_nav_data(msg, nav_frame_buffer):
|
||||
|
||||
# parse GPS ephem
|
||||
gnssId = msg_meta_data['gnssId']
|
||||
if gnssId == 0:
|
||||
svId = msg_meta_data['svid']
|
||||
subframeId = GET_FIELD_U(measurements[1]['dwrd'], 3, 8)
|
||||
if gnssId == 0:
|
||||
svId = msg_meta_data['svid']
|
||||
subframeId = GET_FIELD_U(measurements[1]['dwrd'], 3, 8)
|
||||
words = []
|
||||
for m in measurements:
|
||||
words.append(m['dwrd'])
|
||||
@@ -243,7 +244,7 @@ def init_reader():
|
||||
return dev
|
||||
except serial.serialutil.SerialException as e:
|
||||
print(e)
|
||||
port_counter = (port_counter + 1)%len(ports)
|
||||
port_counter = (port_counter + 1) % len(ports)
|
||||
time.sleep(2)
|
||||
|
||||
def handle_msg(dev, msg, nav_frame_buffer):
|
||||
@@ -280,7 +281,7 @@ def main():
|
||||
global gpsLocationExternal, ubloxGnss
|
||||
nav_frame_buffer = {}
|
||||
nav_frame_buffer[0] = {}
|
||||
for i in range(1,33):
|
||||
for i in range(1, 33):
|
||||
nav_frame_buffer[0][i] = {}
|
||||
|
||||
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#!/usr/bin/env python3
|
||||
# type: ignore
|
||||
|
||||
import os
|
||||
from selfdrive.locationd.test import ublox
|
||||
|
||||
@@ -1,3 +1,5 @@
|
||||
# type: ignore
|
||||
|
||||
import sys
|
||||
import os
|
||||
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <signal.h>
|
||||
#include <unistd.h>
|
||||
#include <sched.h>
|
||||
@@ -12,13 +11,8 @@
|
||||
#include <math.h>
|
||||
#include <ctime>
|
||||
#include <chrono>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
|
||||
#include <capnp/serialize.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
|
||||
@@ -12,12 +12,8 @@
|
||||
#include <math.h>
|
||||
#include <ctime>
|
||||
#include <chrono>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
#include "messaging.hpp"
|
||||
#include <capnp/serialize.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
|
||||
@@ -12,13 +12,8 @@
|
||||
#include <math.h>
|
||||
#include <ctime>
|
||||
#include <chrono>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
#include "messaging.hpp"
|
||||
#include <capnp/serialize.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#include "common/util.h"
|
||||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
@@ -33,7 +28,7 @@ void set_do_exit(int sig) {
|
||||
}
|
||||
|
||||
using namespace ublox;
|
||||
|
||||
const long ZMQ_POLL_TIMEOUT = 1000; // In miliseconds
|
||||
int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) {
|
||||
LOGW("starting ubloxd");
|
||||
signal(SIGINT, (sighandler_t) set_do_exit);
|
||||
@@ -41,30 +36,15 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
|
||||
|
||||
UbloxMsgParser parser;
|
||||
|
||||
Context * c = Context::create();
|
||||
PubSocket * gpsLocationExternal = PubSocket::create(c, "gpsLocationExternal");
|
||||
PubSocket * ubloxGnss = PubSocket::create(c, "ubloxGnss");
|
||||
SubSocket * ubloxRaw = SubSocket::create(c, "ubloxRaw");
|
||||
|
||||
assert(gpsLocationExternal != NULL);
|
||||
assert(ubloxGnss != NULL);
|
||||
assert(ubloxRaw != NULL);
|
||||
|
||||
Poller * poller = Poller::create({ubloxRaw});
|
||||
|
||||
SubMaster sm({"ubloxRaw"});
|
||||
PubMaster pm({"ubloxGnss", "gpsLocationExternal"});
|
||||
|
||||
while (!do_exit) {
|
||||
Message * msg = poll_func(poller);
|
||||
if (msg == NULL) continue;
|
||||
if (sm.update(ZMQ_POLL_TIMEOUT) == 0) continue;
|
||||
|
||||
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), msg->getData(), msg->getSize());
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
const uint8_t *data = event.getUbloxRaw().begin();
|
||||
size_t len = event.getUbloxRaw().size();
|
||||
auto ubloxRaw = sm["ubloxRaw"].getUbloxRaw();
|
||||
const uint8_t *data = ubloxRaw.begin();
|
||||
size_t len = ubloxRaw.size();
|
||||
size_t bytes_consumed = 0;
|
||||
while(bytes_consumed < len && !do_exit) {
|
||||
size_t bytes_consumed_this_time = 0U;
|
||||
@@ -76,7 +56,7 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
|
||||
auto words = parser.gen_solution();
|
||||
if(words.size() > 0) {
|
||||
auto bytes = words.asBytes();
|
||||
send_func(gpsLocationExternal, bytes.begin(), bytes.size());
|
||||
pm.send("gpsLocationExternal", bytes.begin(), bytes.size());
|
||||
}
|
||||
} else
|
||||
LOGW("Unknown nav msg id: 0x%02X", parser.msg_id());
|
||||
@@ -86,14 +66,14 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
|
||||
auto words = parser.gen_raw();
|
||||
if(words.size() > 0) {
|
||||
auto bytes = words.asBytes();
|
||||
send_func(ubloxGnss, bytes.begin(), bytes.size());
|
||||
pm.send("ubloxGnss", bytes.begin(), bytes.size());
|
||||
}
|
||||
} else if(parser.msg_id() == MSG_RXM_SFRBX) {
|
||||
//LOGD("MSG_RXM_SFRBX");
|
||||
auto words = parser.gen_nav_data();
|
||||
if(words.size() > 0) {
|
||||
auto bytes = words.asBytes();
|
||||
send_func(ubloxGnss, bytes.begin(), bytes.size());
|
||||
pm.send("ubloxGnss", bytes.begin(), bytes.size());
|
||||
}
|
||||
} else
|
||||
LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id());
|
||||
@@ -103,7 +83,7 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
|
||||
auto words = parser.gen_mon_hw();
|
||||
if(words.size() > 0) {
|
||||
auto bytes = words.asBytes();
|
||||
send_func(ubloxGnss, bytes.begin(), bytes.size());
|
||||
pm.send("ubloxGnss", bytes.begin(), bytes.size());
|
||||
}
|
||||
} else {
|
||||
LOGW("Unknown mon msg id: 0x%02X", parser.msg_id());
|
||||
@@ -114,14 +94,7 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
|
||||
}
|
||||
bytes_consumed += bytes_consumed_this_time;
|
||||
}
|
||||
delete msg;
|
||||
}
|
||||
|
||||
delete poller;
|
||||
delete ubloxRaw;
|
||||
delete ubloxGnss;
|
||||
delete gpsLocationExternal;
|
||||
delete c;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -12,14 +12,10 @@
|
||||
#include <math.h>
|
||||
#include <ctime>
|
||||
#include <chrono>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
|
||||
#include "messaging.hpp"
|
||||
#include "impl_zmq.hpp"
|
||||
#include <capnp/serialize.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
@@ -30,13 +26,13 @@
|
||||
using namespace ublox;
|
||||
extern volatile sig_atomic_t do_exit;
|
||||
|
||||
void write_file(std::string fpath, uint8_t *data, int len) {
|
||||
void write_file(std::string fpath, uint8_t *to_write, int len) {
|
||||
FILE* f = fopen(fpath.c_str(), "wb");
|
||||
if (!f) {
|
||||
std::cout << "Open " << fpath << " failed" << std::endl;
|
||||
return;
|
||||
}
|
||||
fwrite(data, len, 1, f);
|
||||
fwrite(to_write, len, 1, f);
|
||||
fclose(f);
|
||||
}
|
||||
|
||||
@@ -72,11 +68,11 @@ Message * poll_ubloxraw_msg(Poller * poller) {
|
||||
}
|
||||
}
|
||||
|
||||
int send_gps_event(PubSocket *s, const void *buf, size_t len) {
|
||||
int send_gps_event(PubSocket *s, const void *buf, size_t length) {
|
||||
assert(s);
|
||||
write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, len);
|
||||
write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, length);
|
||||
save_idx++;
|
||||
return len;
|
||||
return length;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
Reference in New Issue
Block a user