diff --git a/.gitignore b/.gitignore
index b411c92..bf2b50d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -15,6 +15,7 @@ __pycache__/
 *$py.class
 
 # C extensions
+*.a
 *.o
 *.so
 
diff --git a/SConstruct b/SConstruct
index 012ae7e..c668d1b 100644
--- a/SConstruct
+++ b/SConstruct
@@ -36,7 +36,8 @@ env = Environment(
   CFLAGS="-std=gnu11",
   CXXFLAGS="-std=c++1z",
   CPPPATH=cpppath,
-  tools=["default", "cython"],
+  REDNOSE_ROOT=Dir("#").abspath,
+  tools=["default", "cython", "rednose_filter"],
 )
 
 # Cython build enviroment
@@ -52,17 +53,7 @@ elif arch == "aarch64":
 else:
   envCython["LINKFLAGS"] = ["-pthread", "-shared"]
 
-rednose_config = {
-  'generated_folder': '#examples/generated',
-  'to_build': {
-    'live': ('#examples/live_kf.py', True, [], []),
-    'kinematic': ('#examples/kinematic_kf.py', True, [], []),
-    'compare': ('#examples/test_compare.py', True, [], []),
-    'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, [], []),
-    'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, [], []),
-    'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, [], []),
-  },
-}
+Export('env', 'envCython', 'common')
 
-Export('env', 'envCython', 'arch', 'rednose_config', 'common')
 SConscript(['#rednose/SConscript'])
+SConscript(['#examples/SConscript'])
diff --git a/examples/SConscript b/examples/SConscript
new file mode 100644
index 0000000..3d178ad
--- /dev/null
+++ b/examples/SConscript
@@ -0,0 +1,19 @@
+Import('env')
+
+gen_dir = Dir('generated/').abspath
+
+env.RednoseCompileFilter(
+  target="live",
+  filter_gen_script="live_kf.py",
+  output_dir=gen_dir,
+)
+env.RednoseCompileFilter(
+  target="kinematic",
+  filter_gen_script="kinematic_kf.py",
+  output_dir=gen_dir,
+)
+env.RednoseCompileFilter(
+  target="compare",
+  filter_gen_script="test_compare.py",
+  output_dir=gen_dir,
+)
diff --git a/examples/test_kinematic_kf.py b/examples/test_kinematic_kf.py
index 2d6c5e5..9f7c908 100644
--- a/examples/test_kinematic_kf.py
+++ b/examples/test_kinematic_kf.py
@@ -38,11 +38,11 @@ class TestKinematic(unittest.TestCase):
 
       # Retrieve kf values
       state = kf.x
-      xs_kf.append(float(state[States.POSITION]))
-      vs_kf.append(float(state[States.VELOCITY]))
+      xs_kf.append(float(state[States.POSITION].item()))
+      vs_kf.append(float(state[States.VELOCITY].item()))
       std = np.sqrt(kf.P)
-      xs_kf_std.append(float(std[States.POSITION, States.POSITION]))
-      vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY]))
+      xs_kf_std.append(float(std[States.POSITION, States.POSITION].item()))
+      vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY].item()))
 
       # Update simulation
       x += v * dt
diff --git a/rednose/SConscript b/rednose/SConscript
index 398b4b4..52e36e0 100644
--- a/rednose/SConscript
+++ b/rednose/SConscript
@@ -1,45 +1,17 @@
-Import('env', 'envCython', 'arch', 'rednose_config', 'common')
+Import('env', 'envCython', 'common')
 
-generated_folder = rednose_config['generated_folder']
-
-templates = Glob('#rednose/templates/*')
-
-sympy_helpers = "#rednose/helpers/sympy_helpers.py"
-ekf_sym = "#rednose/helpers/ekf_sym.py"
-ekf_sym_pyx = "#rednose/helpers/ekf_sym_pyx.pyx"
-ekf_sym_cc = env.Object("#rednose/helpers/ekf_sym.cc")
-common_ekf = "#rednose/helpers/common_ekf.cc"
-
-found = {}
-for target, (command, *args) in rednose_config['to_build'].items():
-  if File(command).exists():
-    found[target] = (command, *args)
-
-lib_target = [common_ekf]
-for target, (command, combined_lib, extra_generated, deps) in found.items():
-  target_files = File([f'{generated_folder}/{target}.cpp', f'{generated_folder}/{target}.h'])
-  extra_generated = [File(f'{generated_folder}/{x}') for x in extra_generated]
-  command_file = File(command)
-
-  env.Command(target_files + extra_generated,
-              [templates, command_file, sympy_helpers, ekf_sym] + deps,
-              command_file.get_abspath() + " " + target + " " + Dir(generated_folder).get_abspath())
-
-  if combined_lib:
-    lib_target.append(target_files[0])
-  else:
-    env.SharedLibrary(f'{generated_folder}/' + target, [target_files[0], common_ekf])
-
-libkf = env.SharedLibrary(f'{generated_folder}/libkf', lib_target)
-
-lenv = envCython.Clone()
-lenv["LINKFLAGS"] += [libkf[0].get_labspath()]
-
-# for SWAGLOG support
+cc_sources = [
+  "helpers/ekf_load.cc",
+  "helpers/ekf_sym.cc",
+]
+libs = ["dl"]
 if common != "":
-  lenv["LIBS"] = ['zmq', common] + lenv["LIBS"]
+  # for SWAGLOG support
+  libs += [common, 'zmq']
 
-ekf_sym_so = lenv.Program('#rednose/helpers/ekf_sym_pyx.so', [ekf_sym_pyx, ekf_sym_cc, common_ekf])
-lenv.Depends(ekf_sym_so, libkf)
+ekf_objects = env.SharedObject(cc_sources)
+rednose = env.Library("helpers/ekf_sym", ekf_objects, LIBS=libs)
+rednose_python = envCython.Program("helpers/ekf_sym_pyx.so", ["helpers/ekf_sym_pyx.pyx", ekf_objects],
+                                   LIBS=libs + envCython["LIBS"])
 
-Export('libkf')
+Export('rednose', 'rednose_python')
diff --git a/rednose/helpers/__init__.py b/rednose/helpers/__init__.py
index 12cce34..b2beede 100644
--- a/rednose/helpers/__init__.py
+++ b/rednose/helpers/__init__.py
@@ -13,11 +13,9 @@ def write_code(folder, name, code, header):
   open(os.path.join(folder, f"{name}.h"), 'w', encoding='utf-8').write(header)
 
 
-def load_code(folder, name, lib_name=None):
-  if lib_name is None:
-    lib_name = name
+def load_code(folder, name):
   shared_ext = "dylib" if platform.system() == "Darwin" else "so"
-  shared_fn = os.path.join(folder, f"lib{lib_name}.{shared_ext}")
+  shared_fn = os.path.join(folder, f"lib{name}.{shared_ext}")
   header_fn = os.path.join(folder, f"{name}.h")
 
   with open(header_fn, encoding='utf-8') as f:
diff --git a/rednose/helpers/common_ekf.cc b/rednose/helpers/common_ekf.cc
deleted file mode 100644
index 1e63902..0000000
--- a/rednose/helpers/common_ekf.cc
+++ /dev/null
@@ -1,19 +0,0 @@
-#include "common_ekf.h"
-
-std::vector<const EKF*>& get_ekfs() {
-  static std::vector<const EKF*> vec;
-  return vec;
-}
-
-void ekf_register(const EKF* ekf) {
-  get_ekfs().push_back(ekf);
-}
-
-const EKF* ekf_lookup(const std::string& ekf_name) {
-  for (const auto& ekfi : get_ekfs()) {
-    if (ekf_name == ekfi->name) {
-      return ekfi;
-    }
-  }
-  return NULL;
-}
diff --git a/rednose/helpers/common_ekf.h b/rednose/helpers/ekf.h
similarity index 85%
rename from rednose/helpers/common_ekf.h
rename to rednose/helpers/ekf.h
index 5dfdd44..2afe6dd 100644
--- a/rednose/helpers/common_ekf.h
+++ b/rednose/helpers/ekf.h
@@ -32,12 +32,11 @@ struct EKF {
   std::unordered_map<std::string, extra_routine_t> extra_routines = {};
 };
 
-std::vector<const EKF*>& get_ekfs();
-const EKF* ekf_lookup(const std::string& ekf_name);
-
-void ekf_register(const EKF* ekf);
-
-#define ekf_init(ekf) \
+#define ekf_lib_init(ekf) \
+extern "C" void* ekf_get() { \
+  return (void*) &ekf; \
+} \
+extern void  __attribute__((weak)) ekf_register(const EKF* ptr); \
 static void __attribute__((constructor)) do_ekf_init_ ## ekf(void) { \
-  ekf_register(&ekf); \
+  if (ekf_register) ekf_register(&ekf); \
 }
diff --git a/rednose/helpers/ekf_load.cc b/rednose/helpers/ekf_load.cc
new file mode 100644
index 0000000..882b0b4
--- /dev/null
+++ b/rednose/helpers/ekf_load.cc
@@ -0,0 +1,39 @@
+#include "ekf_load.h"
+#include <dlfcn.h>
+
+std::vector<const EKF*>& ekf_get_all() {
+  static std::vector<const EKF*> vec;
+  return vec;
+}
+
+void ekf_register(const EKF* ekf) {
+  ekf_get_all().push_back(ekf);
+}
+
+const EKF* ekf_lookup(const std::string& ekf_name) {
+  for (const auto& ekfi : ekf_get_all()) {
+    if (ekf_name == ekfi->name) {
+      return ekfi;
+    }
+  }
+  return NULL;
+}
+
+void ekf_load_and_register(const std::string& ekf_directory, const std::string& ekf_name) {
+  if (ekf_lookup(ekf_name)) {
+    return;
+  }
+
+#ifdef __APPLE__
+  std::string dylib_ext = ".dylib";
+#else
+  std::string dylib_ext = ".so";
+#endif
+  std::string ekf_path = ekf_directory + "/lib" + ekf_name + dylib_ext;
+  void* handle = dlopen(ekf_path.c_str(), RTLD_NOW);
+  assert(handle);
+  void* (*ekf_get)() = (void*(*)())dlsym(handle, "ekf_get");
+  assert(ekf_get != NULL);
+  const EKF* ekf = (const EKF*)ekf_get();
+  ekf_register(ekf);
+}
diff --git a/rednose/helpers/ekf_load.h b/rednose/helpers/ekf_load.h
new file mode 100644
index 0000000..89180b8
--- /dev/null
+++ b/rednose/helpers/ekf_load.h
@@ -0,0 +1,9 @@
+#include <vector>
+#include <string>
+
+#include "ekf.h"
+
+std::vector<const EKF*>& ekf_get_all();
+const EKF* ekf_lookup(const std::string& ekf_name);
+void ekf_register(const EKF* ekf);
+void ekf_load_and_register(const std::string& ekf_directory, const std::string& ekf_name);
diff --git a/rednose/helpers/ekf_sym.cc b/rednose/helpers/ekf_sym.cc
index cf3c88f..38d419d 100644
--- a/rednose/helpers/ekf_sym.cc
+++ b/rednose/helpers/ekf_sym.cc
@@ -9,7 +9,6 @@ EKFSym::EKFSym(std::string name, Map<MatrixXdr> Q, Map<VectorXd> x_initial, Map<
     std::vector<int> quaternion_idxs, std::vector<std::string> global_vars, double max_rewind_age)
 {
   // TODO: add logger
-
   this->ekf = ekf_lookup(name);
   assert(this->ekf);
 
diff --git a/rednose/helpers/ekf_sym.h b/rednose/helpers/ekf_sym.h
index 86d6ca1..b83d7f3 100644
--- a/rednose/helpers/ekf_sym.h
+++ b/rednose/helpers/ekf_sym.h
@@ -12,7 +12,8 @@
 
 #include <eigen3/Eigen/Dense>
 
-#include "common_ekf.h"
+#include "ekf.h"
+#include "ekf_load.h"
 
 #define REWIND_TO_KEEP 512
 
diff --git a/rednose/helpers/ekf_sym.py b/rednose/helpers/ekf_sym.py
index de68991..8f0425c 100644
--- a/rednose/helpers/ekf_sym.py
+++ b/rednose/helpers/ekf_sym.py
@@ -116,7 +116,7 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p
   sympy_header, code = sympy_into_c(sympy_functions, global_vars)
 
   header = "#pragma once\n"
-  header += "#include \"rednose/helpers/common_ekf.h\"\n"
+  header += "#include \"rednose/helpers/ekf.h\"\n"
   header += "extern \"C\" {\n"
 
   pre_code = f"#include \"{name}.h\"\n"
@@ -200,7 +200,7 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p
     post_code += f"    {{ \"{f}\", {name}_{f} }},\n"
   post_code += "  },\n"
   post_code += "};\n\n"
-  post_code += f"ekf_init({name});\n"
+  post_code += f"ekf_lib_init({name})\n"
 
   # merge code blocks
   header += "}"
@@ -252,7 +252,7 @@ class EKF_sym():
     self.rewind_obscache = []
     self.init_state(x_initial, P_initial, None)
 
-    ffi, lib = load_code(folder, name, "kf")
+    ffi, lib = load_code(folder, name)
     kinds, self.feature_track_kinds = [], []
     for func in dir(lib):
       if func[:len(name) + 3] == f'{name}_h_':
diff --git a/rednose/helpers/ekf_sym_pyx.pyx b/rednose/helpers/ekf_sym_pyx.pyx
index e746128..f552ea7 100644
--- a/rednose/helpers/ekf_sym_pyx.pyx
+++ b/rednose/helpers/ekf_sym_pyx.pyx
@@ -11,12 +11,16 @@ cimport numpy as np
 
 import numpy as np
 
+
 cdef extern from "<optional>" namespace "std" nogil:
   cdef cppclass optional[T]:
     ctypedef T value_type
     bool has_value()
     T& value()
 
+cdef extern from "rednose/helpers/ekf_load.h":
+  cdef void ekf_load_and_register(string directory, string name)
+
 cdef extern from "rednose/helpers/ekf_sym.h" namespace "EKFS":
   cdef cppclass MapVectorXd "Eigen::Map<Eigen::VectorXd>":
     MapVectorXd(double*, int)
@@ -85,6 +89,7 @@ cdef class EKF_sym_pyx:
       int dim_main_err, int N=0, int dim_augment=0, int dim_augment_err=0, list maha_test_kinds=[],
       list quaternion_idxs=[], list global_vars=[], double max_rewind_age=1.0, logger=None):
     # TODO logger
+    ekf_load_and_register(gen_dir.encode('utf8'), name.encode('utf8'))
 
     cdef np.ndarray[np.float64_t, ndim=2, mode='c'] Q_b = np.ascontiguousarray(Q, dtype=np.double)
     cdef np.ndarray[np.float64_t, ndim=1, mode='c'] x_initial_b = np.ascontiguousarray(x_initial, dtype=np.double)
diff --git a/rednose/helpers/feature_handler.py b/rednose/helpers/feature_handler.py
deleted file mode 100755
index 165c9a4..0000000
--- a/rednose/helpers/feature_handler.py
+++ /dev/null
@@ -1,160 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-import sys
-
-import numpy as np
-
-from rednose.helpers import TEMPLATE_DIR, load_code, write_code
-from rednose.helpers.sympy_helpers import quat_matrix_l, rot_matrix
-
-
-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(generated_dir, 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 += "extern \"C\" {\n"
-    c_code += "\n" + open(os.path.join(TEMPLATE_DIR, "feature_handler.c"), encoding='utf-8').read()
-    c_code += "\n}\n"
-
-    filename = f"{FeatureHandler.name}_{K}"
-    write_code(generated_dir, filename, c_code, c_header)
-
-  def __init__(self, generated_dir, 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(generated_dir, 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, 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 rednose.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(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__":
-  K = int(sys.argv[1].split("_")[-1])
-  generated_dir = sys.argv[2]
-  FeatureHandler.generate_code(generated_dir, K=K)
diff --git a/rednose/helpers/lst_sq_computer.py b/rednose/helpers/lst_sq_computer.py
deleted file mode 100755
index 3c74eaa..0000000
--- a/rednose/helpers/lst_sq_computer.py
+++ /dev/null
@@ -1,175 +0,0 @@
-#!/usr/bin/env python3
-import os
-import sys
-
-import numpy as np
-import sympy as sp
-
-from rednose.helpers import TEMPLATE_DIR, load_code, write_code
-from rednose.helpers.sympy_helpers import quat_rotate, sympy_into_c, rot_matrix, rotations_from_quats
-
-
-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(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(generated_dir, K=4):
-    sympy_functions = generate_residual(K)
-    header, sympy_code = sympy_into_c(sympy_functions)
-
-    code = "\n#include \"rednose/helpers/common_ekf.h\"\n"
-    code += "\n#define KDIM %d\n" % K
-    code += "extern \"C\" {\n"
-    code += sympy_code
-    code += "\n" + open(os.path.join(TEMPLATE_DIR, "compute_pos.c"), encoding='utf-8').read() + "\n"
-    code += "}\n"
-
-    header += "\nvoid compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);\n"
-
-    filename = f"{LstSqComputer.name}_{K}"
-    write_code(generated_dir, filename, code, header)
-
-  def __init__(self, generated_dir, K=4, MIN_DEPTH=2, MAX_DEPTH=500):
-    self.to_c = 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(generated_dir, 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 = (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 = 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])
-  generated_dir = sys.argv[2]
-  LstSqComputer.generate_code(generated_dir, K=K)
diff --git a/site_scons/site_tools/rednose_filter.py b/site_scons/site_tools/rednose_filter.py
new file mode 100644
index 0000000..b3fee62
--- /dev/null
+++ b/site_scons/site_tools/rednose_filter.py
@@ -0,0 +1,48 @@
+import platform
+
+from SCons.Script import Dir, File
+
+
+def compile_single_filter(env, target, filter_gen_script, output_dir, extra_gen_artifacts, script_deps):
+  generated_src_files = [File(f) for f in [f'{output_dir}/{target}.cpp', f'{output_dir}/{target}.h']]
+  extra_generated_files = [File(f'{output_dir}/{x}') for x in extra_gen_artifacts]
+  generator_file = File(filter_gen_script)
+
+  env.Command(generated_src_files + extra_generated_files,
+              [generator_file] + script_deps, f"{generator_file.get_abspath()} {target} {Dir(output_dir).get_abspath()}")
+
+  generated_cc_file = File(generated_src_files[:1])
+
+  return generated_cc_file
+
+
+class BaseRednoseCompileMethod:
+  def __init__(self, base_py_deps, base_cc_deps):
+    self.base_py_deps = base_py_deps
+    self.base_cc_deps = base_cc_deps
+
+
+class CompileFilterMethod(BaseRednoseCompileMethod):
+  def __call__(self, env, target, filter_gen_script, output_dir, extra_gen_artifacts=[], gen_script_deps=[]):
+    objects = compile_single_filter(env, target, filter_gen_script, output_dir, extra_gen_artifacts, self.base_py_deps + gen_script_deps)
+    linker_flags = env.get("LINKFLAGS", [])
+    if platform.system() == "Darwin":
+      linker_flags = ["-undefined", "dynamic_lookup"]
+    lib_target = env.SharedLibrary(f'{output_dir}/{target}', [self.base_cc_deps, objects], LINKFLAGS=linker_flags)
+
+    return lib_target
+
+
+def generate(env):
+  templates = env.Glob("$REDNOSE_ROOT/rednose/templates/*")
+  sympy_helpers = env.File("$REDNOSE_ROOT/rednose/helpers/sympy_helpers.py")
+  ekf_sym = env.File("$REDNOSE_ROOT/rednose/helpers/ekf_sym.py")
+
+  gen_script_deps = templates + [sympy_helpers, ekf_sym]
+  filter_lib_deps = []
+
+  env.AddMethod(CompileFilterMethod(gen_script_deps, filter_lib_deps), "RednoseCompileFilter")
+
+
+def exists(env):
+  return True