def generate_code(generated_dir):
name = LiveKalman.name
dim_state = LiveKalman.initial_x.shape[0]
dim_state_err = LiveKalman.initial_P_diag.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
acc_bias = state[States.ACC_BIAS, :]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix(
[[0, -vroll, -vpitch, -vyaw], [vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll], [vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[
States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (
acceleration + acceleration_err)
f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
H_mod_sym[States.ECEF_POS,
States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop -
States.ECEF_POS.start)
H_mod_sym[States.ECEF_ORIENTATION,
States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(
state[3:7])[:, 1:]
H_mod_sym[States.ECEF_ORIENTATION.stop:,
States.ECEF_ORIENTATION_ERR.stop:] = np.eye(
dim_state - States.ECEF_ORIENTATION.stop)
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones((4)))
delta_quat[1:, :] = sp.Matrix(0.5 *
delta_x[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POS, :] = sp.Matrix(
nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(
nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(
nom_x[States.ECEF_ORIENTATION.stop:, :] +
delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[States.ECEF_POS_ERR,
0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] +
true_x[States.ECEF_POS, 0])
delta_quat = quat_matrix_r(
nom_x[States.ECEF_ORIENTATION,
0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR,
0] = sp.Matrix(2 * delta_quat[1:])
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(
-nom_x[States.ECEF_ORIENTATION.stop:, 0] +
true_x[States.ECEF_ORIENTATION.stop:, 0])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x], H_mod_sym,
f_err_sym, state_err_sym]
#
# Observation functions
#
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 = (gravity + acceleration + acc_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_orientation_sym = q
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[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_acc_stationary_sym, ObservationKind.NO_ACCEL, None]
]
# this returns a sympy routine for the jacobian of the observation function of the local vel
in_vec = sp.MatrixSymbol('in_vec', 6,
1) # roll, pitch, yaw, vx, vy, vz
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix(
[in_vec[3], in_vec[4], in_vec[5]]))
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
gen_code(generated_dir,
name,
f_sym,
dt,
state_sym,
obs_eqs,
dim_state,
dim_state_err,
eskf_params,
extra_routines=extra_routines)
# write constants to extra header file for use in cpp
live_kf_header = "#pragma once\n\n"
live_kf_header += "#include <unordered_map>\n"
live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
for state, slc in inspect.getmembers(States,
lambda x: type(x) == slice):
assert (slc.step is None) # unsupported
live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
live_kf_header += "\n"
for kind, val in inspect.getmembers(ObservationKind,
lambda x: type(x) == int):
live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
live_kf_header += "\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
for kind, noise in LiveKalman.obs_noise_diag.items():
live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n"
live_kf_header += "};\n\n"
open(os.path.join(generated_dir, "live_kf_constants.h"),
'w').write(live_kf_header)
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=[]):
# 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)
for sym in x_err_sym:
F_sym = F_sym.subs(sym, 0)
for i in xrange(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)
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 += "\n" + extra_header
code += "\n" + open(os.path.join(EXTERNAL_PATH, "ekf_c.c")).read()
code += "\n" + extra_post
header += "\n" + extra_header
compile_code(name, code, header, EXTERNAL_PATH)
def generate_code(generated_dir):
name = LiveKalman.name
dim_state = LiveKalman.initial_x.shape[0]
dim_state_err = LiveKalman.initial_P_diag.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
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, :]
acceleration = state[States.ACCELERATION, :]
imu_angles = state[States.IMU_OFFSET, :]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix(
[[0, -vroll, -vpitch, -vyaw], [vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll], [vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[
States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (
acceleration + acceleration_err)
f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
H_mod_sym[States.ECEF_POS,
States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop -
States.ECEF_POS.start)
H_mod_sym[States.ECEF_ORIENTATION,
States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(
state[3:7])[:, 1:]
H_mod_sym[States.ECEF_ORIENTATION.stop:,
States.ECEF_ORIENTATION_ERR.stop:] = np.eye(
dim_state - States.ECEF_ORIENTATION.stop)
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones((4)))
delta_quat[1:, :] = sp.Matrix(0.5 *
delta_x[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POS, :] = sp.Matrix(
nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(
nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(
nom_x[States.ECEF_ORIENTATION.stop:, :] +
delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[States.ECEF_POS_ERR,
0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] +
true_x[States.ECEF_POS, 0])
delta_quat = quat_matrix_r(
nom_x[States.ECEF_ORIENTATION,
0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR,
0] = sp.Matrix(2 * delta_quat[1:])
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(
-nom_x[States.ECEF_ORIENTATION.stop:, 0] +
true_x[States.ECEF_ORIENTATION.stop:, 0])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x], H_mod_sym,
f_err_sym, state_err_sym]
#
# Observation functions
#
#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 = (gravity + acceleration)
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
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)
obs_eqs = [
[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[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(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state,
dim_state_err, eskf_params)
x = sympy.symbols("x")
y = sympy.Function("y")
f = y(x)**2 + x
f_np = sympy.lambdify((y(x), x), f)
y0 = 0
xp = np.linspace(0, 1.9, 100)
yp = sp.integrate.odeint(f_np, y0, xp)
xm = np.linspace(0, -5, 100)
ym = sp.integrate.odeint(f_np, y0, xm)
fig, ax = plt.subplots(1, 1, figsize=(4, 4))
plot_direction_field(x, y(x), f, ax=ax)
ax.plot(xm, ym, 'b', lw=2)
ax.plot(xp, yp, 'r', lw=2)
A = sympy.MatrixSymbol('A', 3, 3)
M = sympy.MatrixSymbol('M', 3, 3)
beta = sympy.symbols("beta")
def rk4(f, x0, y0, x1, n):
vx = [0] * (n + 1)
vy = [0] * (n + 1)
h = (x1 - x0) / float(n)
vx[0] = x = x0
vy[0] = y = y0
for i in range(1, n + 1):
k1 = h * f(x, y)
k2 = h * f(x + 0.5 * h, y + 0.5 * k1)
k3 = h * f(x + 0.5 * h, y + 0.5 * k2)
k4 = h * f(x + h, y + k3)
Ys[i] = np.dot(p0, E[1])
boundingH = max([np.amax(Xs), np.amin(Xs)]) * 2
boundingV = max([np.amax(Ys), np.amin(Ys)]) * 2
for i in range(node_num):
Xs_scaled[i] = scale(Xs[i], True)
Ys_scaled[i] = scale(Ys[i], False)
update_points()
print("init: ready")
# sympy
a1, b1, c1, a2, b2, c2, t, s = sp.symbols('a1 b1 c1 a2 b2 c2 t s') # variables
x2_s, y2_s = sp.symbols('x2_s y2_s') # values
P_i = sp.MatrixSymbol('P_i', high_dim, 1)
E1 = sp.MatrixSymbol('E1', high_dim, 1)
E2 = sp.MatrixSymbol('E2', high_dim, 1)
var = (x2_s, y2_s, P_i, E1, E2, a1, b1, c1, a2, b2, c2, t, s)
_E1 = sp.Matrix(a1 * E1 + b1 * E2 + c1 * P_i)
_E2 = sp.Matrix(a2 * E1 + b2 * E2 + c2 * P_i)
R = sp.Matrix(s * E1 + t * E2)
f = Matrix([
_E1.T * _E1 - Matrix([1]), _E2.T * _E2 - Matrix([1]), _E1.T * _E2,
R.T * R - Matrix([1]), _E1.T * R - sp.Matrix(E1).T * R,
_E2.T * R - sp.Matrix(E2).T * R,
sp.Matrix(P_i).T * (_E1) - Matrix([x2_s]),
sp.Matrix(P_i).T * (_E2) - Matrix([y2_s])
])
if compile_pdf:
cur_dir = os.path.abspath(os.path.curdir)
os.chdir(makefile_trunk)
import subprocess
tex_out = subprocess.check_output(
['pdflatex', '--interaction=nonstopmode',
os.path.split(tex_filename)[-1]],stderr=subprocess.STDOUT)
os.chdir(cur_dir)
if __name__== "__main__":
import sympy as sp
s0 = sp.Symbol('\sigma_0^2')
s1 = sp.Symbol('\sigma_1^2')
a01 = sp.Symbol('a_{01}')
a02 = sp.Symbol('a_{02}')
a12 = sp.Symbol('a_{12}')
y = sp.Symbol('y')
x = sp.Matrix([[sp.Symbol('x_0')],[sp.Symbol('x_1')]])
Sxx = sp.Matrix([[s0,a01*s0],[a01*s0,s1]])
a = sp.Matrix([[a02],[a12]])
print(Sxx)
print((a.T*Sxx*a)[0,0])
from pyapprox.sympy_utilities import convert_sympy_equations_to_latex
convert_sympy_equations_to_latex([
sp.Eq(sp.MatrixSymbol('\Sigma_{xx}',2,2),Sxx),
sp.Eq(y,(a.T*x)[0]),
sp.simplify((a.T*Sxx*a)[0,0]),],'temp/temp.tex')
def generate_code(generated_dir):
dim_state = CarKalman.initial_x.shape[0]
name = CarKalman.name
# globals
m, j, aF, aR, cF_orig, cR_orig = CarKalman.global_vars
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
x = state[States.STIFFNESS, :][0, 0]
cF, cR = x * cF_orig, x * cR_orig
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast)
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[
sp.Matrix([angle_offset_fast]),
ObservationKind.ANGLE_OFFSET_FAST, None
],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([x]), ObservationKind.STIFFNESS, None],
]
gen_code(generated_dir,
name,
f_sym,
dt,
state_sym,
obs_eqs,
dim_state,
dim_state,
global_vars=CarKalman.global_vars)
def __init__(self, scheme, sorder, generator, settings=None):
xx, yy, zz = sp.symbols('xx, yy, zz')
self.symb_coord_local = [xx, yy, zz]
self.symb_coord = scheme.symb_coord
self.dim = scheme.dim
self.ns = scheme.stencil.nv_ptr[-1]
self.M = scheme.M.subs(scheme.param.items())
self.invM = scheme.invM.subs(scheme.param.items())
self.all_velocities = scheme.stencil.get_all_velocities()
self.mv = sp.MatrixSymbol('m', self.ns, 1)
if scheme.rel_vel is not None:
self.rel_vel_symb = [rel_ux, rel_uy, rel_uz][:self.dim]
self.rel_vel = sp.Matrix(scheme.rel_vel)
self.Tu = scheme.Tu.subs(scheme.param.items())
self.Tmu = scheme.Tmu.subs(scheme.param.items())
self.Mu = self.Tu * self.M
self.invMu = self.invM * self.Tmu
alltogether(self.Mu, nsimplify=True)
alltogether(self.invMu, nsimplify=True)
else:
self.rel_vel_symb = None
self.consm = {}
for k, v in scheme.consm.items():
self.consm[str(k)] = v
self.sorder = sorder
self.generator = generator
subs_coords = list(zip(self.symb_coord, self.symb_coord_local))
subs_moments = list(zip(scheme.consm.keys(), [self.mv[int(i), 0] for i in scheme.consm.values()]))
to_subs = subs_coords + list(scheme.param.items())
to_subs_full = to_subs + subs_moments
self.eq = recursive_sub(scheme.EQ, to_subs_full)
self.s = recursive_sub(scheme.s, to_subs_full)
alltogether(self.eq, nsimplify=True)
alltogether(self.s)
if self.rel_vel_symb:
self.rel_vel = recursive_sub(self.rel_vel, to_subs_full)
alltogether(self.rel_vel)
self.source_eq = []
for source in scheme._source_terms:
if source:
for k, v in source.items():
lhs = recursive_sub(k, to_subs_full)
if isinstance(v, (float, int)):
rhs = v
else:
rhs = recursive_sub(v, to_subs)
self.source_eq.append((lhs, rhs))
self.vmax = [0]*3
self.vmax[:scheme.dim] = scheme.stencil.vmax
self.local_vars = self.symb_coord_local[:self.dim]
self.settings = settings if settings else {}
def ekf_generate_c_code(f, F, h, H, x, u, f_additional_in=[], h_additional_in=[]):
# generated C code matrix access is in row-major order
# [source: sympy/printing/ccode.py:215, _print_MatrixElement()]
# Since Eigen uses column-major by default we transpose the matrices before
# generating the C code
H = H.transpose()
F = F.transpose()
f_additional_in_sym = [sp.symbols('in{}_{}'.format(i+2, var.name)) for i, var in enumerate(f_additional_in)]
f_add_subs_tab = list(zip(f_additional_in, f_additional_in_sym))
h_additional_in_sym = [sp.symbols('in{}_{}'.format(i+1, var.name)) for i, var in enumerate(h_additional_in)]
h_add_subs_tab = list(zip(h_additional_in, h_additional_in_sym))
x_sym = sp.MatrixSymbol('in0_x', len(x), 1)
x_subs_tab = [(elem_sym, x_sym[i, 0]) for i, elem_sym in enumerate(x)]
u_sym = sp.MatrixSymbol('in1_u', len(u), 1)
u_subs_tab = [(elem_sym, u_sym[i, 0]) for i, elem_sym in enumerate(u)]
subs_tab = x_subs_tab + u_subs_tab + f_add_subs_tab + h_add_subs_tab
# c_code = generate_c_code('f', f.subs(subs_tab)) + '\n'
# c_code += generate_c_code('F', F.subs(subs_tab)) + '\n'
# c_code += generate_c_code('h', h.subs(subs_tab)) + '\n'
# c_code += generate_c_code('H', H.subs(subs_tab)) + '\n'
no_return_val = []
no_local_vars = []
f_additional_in_arg = [cg.InputArgument(sym) for sym in f_additional_in_sym]
h_additional_in_arg = [cg.InputArgument(sym) for sym in h_additional_in_sym]
f_out_sym = sp.MatrixSymbol('f_out', len(x), 1)
f_arg_list = [cg.InputArgument(x_sym, dimensions=x_sym.shape),
cg.InputArgument(u_sym, dimensions=u_sym.shape)]
f_arg_list += f_additional_in_arg
f_arg_list += [cg.OutputArgument(f_out_sym, f_out_sym, f.subs(subs_tab), dimensions=f_out_sym.shape)]
F_out_sym = sp.MatrixSymbol('F_out', F.shape[0], F.shape[1])
F_arg_list = [cg.InputArgument(x_sym, dimensions=x_sym.shape),
cg.InputArgument(u_sym, dimensions=u_sym.shape)]
F_arg_list += f_additional_in_arg
F_arg_list += [cg.OutputArgument(F_out_sym, F_out_sym, F.subs(subs_tab), dimensions=F_out_sym.shape)]
h_out_sym = sp.MatrixSymbol('h_out', len(h), 1)
h_arg_list = [cg.InputArgument(x_sym, dimensions=x_sym.shape)]
h_arg_list += h_additional_in_arg
h_arg_list += [cg.OutputArgument(h_out_sym, h_out_sym, h.subs(subs_tab), dimensions=h_out_sym.shape)]
H_out_sym = sp.MatrixSymbol('H_out', H.shape[0], H.shape[1])
H_arg_list = [cg.InputArgument(x_sym, dimensions=x_sym.shape)]
H_arg_list += h_additional_in_arg
H_arg_list += [cg.OutputArgument(H_out_sym, H_out_sym, H.subs(subs_tab), dimensions=H_out_sym.shape)]
routines = [cg.Routine("f", f_arg_list, no_return_val, no_local_vars),
cg.Routine("F", F_arg_list, no_return_val, no_local_vars),
cg.Routine("h", h_arg_list, no_return_val, no_local_vars),
cg.Routine("H", H_arg_list, no_return_val, no_local_vars)]
code_gen = cg.get_code_generator("C", "projectname")
[(c_name, c_code), (h_name, c_header)] = code_gen.write(routines, "prefix", header=False)
c_code = clean_c_code(c_code, use_single_float=True)
c_code_head = '// This file has been automatically generated\n'
c_code_head += '// DO NOT EDIT!\n\n'
c_code_head += '#include <math.h>\n\n'
c_code_head += 'const int STATE_DIM = {};\n'.format(len(x))
c_code_head += 'const int CONTROL_DIM = {};\n'.format(len(u))
c_code_head += 'const int MEASURE_DIM = {};\n'.format(len(h))
c_code_head += '\n\n'
return c_code_head + c_code
def VEC(name: str, n: int) -> sp.MatrixSymbol:
'n次元ベクトルを表す記号を返す'
return sp.MatrixSymbol(name, n, 1)
def Vec(n: int, name: str) -> sp.MatrixSymbol:
'n次元ベクトルの記号を与える'
return sp.MatrixSymbol(sp.Symbol(name), n, 1)
def build_module_autowrap(expr,syms,module_name,tmp_dir,out_dir,dummify=False,cse=False,cse_ordering="canonical",build_vectorized=False,pretty_dummy_symbol_names=False,verbose=False,request_delete_tmp_dir=True):
tmp_dir_exists_before_call = os.path.exists(tmp_dir)
is_matrix_expr = isinstance(expr,sympy.Matrix) or isinstance(expr,sympy.MutableMatrix) or isinstance(expr,sympy.ImmutableMatrix)
if dummify:
if verbose: print "flashlight.sympy: Generating dummy symbols for %s..." % module_name
expr,syms = _dummify(expr,syms,pretty_dummy_symbol_names)
if verbose: print "flashlight.sympy: Finished generating dummy symbols for %s." % module_name
if cse:
include_str,subexpr_eval_str = generate_c_code_cse(expr,syms,module_name,tmp_dir,out_dir,cse_ordering=cse_ordering,verbose=verbose,request_delete_tmp_dir=request_delete_tmp_dir)
if is_matrix_expr:
expr = sympy.Matrix.zeros(expr.rows,expr.cols)
else:
expr = 0
syms = list(syms)
if is_matrix_expr:
out_sym = sympy.MatrixSymbol('out_%s' % abs(hash(sympy.ImmutableMatrix(expr))), *expr.shape)
syms = syms + [out_sym]
if verbose: print "flashlight.sympy: Generating Cython code for %s..." % module_name
_autowrap_skip_compile(expr=expr,backend="cython",verbose=verbose,args=syms,tempdir=tmp_dir)
if verbose: print "flashlight.sympy: Modifying autogenerated Cython files for %s" % module_name
with open("%s/setup.py" % tmp_dir, "r") as f:
setup_py_str = f.read()
tmp_module_name = re.findall("wrapper_module_[0-9]*", setup_py_str)
tmp_autofunc_code_name = re.findall("wrapped_code_[0-9]*", setup_py_str)
assert len(tmp_module_name) == 2
assert len(tmp_autofunc_code_name) == 1
assert tmp_module_name[0] == tmp_module_name[1]
tmp_module_name = tmp_module_name[0]
tmp_autofunc_code_name = tmp_autofunc_code_name[0]
with open("%s/%s.pyx" % (tmp_dir,tmp_module_name), "r") as f:
wrapper_module_pyx_str = f.read()
with open("%s/%s.c" % (tmp_dir,tmp_autofunc_code_name), "r") as f:
autofunc_c_str = f.read()
with open("%s/%s.h" % (tmp_dir,tmp_autofunc_code_name), "r") as f:
autofunc_h_str = f.read()
with open("%s/%s_setup.py" % (tmp_dir,module_name), "w") as f:
setup_py_str_mod = setup_py_str
setup_py_str_mod = setup_py_str_mod.replace("from Cython.Distutils import build_ext","from Cython.Distutils import build_ext\n\nimport numpy\nimport os\n\nos.environ['CC'] = '%s'\nos.environ['CXX'] = '%s'\n\n" % (cc_compiler, cxx_compiler))
setup_py_str_mod = setup_py_str_mod.replace("extra_compile_args=['-std=c99']","extra_compile_args=['-std=c99','-fno-var-tracking','-fno-var-tracking-assignments'], include_dirs=[numpy.get_include()]")
setup_py_str_mod = setup_py_str_mod.replace("%s" % tmp_module_name, "%s" % module_name)
setup_py_str_mod = setup_py_str_mod.replace("%s" % tmp_autofunc_code_name, "%s_autofunc" % module_name)
f.write(setup_py_str_mod)
with open("%s/%s.pyx" % (tmp_dir,module_name), "w") as f:
wrapper_module_pyx_str_mod = wrapper_module_pyx_str
wrapper_module_pyx_str_mod = wrapper_module_pyx_str_mod.replace("%s" % tmp_autofunc_code_name, "%s_autofunc" % module_name)
f.write(wrapper_module_pyx_str_mod)
if not cse:
with open("%s/%s_autofunc.c" % (tmp_dir,module_name), "w") as f:
autofunc_c_str_mod = autofunc_c_str
autofunc_c_str_mod = autofunc_c_str_mod.replace('#include "%s.h"' % tmp_autofunc_code_name, '#include "%s.h"' % (module_name+"_autofunc"))
f.write(autofunc_c_str_mod)
with open("%s/%s_autofunc.h" % (tmp_dir,module_name), "w") as f:
autofunc_h_str_mod = autofunc_h_str
autofunc_h_str_mod = autofunc_h_str_mod.replace("AUTOWRAP__%s__H" % tmp_autofunc_code_name.upper(), "%s_H" % (module_name+"_autofunc").upper())
f.write(autofunc_h_str_mod)
if build_vectorized:
if verbose: print "flashlight.sympy: Generating Cython code for %s_vectorized..." % module_name
if is_matrix_expr:
c_return_type_str = "void"
c_out_var_str = "out_123456789"
c_func_signature_str = str( [ "double %s" % str(sym) for sym in syms[:-1] ] )[1:-1].replace("'","") + ", double *%s" % syms[-1]
cython_out_type_str = "numpy.ndarray[FLOAT64_DTYPE_t, ndim=3]"
cython_out_shape_str = "(args.shape[0],%s,%s)" % (expr.rows,expr.cols)
cython_out_v_type_str = "FLOAT64_DTYPE_t [:,:,:]"
cython_args_str = str( [ "args[i,%d]" % i for i in range(len(syms[:-1])) ] )[1:-1].replace("'","") + ", &out_v[i,0,0]"
cython_loop_body_str = "autofunc(%s)" % cython_args_str
else:
c_return_type_str = "double"
c_func_signature_str = str( [ "double %s" % str(sym) for sym in syms ] )[1:-1].replace("'","")
cython_out_type_str = "numpy.ndarray[FLOAT64_DTYPE_t, ndim=1]"
cython_out_shape_str = "(args.shape[0])"
cython_out_v_type_str = "FLOAT64_DTYPE_t [:]"
cython_args_str = str( [ "args[i,%d]" % i for i in range(len(syms)) ] )[1:-1].replace("'","")
cython_loop_body_str = "out_v[i] = autofunc(%s)" % cython_args_str
vectorized_setup_py_str_eval = vectorized_setup_py_str % (cc_compiler, cxx_compiler, module_name, module_name, module_name)
vectorized_pyx_str_eval = vectorized_pyx_str % (module_name, c_return_type_str, c_func_signature_str, cython_out_type_str, cython_out_shape_str, cython_out_v_type_str, cython_loop_body_str)
with open("%s/%s_vectorized_setup.py" % (tmp_dir,module_name), "w") as f:
vectorized_setup_py_str_mod = vectorized_setup_py_str_eval
f.write(vectorized_setup_py_str_mod)
with open("%s/%s_vectorized.pyx" % (tmp_dir,module_name), "w") as f:
vectorized_pyx_str_mod = vectorized_pyx_str_eval
f.write(vectorized_pyx_str_mod)
cwd = os.getcwd()
try:
os.chdir(tmp_dir)
if verbose: print "flashlight.sympy: Building Cython code for %s..." % module_name
cmd = "python %s_setup.py build_ext --inplace" % module_name
if verbose: print cmd
output = subprocess.check_output(cmd, shell=True)
if verbose and len(output) > 0: print output
if build_vectorized:
if verbose: print "flashlight.sympy: Building Cython code for %s_vectorized..." % module_name
cmd = "python %s_vectorized_setup.py build_ext --inplace" % module_name
if verbose: print cmd
output = subprocess.check_output(cmd, shell=True)
if verbose and len(output) > 0: print output
finally:
os.chdir(cwd)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
cmd = "cp %s/%s.so %s/%s.so" % (tmp_dir,module_name,out_dir,module_name)
if verbose: print cmd
output = subprocess.check_output(cmd, shell=True)
if verbose and len(output) > 0: print output
if build_vectorized:
cmd = "cp %s/%s_vectorized.so %s/%s_vectorized.so" % (tmp_dir,module_name,out_dir,module_name)
if verbose: print cmd
output = subprocess.check_output(cmd, shell=True)
if verbose and len(output) > 0: print output
# cmd = "cp %s/%s_autofunc.c %s/%s_autofunc.c" % (tmp_dir,module_name,out_dir,module_name)
# if verbose: print cmd
# output = subprocess.check_output(cmd, shell=True)
# if verbose and len(output) > 0: print output
# cmd = "cp %s/%s_autofunc.h %s/%s_autofunc.h" % (tmp_dir,module_name,out_dir,module_name)
# if verbose: print cmd
# output = subprocess.check_output(cmd, shell=True)
# if verbose and len(output) > 0: print output
if not tmp_dir_exists_before_call and request_delete_tmp_dir:
cmd = "rm -rf %s" % tmp_dir
if verbose: print cmd
output = subprocess.check_output(cmd, shell=True)
if verbose and len(output) > 0: print output
from sympy.codegen.ast import real, float32
from sympy.printing.ccode import C99CodePrinter
printer = C99CodePrinter()
# custom code printer that will not generate such nonesene as x^2 -> pow(x, 2)
class CustomCodePrinter(C99CodePrinter):
def _print_Pow(self, expr):
if expr.exp.is_integer and expr.exp > 0 and expr.exp < 5:
return '*'.join([self._print(expr.base) for i in range(expr.exp)])
else:
return super()._print_Pow(expr)
customprinter = CustomCodePrinter()
customprinter.type_aliases[real] = float32 # cosf instead of cos
default_datatypes["float"].cname = "float" # float instead of double
params = [
theta, tx0, ty0, tz0, tx1, ty1, tz1, qw0, qx0, qy0, qz0, qw1, qx1, qy1,
qz1, s000, s001, s002, s003, s011, s012, s013, s022, s023, s100, s101,
s102, s103, s111, s112, s113, s122, s123
]
R = sp.MatrixSymbol("coeff", A.shape[0], A.shape[1])
P = sp.MatrixSymbol('p', len(params), 1)
param_map = dict(zip(params, P))
B = A.xreplace(param_map)
codegen(('motion_derivative_coefficients', sp.Eq(R, B)),
language='c',
printer=customprinter,
prefix='motion_derivative_coefficients',
to_files=True)
def generate_code(N=4):
dim_augment = LocKalman.dim_augment
dim_augment_err = LocKalman.dim_augment_err
dim_main = LocKalman.x_initial.shape[0]
dim_main_err = LocKalman.P_initial.shape[0]
dim_state = dim_main + dim_augment * N
dim_state_err = dim_main_err + dim_augment_err * N
maha_test_kinds = LocKalman.maha_test_kinds
name = f"{LocKalman.name}_{N}"
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
#cb = state[States.CLOCK_BIAS, :][0, 0]
#cd = state[States.CLOCK_DRIFT, :][0, 0]
cb, cd = state[13:15, :]
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
#odo_scale = state[States.ODO_SCALE, :][0,0]
odo_scale = state[18, :]
acceleration = state[States.ACCELERATION, :]
#focal_scale = state[States.FOCAL_SCALE, :][0,0]
focal_scale = state[22, :]
imu_angles = state[States.IMU_OFFSET, :]
glonass_bias, glonass_freq_slope = state[26:28, :]
ca = state[28, 0]
#glonass_bias = state[States.GLONASS_BIAS, :][0,0]
#glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :][0,0]
#ca = state[States.CLOCK_ACCELERATION, :][0,0]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix(
[[0, -vroll, -vpitch, -vyaw], [vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll], [vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
state_dot[13, 0] = cd
state_dot[14, 0] = ca
#state_dot[States.CLOCK_BIAS, 0][0,0] = cd
state_dot[States.CLOCK_DRIFT, 0][0, 0] = ca
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
#cd_err = state_err[States.CLOCK_DRIFT_ERR, :][0,:]
cd_err = state_err[13, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
ca_err = state_err[27, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[
States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (
acceleration + acceleration_err)
#state_err_dot[States.CLOCK_BIAS_ERR, :][0,:] = cd_err
#state_err_dot[States.CLOCK_DRIFT_ERR, :][0,:] = ca_err
state_err_dot[12, :][0, :] = cd_err
state_err_dot[13, :][0, :] = ca_err
f_err_sym = state_err + dt * state_err_dot
# convenient indexing
# q idxs are for quats and p idxs are for other
q_idxs = [[3, dim_augment]] + [[
dim_main + n * dim_augment + 3, dim_main + (n + 1) * dim_augment
] for n in range(N)]
q_err_idxs = [[3, dim_augment_err]] + [[
dim_main_err + n * dim_augment_err + 3, dim_main_err +
(n + 1) * dim_augment_err
] for n in range(N)]
p_idxs = [[0, 3]] + [[dim_augment, dim_main]] + [[
dim_main + n * dim_augment, dim_main + n * dim_augment + 3
] for n in range(N)]
p_err_idxs = [[0, 3]] + [[dim_augment_err, dim_main_err]] + [[
dim_main_err + n * dim_augment_err,
dim_main_err + n * dim_augment_err + 3
] for n in range(N)]
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
H_mod_sym[p_idx[0]:p_idx[1],
p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1] - p_idx[0])
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
H_mod_sym[q_idx[0]:q_idx[1],
q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r(
state[q_idx[0]:q_idx[1]])[:, 1:]
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:, :] = sp.Matrix(
0.5 * delta_x[q_err_idx[0]:q_err_idx[1], :])
err_function_sym[q_idx[0]:q_idx[1], 0] = quat_matrix_r(
nom_x[q_idx[0]:q_idx[1], 0]) * delta_quat
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
err_function_sym[p_idx[0]:p_idx[1], :] = sp.Matrix(
nom_x[p_idx[0]:p_idx[1], :] +
delta_x[p_err_idx[0]:p_err_idx[1], :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
inv_err_function_sym[p_err_idx[0]:p_err_idx[1],
0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1], 0] +
true_x[p_idx[0]:p_idx[1], 0])
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
delta_quat = quat_matrix_r(
nom_x[q_idx[0]:q_idx[1], 0]).T * true_x[q_idx[0]:q_idx[1], 0]
inv_err_function_sym[q_err_idx[0]:q_err_idx[1],
0] = sp.Matrix(2 * delta_quat[1:])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x], H_mod_sym,
f_err_sym, state_err_sym]
#
# Observation functions
#
# extra args
sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1)
sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1)
sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1)
orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1)
# expand extra args
sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym
sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:]
los_x, los_y, los_z = sat_los_sym
orb_x, orb_y, orb_z = orb_epos_sym
h_pseudorange_sym = sp.Matrix(
[sp.sqrt((x - sat_x)**2 + (y - sat_y)**2 + (z - sat_z)**2) + cb])
h_pseudorange_glonass_sym = sp.Matrix([
sp.sqrt((x - sat_x)**2 + (y - sat_y)**2 + (z - sat_z)**2) + cb +
glonass_bias + glonass_freq_slope * glonass_freq
])
los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z]))
los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 +
los_vector[2]**2)
h_pseudorange_rate_sym = sp.Matrix([
los_vector[0] * (sat_vx - vx) + los_vector[1] * (sat_vy - vy) +
los_vector[2] * (sat_vz - vz) + cd
])
imu_rot = euler_rotate(*imu_angles)
h_gyro_sym = imu_rot * 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_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
speed = sp.sqrt(vx**2 + vy**2 + vz**2)
h_speed_sym = sp.Matrix([speed * odo_scale])
# orb stuff
orb_pos_sym = sp.Matrix([orb_x - x, orb_y - y, orb_z - z])
orb_pos_rot_sym = quat_rot.T * orb_pos_sym
s = orb_pos_rot_sym[0]
h_orb_point_sym = sp.Matrix([(1 / s) * (orb_pos_rot_sym[1]),
(1 / s) * (orb_pos_rot_sym[2])])
h_pos_sym = sp.Matrix([x, y, z])
h_imu_frame_sym = sp.Matrix(imu_angles)
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [
[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[
h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS,
sat_pos_freq_sym
],
[
h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS,
sat_pos_freq_sym
],
[
h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS,
sat_pos_vel_sym
],
[
h_pseudorange_rate_sym,
ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym
], [h_pos_sym, ObservationKind.ECEF_POS, 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],
[h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym]
]
# MSCKF configuration
if N > 0:
focal_scale = 1
# Add observation functions for orb feature tracks
track_epos_sym = sp.MatrixSymbol('track_epos_sym', 3, 1)
track_x, track_y, track_z = track_epos_sym
h_track_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1)))
track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
track_pos_rot_sym = quat_rot.T * track_pos_sym
h_track_sym[-2:, :] = sp.Matrix([
focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]),
focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])
])
h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N) * 3, 1)))
h_msckf_test_sym[-3:, :] = sp.Matrix(
[track_x - x, track_y - y, track_z - z])
for n in range(N):
idx = dim_main + n * dim_augment
# err_idx = dim_main_err + n * dim_augment_err # FIXME: Why is this not used?
x, y, z = state[idx:idx + 3]
q = state[idx + 3:idx + 7]
quat_rot = quat_rotate(*q)
track_pos_sym = sp.Matrix(
[track_x - x, track_y - y, track_z - z])
track_pos_rot_sym = quat_rot.T * track_pos_sym
h_track_sym[n * 2:n * 2 + 2, :] = sp.Matrix([
focal_scale *
(track_pos_rot_sym[1] / track_pos_rot_sym[0]),
focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])
])
h_msckf_test_sym[n * 3:n * 3 + 3, :] = sp.Matrix(
[track_x - x, track_y - y, track_z - z])
obs_eqs.append(
[h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym])
obs_eqs.append(
[h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym])
obs_eqs.append([
h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym
])
msckf_params = [
dim_main, dim_augment, dim_main_err, dim_augment_err, N,
[ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES]
]
else:
msckf_params = None
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err,
eskf_params, msckf_params, maha_test_kinds)
def __call__(self, equations, variables=None, method_options=None):
method_options = extract_method_options(method_options,
{'simplify': True})
if equations.is_stochastic:
raise UnsupportedEquationsException('Cannot solve stochastic '
'equations with this state '
'updater.')
if variables is None:
variables = {}
# Get a representation of the ODE system in the form of
# dX/dt = M*X + B
varnames, matrix, constants = get_linear_system(equations, variables)
# No differential equations, nothing to do (this occurs sometimes in the
# test suite where the whole model is nothing more than something like
# 'v : 1')
if matrix.shape == (0, 0):
return ''
# Make sure that the matrix M is constant, i.e. it only contains
# external variables or constant variables
t = Symbol('t', real=True, positive=True)
# Check for time dependence
dt_value = variables['dt'].get_value(
)[0] if 'dt' in variables else None
# This will raise an error if we meet the symbol "t" anywhere
# except as an argument of a locally constant function
for entry in itertools.chain(matrix, constants):
if not is_constant_over_dt(entry, variables, dt_value):
raise UnsupportedEquationsException(
('Expression "{}" is not guaranteed to be constant over a '
'time step').format(sympy_to_str(entry)))
symbols = [Symbol(variable, real=True) for variable in varnames]
solution = sp.solve_linear_system(matrix.row_join(constants), *symbols)
if solution is None or set(symbols) != set(solution.keys()):
raise UnsupportedEquationsException('Cannot solve the given '
'equations with this '
'stateupdater.')
b = sp.ImmutableMatrix([solution[symbol] for symbol in symbols])
# Solve the system
dt = Symbol('dt', real=True, positive=True)
try:
A = (matrix * dt).exp()
except NotImplementedError:
raise UnsupportedEquationsException('Cannot solve the given '
'equations with this '
'stateupdater.')
if method_options['simplify']:
A = A.applyfunc(
lambda x: sp.factor_terms(sp.cancel(sp.signsimp(x))))
C = sp.ImmutableMatrix(A * b) - b
_S = sp.MatrixSymbol('_S', len(varnames), 1)
updates = A * _S + C
updates = updates.as_explicit()
# The solution contains _S[0, 0], _S[1, 0] etc. for the state variables,
# replace them with the state variable names
abstract_code = []
for idx, (variable, update) in enumerate(zip(varnames, updates)):
rhs = update
if rhs.has(I, re, im):
raise UnsupportedEquationsException(
'The solution to the linear system '
'contains complex values '
'which is currently not implemented.')
for row_idx, varname in enumerate(varnames):
rhs = rhs.subs(_S[row_idx, 0], varname)
# Do not overwrite the real state variables yet, the update step
# of other state variables might still need the original values
abstract_code.append('_' + variable + ' = ' + sympy_to_str(rhs))
# Update the state variables
for variable in varnames:
abstract_code.append(
'{variable} = _{variable}'.format(variable=variable))
return '\n'.join(abstract_code)
def gen_model(name, dim_state, maha_test_kinds):
# check if rebuild is needed
try:
dir_path = os.path.dirname(__file__)
deps = [dir_path + '/' + 'ekf_c.c',
dir_path + '/' + 'ekf_sym.py',
dir_path + '/' + 'gnss_model.py',
dir_path + '/' + 'gnss_kf.py']
outs = [dir_path + '/' + name + '.o',
dir_path + '/' + name + '.so',
dir_path + '/' + name + '.cpp']
out_times = list(map(os.path.getmtime, outs))
dep_times = list(map(os.path.getmtime, deps))
rebuild = os.getenv("REBUILD", False)
if min(out_times) > max(dep_times) and not rebuild:
return
list(map(os.remove, outs))
except OSError as e:
pass
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x,y,z = state[0:3,:]
v = state[3:6,:]
vx, vy, vz = v
cb, cd, ca = state[6:9,:]
glonass_bias, glonass_freq_slope = state[9:11,:]
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[:3,:] = v
state_dot[6,0] = cd
state_dot[7,0] = ca
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt*state_dot
#
# Observation functions
#
# extra args
sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1)
sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1)
sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1)
orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1)
# expand extra args
sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym
sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:]
los_x, los_y, los_z = sat_los_sym
orb_x, orb_y, orb_z = orb_epos_sym
h_pseudorange_sym = sp.Matrix([sp.sqrt(
(x - sat_x)**2 +
(y - sat_y)**2 +
(z - sat_z)**2) +
cb])
h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt(
(x - sat_x)**2 +
(y - sat_y)**2 +
(z - sat_z)**2) +
cb + glonass_bias + glonass_freq_slope*glonass_freq])
los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z]))
los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2)
h_pseudorange_rate_sym = sp.Matrix([los_vector[0]*(sat_vx - vx) +
los_vector[1]*(sat_vy - vy) +
los_vector[2]*(sat_vz - vz) +
cd])
obs_eqs = [[h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym],
[h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym],
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym],
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym]]
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds)
print(Numbering)
T = sympy.Matrix(numpy.zeros((6, 6)))
for i in range(3):
for j in range(3):
# if i==j:
# alpha = j
# else:
# alpha = 9 - i - j
alpha = Numbering[i, j]
for p in range(3):
for q in range(3):
beta = Numbering[p, q]
if alpha < 3 and beta < 3:
T[alpha, beta] = a[i, p] * a[i, p]
if alpha >= 3 and beta < 3:
T[alpha, beta] = a[i, p] * a[j, p]
if alpha < 3 and beta >= 3:
T[alpha, beta] = a[i, q] * a[i, p] + a[i, p] * a[i, q]
if alpha >= 3 and beta >= 3:
T[alpha, beta] = a[i, p] * a[j, q] + a[i, q] * a[j, p]
print(T)
R = sympy.eye(6) #Reuter matrix
R[3, 3] = R[4, 4] = R[5, 5] = 2
Tbar = R * T * R**-1
print(Tbar)
v = sympy.MatrixSymbol('v', 6, 1).as_explicit()
print(Tbar * v)
def test_MatAdd():
X = sympy.MatrixSymbol('X', 4, 4)
Y = sympy.MatrixSymbol('X', 4, 4)
Z = sympy.MatrixSymbol('X', 4, 4)
expr = X+Y+Z
assert isinstance(theano_code(expr).owner.op, tt.Elemwise)
P = np.genfromtxt('csv/mdSpace.csv', delimiter=",")
n = len(P)
L = np.genfromtxt('csv/eigVals.csv', delimiter=",")
L_pos = np.array([L[i] if L[i] > 0 else 0 for i in range(n)])
d = np.count_nonzero(L_pos) # d ... the number of positive values
Ln = np.sqrt(L_pos)
f2 = np.array(Ln[0:d])
f2[::2] = 0
f1 = Ln[0:d] - f2
e1 = f1 / np.linalg.norm(f1)
e2 = f2 / np.linalg.norm(f2)
a1, b1, c1, a2, b2, c2, t, s = sp.symbols('a1 b1 c1 a2 b2 c2 t s') # variables
x2_s, y2_s = sp.symbols('x2_s y2_s') # values
P_i = sp.MatrixSymbol('P_i', d, 1)
E1 = sp.MatrixSymbol('E1', d, 1)
E2 = sp.MatrixSymbol('E2', d, 1)
var = (x2_s, y2_s, P_i, E1, E2, a1, b1, c1, a2, b2, c2, t, s)
_E1 = a1 * sp.Matrix(E1) + b1 * sp.Matrix(E2) + c1 * sp.Matrix(P_i)
_E2 = a2 * sp.Matrix(E1) + b2 * sp.Matrix(E2) + c2 * sp.Matrix(P_i)
R = s * sp.Matrix(E1) + t * sp.Matrix(E2)
f = Matrix([
_E1.dot(_E1) - 1,
_E2.dot(_E2) - 1,
_E1.dot(_E2),
R.dot(R) - 1,
_E1.dot(R) - sp.Matrix(E1).dot(R),
_E2.dot(R) - sp.Matrix(E2).dot(R),
def test_MatrixSymbol():
X = sympy.MatrixSymbol('X', 4, 5)
Xt = theano_code(X)
assert isinstance(Xt, tt.TensorVariable)
assert Xt.broadcastable == (False, False)
def generate_code(generated_dir, N=4):
dim_augment = LocKalman.dim_augment
dim_augment_err = LocKalman.dim_augment_err
dim_main = LocKalman.x_initial.shape[0]
dim_main_err = LocKalman.P_initial.shape[0]
dim_state = dim_main + dim_augment * N
dim_state_err = dim_main_err + dim_augment_err * N
maha_test_kinds = LocKalman.maha_test_kinds
name = f"{LocKalman.name}_{N}"
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
cb = state[States.CLOCK_BIAS, :]
cd = state[States.CLOCK_DRIFT, :]
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
imu_from_device_euler = state[States.IMU_FROM_DEVICE_EULER, :]
imu_from_device_euler[0, 0] = 0 # not observable enough
imu_from_device_euler[2, 0] = 0 # not observable enough
glonass_bias = state[States.GLONASS_BIAS, :]
glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :]
ca = state[States.CLOCK_ACCELERATION, :]
accel_bias = state[States.ACCELEROMETER_BIAS, :]
wide_from_device_euler = state[States.WIDE_FROM_DEVICE_EULER, :]
wide_from_device_euler[0, 0] = 0 # not observable enough
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix(
[[0, -vroll, -vpitch, -vyaw], [vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll], [vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
state_dot[States.CLOCK_BIAS, :] = cd
state_dot[States.CLOCK_DRIFT, :] = ca
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
cd_err = state_err[States.CLOCK_DRIFT_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
ca_err = state_err[States.CLOCK_ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[
States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (
acceleration + acceleration_err)
state_err_dot[States.CLOCK_BIAS_ERR, :] = cd_err
state_err_dot[States.CLOCK_DRIFT_ERR, :] = ca_err
f_err_sym = state_err + dt * state_err_dot
# convenient indexing
# q idxs are for quats and p idxs are for other
q_idxs = [[3, dim_augment]] + [[
dim_main + n * dim_augment + 3, dim_main + (n + 1) * dim_augment
] for n in range(N)]
q_err_idxs = [[3, dim_augment_err]] + [[
dim_main_err + n * dim_augment_err + 3, dim_main_err +
(n + 1) * dim_augment_err
] for n in range(N)]
p_idxs = [[0, 3]] + [[dim_augment, dim_main]] + [[
dim_main + n * dim_augment, dim_main + n * dim_augment + 3
] for n in range(N)]
p_err_idxs = [[0, 3]] + [[dim_augment_err, dim_main_err]] + [[
dim_main_err + n * dim_augment_err,
dim_main_err + n * dim_augment_err + 3
] for n in range(N)]
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
H_mod_sym[p_idx[0]:p_idx[1],
p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1] - p_idx[0])
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
H_mod_sym[q_idx[0]:q_idx[1],
q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r(
state[q_idx[0]:q_idx[1]])[:, 1:]
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:, :] = sp.Matrix(
0.5 * delta_x[q_err_idx[0]:q_err_idx[1], :])
err_function_sym[q_idx[0]:q_idx[1], 0] = quat_matrix_r(
nom_x[q_idx[0]:q_idx[1], 0]) * delta_quat
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
err_function_sym[p_idx[0]:p_idx[1], :] = sp.Matrix(
nom_x[p_idx[0]:p_idx[1], :] +
delta_x[p_err_idx[0]:p_err_idx[1], :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
inv_err_function_sym[p_err_idx[0]:p_err_idx[1],
0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1], 0] +
true_x[p_idx[0]:p_idx[1], 0])
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
delta_quat = quat_matrix_r(
nom_x[q_idx[0]:q_idx[1], 0]).T * true_x[q_idx[0]:q_idx[1], 0]
inv_err_function_sym[q_err_idx[0]:q_err_idx[1],
0] = sp.Matrix(2 * delta_quat[1:])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x], H_mod_sym,
f_err_sym, state_err_sym]
#
# Observation functions
#
# extra args
sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1)
sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1)
# sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1)
# expand extra args
sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym
sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:]
h_pseudorange_sym = sp.Matrix([
sp.sqrt((x - sat_x)**2 + (y - sat_y)**2 + (z - sat_z)**2) + cb[0]
])
h_pseudorange_glonass_sym = sp.Matrix([
sp.sqrt((x - sat_x)**2 + (y - sat_y)**2 + (z - sat_z)**2) + cb[0] +
glonass_bias[0] + glonass_freq_slope[0] * glonass_freq
])
los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z]))
los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 +
los_vector[2]**2)
h_pseudorange_rate_sym = sp.Matrix([
los_vector[0] * (sat_vx - vx) + los_vector[1] * (sat_vy - vy) +
los_vector[2] * (sat_vz - vz) + cd[0]
])
imu_from_device = euler_rotate(*imu_from_device_euler)
h_gyro_sym = imu_from_device * sp.Matrix(
[vroll + roll_bias, vpitch + pitch_bias, vyaw + yaw_bias])
pos = sp.Matrix([x, y, z])
# add 1 for stability, prevent division by 0
gravity = quat_rot.T * ((EARTH_GM / (
(x**2 + y**2 + z**2 + 1)**(3.0 / 2.0))) * pos)
h_acc_sym = imu_from_device * (gravity + acceleration + accel_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[
h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS,
sat_pos_freq_sym
],
[
h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS,
sat_pos_freq_sym
],
[
h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS,
sat_pos_vel_sym
],
[
h_pseudorange_rate_sym,
ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym
],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]
]
wide_from_device = euler_rotate(*wide_from_device_euler)
# MSCKF configuration
if N > 0:
# experimentally found this is correct value for imx298 with 910 focal length
# this is a variable so it can change with focus, but we disregard that for now
# TODO: this isn't correct for tici
focal_scale = 1.01
# Add observation functions for orb feature tracks
track_epos_sym = sp.MatrixSymbol('track_epos_sym', 3, 1)
track_x, track_y, track_z = track_epos_sym
h_track_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1)))
h_track_wide_cam_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1)))
track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
track_pos_rot_sym = quat_rot.T * track_pos_sym
track_pos_rot_wide_cam_sym = wide_from_device * track_pos_rot_sym
h_track_sym[-2:, :] = sp.Matrix([
focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]),
focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])
])
h_track_wide_cam_sym[-2:, :] = sp.Matrix([
focal_scale * (track_pos_rot_wide_cam_sym[1] /
track_pos_rot_wide_cam_sym[0]), focal_scale *
(track_pos_rot_wide_cam_sym[2] / track_pos_rot_wide_cam_sym[0])
])
h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N) * 3, 1)))
h_msckf_test_sym[-3:, :] = track_pos_sym
for n in range(N):
idx = dim_main + n * dim_augment
# err_idx = dim_main_err + n * dim_augment_err # FIXME: Why is this not used?
x, y, z = state[idx:idx + 3]
q = state[idx + 3:idx + 7]
quat_rot = quat_rotate(*q)
track_pos_sym = sp.Matrix(
[track_x - x, track_y - y, track_z - z])
track_pos_rot_sym = quat_rot.T * track_pos_sym
track_pos_rot_wide_cam_sym = wide_from_device * track_pos_rot_sym
h_track_sym[n * 2:n * 2 + 2, :] = sp.Matrix([
focal_scale *
(track_pos_rot_sym[1] / track_pos_rot_sym[0]),
focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])
])
h_track_wide_cam_sym[n * 2:n * 2 + 2, :] = sp.Matrix([
focal_scale * (track_pos_rot_wide_cam_sym[1] /
track_pos_rot_wide_cam_sym[0]),
focal_scale * (track_pos_rot_wide_cam_sym[2] /
track_pos_rot_wide_cam_sym[0])
])
h_msckf_test_sym[n * 3:n * 3 + 3, :] = track_pos_sym
obs_eqs.append(
[h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym])
obs_eqs.append(
[h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym])
obs_eqs.append([
h_track_wide_cam_sym, ObservationKind.ORB_FEATURES_WIDE,
track_epos_sym
])
obs_eqs.append([
h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym
])
msckf_params = [
dim_main, dim_augment, dim_main_err, dim_augment_err, N,
[
ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES,
ObservationKind.ORB_FEATURES_WIDE
]
]
else:
msckf_params = None
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state,
dim_state_err, eskf_params, msckf_params, maha_test_kinds)
def test_MatMul():
X = sympy.MatrixSymbol('X', 4, 4)
Y = sympy.MatrixSymbol('X', 4, 4)
Z = sympy.MatrixSymbol('X', 4, 4)
expr = X*Y*Z
assert isinstance(theano_code(expr).owner.op, tt.Dot)
def gen_model(name, dim_state, dim_state_err, maha_test_kinds):
# check if rebuild is needed
try:
dir_path = os.path.dirname(__file__)
deps = [
dir_path + '/' + 'ekf_c.c', dir_path + '/' + 'ekf_sym.py',
dir_path + '/' + name + '_model.py',
dir_path + '/' + name + '_kf.py'
]
outs = [
dir_path + '/' + name + '.o',
dir_path + '/' + name + sysconfig.get_config_var('EXT_SUFFIX'),
dir_path + '/' + name + '.cpp'
]
out_times = list(map(os.path.getmtime, outs))
dep_times = list(map(os.path.getmtime, deps))
rebuild = os.getenv("REBUILD", False)
if min(out_times) > max(dep_times) and not rebuild:
return
list(map(os.remove, outs))
except OSError as e:
print('HAHAHA')
print(e)
pass
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.GYRO_BIAS, :]
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
odo_scale = state[16, :]
acceleration = state[States.ACCELERATION, :]
imu_angles = state[States.IMU_OFFSET, :]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw], [
vroll, 0, vyaw, -vpitch
], [vpitch, -vyaw, 0, vroll], [vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (
acceleration + acceleration_err)
f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
H_mod_sym[0:3, 0:3] = np.eye(3)
H_mod_sym[3:7, 3:6] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones((4)))
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[3:6, :])
err_function_sym[3:7, 0] = quat_matrix_r(nom_x[3:7, 0]) * delta_quat
err_function_sym[0:3, :] = sp.Matrix(nom_x[0:3, :] + delta_x[0:3, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[0:3, 0] = sp.Matrix(-nom_x[0:3, 0] + true_x[0:3, 0])
delta_quat = quat_matrix_r(nom_x[3:7, 0]).T * true_x[3:7, 0]
inv_err_function_sym[3:6, 0] = sp.Matrix(2 * delta_quat[1:])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x], H_mod_sym, f_err_sym,
state_err_sym]
#
# Observation functions
#
imu_rot = euler_rotate(*imu_angles)
h_gyro_sym = imu_rot * 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_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
speed = vx**2 + vy**2 + vz**2
h_speed_sym = sp.Matrix([sp.sqrt(speed) * odo_scale])
h_pos_sym = sp.Matrix([x, y, z])
h_imu_frame_sym = sp.Matrix(imu_angles)
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, 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)
def test_Transpose():
X = sympy.MatrixSymbol('X', 4, 4)
assert isinstance(theano_code(X.T).owner.op, tt.DimShuffle)
def generate_code():
dim_state = CarKalman.x_initial.shape[0]
name = CarKalman.name
maha_test_kinds = CarKalman.maha_test_kinds
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
# TODO: Read from car params at runtime
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.toyota.interface import CarInterface
from selfdrive.car.toyota.values import CAR
CP = CarInterface.get_params(CAR.COROLLA_TSS2)
VM = VehicleModel(CP)
m = VM.m
j = VM.j
aF = VM.aF
aR = VM.aR
x = state[States.STIFFNESS, :][0, 0]
cF, cR = x * VM.cF, x * VM.cR
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast)
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[
sp.Matrix([angle_offset_fast]),
ObservationKind.ANGLE_OFFSET_FAST, None
],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[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)
def generate_code():
dim_state = GNSSKalman.x_initial.shape[0]
name = GNSSKalman.name
maha_test_kinds = GNSSKalman.maha_test_kinds
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[0:3, :]
v = state[3:6, :]
vx, vy, vz = v
cb, cd, ca = state[6:9, :]
glonass_bias, glonass_freq_slope = state[9:11, :]
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[:3, :] = v
state_dot[6, 0] = cd
state_dot[7, 0] = ca
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
# extra args
sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1)
sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1)
sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1)
orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1)
# expand extra args
sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym
sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:]
los_x, los_y, los_z = sat_los_sym
orb_x, orb_y, orb_z = orb_epos_sym
h_pseudorange_sym = sp.Matrix(
[sp.sqrt((x - sat_x)**2 + (y - sat_y)**2 + (z - sat_z)**2) + cb])
h_pseudorange_glonass_sym = sp.Matrix([
sp.sqrt((x - sat_x)**2 + (y - sat_y)**2 + (z - sat_z)**2) + cb +
glonass_bias + glonass_freq_slope * glonass_freq
])
los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z]))
los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 +
los_vector[2]**2)
h_pseudorange_rate_sym = sp.Matrix([
los_vector[0] * (sat_vx - vx) + los_vector[1] * (sat_vy - vy) +
los_vector[2] * (sat_vz - vz) + cd
])
obs_eqs = [[
h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS,
sat_pos_freq_sym
],
[
h_pseudorange_glonass_sym,
ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym
],
[
h_pseudorange_rate_sym,
ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym
],
[
h_pseudorange_rate_sym,
ObservationKind.PSEUDORANGE_RATE_GLONASS,
sat_pos_vel_sym
]]
gen_code(name,
f_sym,
dt,
state_sym,
obs_eqs,
dim_state,
dim_state,
maha_test_kinds=maha_test_kinds)
def generate_code(order,
name,
precision='double',
cython=False,
CSE=False,
harmonic_derivs=False,
include_dir=None,
src_dir=None,
potential=True,
field=True,
source_order=0,
atomic=False,
gpu=False,
minpow=0,
language='c',
save_opscounts=None):
"""
Inputs:
order, int:
Expansion order of the FMM operators.
name, str:
Name of generated files.
Note: Cython compiles *.pyx files to the *.c files with the same name,
which means that we have to use a different name for the cython file.
We therefore append "wrap" to the end of the name for the cython file,
and this is therefore the name of the Python module which must be
imported if using pyximport.
cython_wrapper, bool:
Enable generation of a Cython wrapper for the C files.
CSE, bool:
Enable common subexpression elimination, to reduce the op count in
the generated code.
harmonic_derivs, bool:
The harmonicity of the Laplace means that at order p, there are only
2p - 1 independent derivatives. Enabling this option therefore computes
some derivatives as combinations of others.
source_order, int:
If source_order > 0 then we set certain multipole terms to zero
in the local expansion, and hence they are not used. This is useful if,
for example, we only have pure dipoles or quadrupoles in the system.
minpow, int:
If minpow is set, pow(x, n) expressions are expanded such that
if n < minpow, the expression in code is printed as multiplications.
e.g. if a sympy expression is pow(x, 2) + pow(y, 6) and minpow is 5,
the printed version will be x*x + pow(y, 6)
save_opcounts, string:
Filename to save opcounts in
"""
if save_opscounts:
f = open(save_opscounts, 'w')
assert language in ['c', 'c++'], "Language must be 'c' or 'c++'"
if language == 'c':
fext = 'c'
hext = 'h'
if language == 'c++':
fext = 'cpp'
hext = 'h'
logger.info(f"Generating FMM operators to order {order}")
assert precision in ['double',
'float'], "Precision must be float or double"
logger.info(f"Precision = {precision}")
if CSE:
logger.info(f"CSE Enabled")
p = FunctionPrinter(precision=precision, debug=False, minpow=minpow)
else:
logger.info(f"CSE Disabled")
p = FunctionPrinter(precision=precision, debug=True, minpow=minpow)
header = ""
body = ""
x, y, z, q = sp.symbols('x y z q')
symbols = (x, y, z)
coords = [x, y, z]
start = source_order
if field:
# No point starting at source_order
# because no field calculation can be done
# at this multipole order - the L2P derivative
# is 0. This is true whether or not we
# calculate potential, so we always increment
# by 1 here the start order, such that the
# field is always calculated.
#
# It may be preferable to instead compute
# the field using a finite-difference approximation,
# or to enable this as a general option.
# However, this would be fairly sensitive, would
# require another user parameter (lengths over which
# to take the f.d. approximation), a decision on
# whether to use forward/backward/central differencing,
# and the order of the approximation. For now, we
# leave it as a simple symbolic derivative.
start += 1
for i in range(start, order):
print(f"Generating Order {i} operators")
M_dict, _ = generate_mappings(i,
symbols,
'grevlex',
source_order=source_order)
L_dict, _ = generate_mappings(i - source_order,
symbols,
'grevlex',
source_order=0)
M = sp.Matrix(generate_M_operators(i, symbols, M_dict))
head, code, P2M_opscount = p.generate(f'P2M_{i}',
'M',
M,
coords + [q],
operator='+=')
print(f"P2M_{i} opscount = {P2M_opscount}")
header += head
body += code
Ms = sp.Matrix(
generate_M_shift_operators(i,
symbols,
M_dict,
source_order=source_order))
head, code, M2M_opscount = p.generate(f'M2M_{i}', 'Ms', Ms,
list(symbols) + \
[sp.MatrixSymbol('M', Nterms(i), 1)],
operator="+=", atomic=atomic)
header += head
body += code + '\n'
print(f"M2M_{i} opscount = {M2M_opscount}")
# Two stages here; generate derivs and then the L matrix. Both
# must be passed to the function printer.
derivs = sp.Matrix(
generate_derivs(i,
symbols,
M_dict,
source_order,
harmonic_derivs=harmonic_derivs))
L = sp.Matrix(
generate_L_operators(i,
symbols,
M_dict,
L_dict,
source_order=source_order))
head, code, M2L_opscount = p.generate(f'M2L_{i}', 'L', L,
list(symbols) + \
[sp.MatrixSymbol('M', Nterms(i), 1)],
operator="+=", atomic=atomic, internal=[('D', derivs)])
header += head
body += code + '\n'
print(f"M2L_{i} opscount = {M2L_opscount}")
Ls = sp.Matrix(
generate_L_shift_operators(i,
symbols,
L_dict,
source_order=source_order))
head, code, L2L_opscount = p.generate(f'L2L_{i}', 'Ls', Ls,
list(symbols) + \
[sp.MatrixSymbol('L', Nterms(i), 1)],
operator="+=", atomic=atomic)
header += head
body += code + '\n'
print(f"L2L_{i} opscount = {L2L_opscount}")
L2P = generate_L2P_operators(i,
symbols,
L_dict,
potential=potential,
field=field)
Fs = sp.Matrix(L2P)
head, code, L2P_opscount = p.generate(f'L2P_{i}', 'F', Fs,
list(symbols) + \
[sp.MatrixSymbol('L', Nterms(i), 1)],
operator="+=", atomic=atomic)
header += head
body += code + '\n'
print(f"L2P_{i} opscount = {L2P_opscount}")
M2P = generate_M2P_operators(i,
symbols,
M_dict,
potential=potential,
field=field,
source_order=source_order,
harmonic_derivs=harmonic_derivs)
Fs = sp.Matrix(M2P)
head, code, M2P_opscount = p.generate(f'M2P_{i}', 'F', Fs,
list(symbols) + \
[sp.MatrixSymbol('M', Nterms(i), 1)],
operator="+=", atomic=atomic)
header += head
body += code + '\n'
print(f"M2P_{i} opscount = {M2P_opscount}")
if i == start:
P2P = sp.Matrix(
generate_P2P_operators(symbols,
M_dict,
potential=potential,
field=field,
source_order=source_order))
head, code, P2P_opscount = p.generate(f'P2P', 'F', P2P,
list(symbols) + \
[sp.MatrixSymbol('S', Nterms(i), 1)],
operator="+=", atomic=atomic
)
print(f"P2P opscount = {P2P_opscount}")
header += head
body += code + '\n'
if save_opscounts:
if i == start:
f.write(f'P2P,{P2P_opscount}\n')
f.write(f'P2M_{i},{P2M_opscount}\n')
f.write(f'M2M_{i},{M2M_opscount}\n')
f.write(f'M2L_{i},{M2L_opscount}\n')
f.write(f'L2P_{i},{L2P_opscount}\n')
f.write(f'L2L_{i},{L2L_opscount}\n')
f.write(f'M2P_{i},{M2P_opscount}\n')
# We now generate wrapper functions that cover all orders generated.
unique_funcs = []
func_definitions = header.split(';\n')
# print(f"func_defs = {func_definitions}")
for func in func_definitions:
# Must do it this way in order to avoid breaking
# for expansions > 10.
function_name = func.split('(')[0]
# print(f"Function_name = {function_name}")
end_string = f'_{start}'
if end_string == function_name[-len(end_string):]:
# print("Unique!")
unique_funcs.append(func)
else:
pass
# print(f"{func} not unique")
# print(f" {end_string} {function_name[-len(end_string)-1:]}")
wrapper_funcs = [
f.replace(')', ', int order)').replace(f'_{start}', '')
for f in unique_funcs
]
# print(wrapper_funcs)
func_definitions += wrapper_funcs
# print('\n'.join(func_definitions))
for wfunc, func in zip(wrapper_funcs, unique_funcs):
# Add to header file
header += wfunc + ';\n'
# Create a switch statement that covers all functions:
code = wfunc + " {\n"
code += 'switch (order) {\n'
for i in range(start, order):
code += ' case {}:\n'.format(i)
# print(func)
replaced_code = func.replace(f'_{start}', f'_{i}').replace(
'* ', '').replace('double ',
'').replace('float ',
'').replace('void ', '')
# print(f"replaced_code: {replaced_code}")
code += ' ' + replaced_code + ';\n break;\n'
code += " }\n}\n"
# print(code)
body += code
if not include_dir:
f = open(f"{name}.{hext}", 'w')
else:
f = open(f"{include_dir.rstrip('/')}/{name}.{hext}", 'w')
f.write(f"#pragma once\n")
f.write(f"#define FMMGEN_MINORDER {start}\n")
f.write(f"#define FMMGEN_MAXORDER {order}\n")
f.write(f"#define FMMGEN_SOURCEORDER {source_order}\n")
f.write(
f"#define FMMGEN_SOURCESIZE {Nterms(source_order) - Nterms(source_order - 1)}\n"
)
if potential and not field:
osize = 1
elif field and not potential:
osize = 3
elif field and potential:
osize = 4
f.write(f"#define FMMGEN_OUTPUTSIZE {osize}\n")
f.write(header)
f.close()
if not src_dir:
f = open(f"{name}.{fext}", 'w')
else:
f = open(f"{src_dir.rstrip('/')}/{name}.{fext}", 'w')
f.write(f'#include "{name}.{hext}"\n')
if language == 'c':
f.write(f'#include "math.h"\n')
elif language == 'c++':
f.write(f'#include<cmath>\n')
f.write(body)
f.close()
if cython and gpu:
raise Warning("Cannot write a Cython wrapper for GPU code; skipping")
elif cython:
logger.info(f"Generating Cython wrapper: {name}_wrap.pyx")
library = f"{name}"
f = open(f"{name}_decl.pxd", "w")
pxdcode = textwrap.dedent("""\
cdef extern from "{}.h":
cdef int FMMGEN_MINORDER
cdef int FMMGEN_MAXORDER
cdef int FMMGEN_SOURCEORDER
cdef int FMMGEN_SOURCESIZE
cdef int FMMGEN_OUTPUTSIZE
{}
""")
f.write(pxdcode.format(name, '\n '.join(func_definitions)))
f.close()
f = open(f"{name}_wrap.pyx", "w")
# expose the C functions from the header file.
pyxcode = textwrap.dedent("""\
# cython: language_level=3
cimport numpy as np
cimport {}
FMMGEN_MINORDER = {}.FMMGEN_MINORDER
FMMGEN_MAXORDER = {}.FMMGEN_MAXORDER
FMMGEN_SOURCEORDER = {}.FMMGEN_SOURCEORDER
FMMGEN_SOURCESIZE = {}.FMMGEN_SOURCESIZE
FMMGEN_OUTPUTSIZE = {}.FMMGEN_OUTPUTSIZE
""").format(*[name + '_decl'] * 6)
subsdict = {" *": "[:]", "void": "cpdef", "_": ""}
# Generate the actual wrapper code
for funcname in func_definitions:
# print(funcname)
if not funcname:
continue
pyfuncname = funcname
for key, value in subsdict.items():
pyfuncname = pyfuncname.replace(key, value)
pyxcode += pyfuncname + ':\n'
function_name = funcname.split("(")[0].split(" ")[1]
args = funcname.split("(")[1][:-1].split(",")
processed_args = []
for arg in args:
if "*" in arg:
arg = arg.replace("* ", "&") + "[0]"
processed_args.append(arg.split(" ")[-1])
pyxcode += ' ' + \
name + '_decl.' + function_name + \
"(" + ', '.join(processed_args) + ')\n\n'
f.write(pyxcode)
f.close()
f = open(f"{name}_wrap.pyxbld", "w")
# print(library)
logger.info(f"Generating Cython buildfile: {name}_wrap.pyxbld")
bldcode = textwrap.dedent("""\
import numpy as np
def make_ext(modname, pyxfilename):
from distutils.extension import Extension
return Extension(name = modname,
sources=[pyxfilename, '{}'],
include_dirs=[np.get_include(), '.'],
library_dirs=['.'],
extra_link_args=[],
extra_compile_args=['-O3', '-fopenmp'])
""").format(library + '.c', library)
f.write(bldcode)
f.close()
def ode_dVdt_expr():
''' integrate the problem vector, which is defined as:
V = [x(14),
Phi_A(14x14), B_bar(14x3), C_bar(14x3), S_bar(14), z_bar(14)]
V has no correlation to v (velocity) except that it contains v inside
'''
import sympy
from old.dynamics_functions_sympy import Dynamics as Dynamics_sympy
matrix_functions = Dynamics_sympy()
matrix_functions.set_parameters({
'alpha': sympy.Symbol('alpha'),
'g_I': sympy.MatrixSymbol('g_I', 3, 1),
'rTB': sympy.MatrixSymbol('rTB', 3, 1),
'J': sympy.MatrixSymbol('J', 3, 3),
})
V = sympy.MatrixSymbol('V', idx[-1], 1)
t = sympy.Symbol('t')
u_t = sympy.MatrixSymbol('u_t', 3, 1)
u_t1 = sympy.MatrixSymbol('u_t1', 3, 1)
sigma = sympy.Symbol('sigma')
dt = sympy.Symbol('dt')
A, B, f = (matrix_functions.A, matrix_functions.B, matrix_functions.f)
alpha = (t / dt)
beta = (1 - t / dt)
# Gather x and u
x = V[0:idx[0]]
u = u_t + alpha * (u_t1 - u_t)
''' Since PhiA(t) * A(t) = Phi(t_end), PhiA(t) = Phi(t_end) * A.inverse '''
#Phi = sympy.Matrix(V[idx[0] : idx[1]]).reshape(14, 14)
Phi = sympy.MatrixSymbol('Phi', 14, 14)
#Phi_A_xi = Phi.inv()
Phi_A_xi = sympy.MatrixSymbol('Phi_A_xi', 14, 14)
A_xus = A(x, u, sigma)
B_xus = B(x, u, sigma)
f_xu = f(x, u)
z_t = -(A_xus * x)
z_t += -(B_xus * u)
#print((sigma * f_xu).shape)
#mul_flat = sympy.Matrix(Phi_A_xi* B_xus).reshape(14 * 3, 1)
mul_ab = Phi_A_xi * B_xus
res_x = sigma * f_xu
# res_phi = sympy.Matrix(A_xus* Phi)
# res_b = sympy.Matrix(mul_ab * alpha)
# res_c = sympy.Matrix(mul_ab * beta)
# res_s= sympy.Matrix(Phi_A_xi* f_xu)
# res_z = sympy.Matrix(Phi_A_xi* z_t)
res_phi = (A_xus * Phi)
res_b = (mul_ab * alpha)
res_c = (mul_ab * beta)
res_s = (Phi_A_xi * f_xu)
res_z = (Phi_A_xi * z_t)
#print(dV_dt)
return (res_x, res_phi, res_b, res_c, res_s, res_z)
tt = theano.tensor
xt, yt, zt = [tt.scalar(name, 'floatX') for name in 'xyz']
Xt, Yt, Zt = [tt.tensor('floatX', (False, False), name=n) for n in 'XYZ']
else:
#bin/test will not execute any tests now
disabled = True
import sympy as sy
from sympy import S
from sympy.abc import x, y, z, t
from sympy.printing.theanocode import (theano_code, dim_handling,
theano_function)
# Default set of matrix symbols for testing - make square so we can both
# multiply and perform elementwise operations between them.
X, Y, Z = [sy.MatrixSymbol(n, 4, 4) for n in 'XYZ']
# For testing AppliedUndef
f_t = sy.Function('f')(t)
def theano_code_(expr, **kwargs):
""" Wrapper for theano_code that uses a new, empty cache by default. """
kwargs.setdefault('cache', {})
with warns_deprecated_sympy():
return theano_code(expr, **kwargs)
def theano_function_(inputs, outputs, **kwargs):
""" Wrapper for theano_function that uses a new, empty cache by default. """
kwargs.setdefault('cache', {})
# %%
eq_zrv_0 = sym.Eq(sym.Integral(f_h(t)*sym.exp(z_n*w_n*t)*sym.cos(t*w_n*sym.sqrt(1-z_n**2)), t))
eq_zrv_1 = eq_zrv_0.replace(sym.cos, sym.sin)
display(eq_zrv_0)
display(eq_zrv_1)
# %%
T = sym.symbols("T")
k_0, k_1 = sym.symbols("k_0, k_1")
eq_convolve_ref_disc = sym.Eq(f_p_star_r(m*T),T*sym.Sum(f_h(k*t)*f_p_r((m-k)*T), (k,k_0, k_1)))
display(eq_convolve_ref_disc)
# %%
f_m = sym.Function(r"\mathbf{m}")
d = sym.MatrixSymbol(r"\mathbf{d}", 1, 6)
eq_constraints_disc = sym.Eq(sym.Sum(f_m(k)*f_h(k*T), (k, k_0, k_1)), d, evaluate=False)
display(eq_constraints_disc)
# %%
eq_m_k = sym.Eq(f_m(k), sym.Matrix([
1,
k*T,
k**2*T**2,
k**3*T**3,
sym.exp(z_n*w_n*k*T)*sym.cos(k*T*w_n*sym.sqrt(1-z_n**2)),
sym.exp(z_n*w_n*k*T)*sym.cos(k*T*t*w_n*sym.sqrt(1-z_n**2))]),
evaluate=False)
display(eq_m_k)
# %%
