IQ.Pilot Release Commit @ bec7652

artifacts/package_sources/rednose/.dockerignore

@@ -0,0 +1 @@
+.sconsign.dblite

artifacts/package_sources/rednose/.editorconfig

@@ -0,0 +1,11 @@
+root = true
+
+[*]
+end_of_line = lf
+insert_final_newline = true
+trim_trailing_whitespace = true
+
+[{*.py, *.pyx, *pxd}]
+charset = utf-8
+indent_style = space
+indent_size = 2

artifacts/package_sources/rednose/.gitignore

@@ -0,0 +1,151 @@
+generated/
+.sconsign.dblite
+*.swp
+*.tmp
+
+# Cython intermediates
+*_pyx.cpp
+*_pyx.h
+*_pyx_api.h
+*.os
+
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.a
+*.o
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/

artifacts/package_sources/rednose/.pre-commit-config.yaml

@@ -0,0 +1,21 @@
+repos:
+-   repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.0.1
+    hooks:
+    -   id: check-ast
+    -   id: check-json
+    -   id: check-xml
+    -   id: check-yaml
+    -   id: check-merge-conflict
+    -   id: check-symlinks
+    -   id: check-executables-have-shebangs
+    -   id: check-shebang-scripts-are-executable
+-   repo: https://github.com/pre-commit/mirrors-mypy
+    rev: v1.4.0
+    hooks:
+    -   id: mypy
+        additional_dependencies: ['numpy']
+-   repo: https://github.com/astral-sh/ruff-pre-commit
+    rev: v0.2.2
+    hooks:
+    - id: ruff

artifacts/package_sources/rednose/Dockerfile

@@ -0,0 +1,14 @@
+FROM ubuntu:24.04
+
+ENV DEBIAN_FRONTEND=noninteractive
+RUN apt-get update && apt-get install -y capnproto libcapnp-dev clang wget git autoconf libtool curl make build-essential libssl-dev zlib1g-dev libbz2-dev libreadline-dev libsqlite3-dev llvm libncurses5-dev libncursesw5-dev xz-utils tk-dev libffi-dev liblzma-dev python3-openssl libeigen3-dev python3-pip python3-dev
+
+WORKDIR /project
+
+ENV PYTHONPATH=/project
+
+COPY . .
+RUN rm -rf .git
+RUN pip3 install --break-system-packages --no-cache-dir -r requirements.txt
+RUN python3 setup.py install
+RUN scons -c && scons -j$(nproc)

artifacts/package_sources/rednose/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2020 comma.ai
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

artifacts/package_sources/rednose/MANIFEST.in

@@ -0,0 +1,4 @@
+include SConstruct
+graft rednose
+graft site_scons
+global-exclude __pycache__ *.pyc *.o *.os *.d

artifacts/package_sources/rednose/README.md

@@ -0,0 +1,51 @@
+## Introduction
+The kalman filter framework described here is an incredibly powerful tool for any optimization problem,
+but particularly for visual odometry, sensor fusion localization or SLAM. It is designed to provide very
+accurate results, work online or offline, be fairly computationally efficient, be easy to design filters with in
+python.
+
+![](examples/kinematic_kf.png)
+
+
+## Feature walkthrough
+
+### Extended Kalman Filter with symbolic Jacobian computation
+Most dynamic systems can be described as a Hidden Markov Process. To estimate the state of such a system with noisy
+measurements one can use a Recursive Bayesian estimator. For a linear Markov Process a regular linear Kalman filter is optimal.
+Unfortunately, a lot of systems are non-linear. Extended Kalman Filters can model systems by linearizing the non-linear
+system at every step, this provides a close to optimal estimator when the linearization is good enough. If the linearization
+introduces too much noise, one can use an Iterated Extended Kalman Filter, Unscented Kalman Filter or a Particle Filter. For
+most applications those estimators are overkill. They add a lot of complexity and require a lot of additional compute.
+
+Conventionally Extended Kalman Filters are implemented by writing the system's dynamic equations and then manually symbolically
+calculating the Jacobians for the linearization. For complex systems this is time consuming and very prone to calculation errors.
+This library symbolically computes the Jacobians using sympy to simplify the system's definition and remove the possibility of introducing calculation errors.
+
+### Error State Kalman Filter
+3D localization algorithms usually also require estimating orientation of an object in 3D. Orientation is generally represented
+with euler angles or quaternions.
+
+Euler angles have several problems, there are multiple ways to represent the same orientation,
+gimbal lock can cause the loss of a degree of freedom and lastly their behaviour is very non-linear when errors are large.
+Quaternions with one strictly positive dimension don't suffer from these issues, but have another set of problems.
+Quaternions need to be normalized otherwise they will grow unbounded, but this cannot be cleanly enforced in a kalman filter.
+Most importantly though a quaternion has 4 dimensions, but only represents 3 degrees of freedom, so there is one redundant dimension.
+
+Kalman filters are designed to minimize the error of the system's state. It is possible to have a kalman filter where state and the error of the state are represented in a different space. As long as there is an error function that can compute the error based on the true state and estimated state. It is problematic to have redundant dimensions in the error of the kalman filter, but not in the state. A good compromise then, is to use the quaternion to represent the system's attitude state and use euler angles to describe the error in attitude. This library supports and defining an arbitrary error that is in  a different space than the state. [Joan Solà](https://arxiv.org/abs/1711.02508) has written a comprehensive description of using ESKFs for robust 3D orientation estimation.
+
+### Multi-State Constraint Kalman Filter
+How do you integrate feature-based visual odometry with a Kalman filter? The problem is that one cannot write an observation equation for 2D feature observations in image space for a localization kalman filter. One needs to give the feature observation a depth so it has a 3D position, then one can write an obvervation equation in the kalman filter. This is possible by tracking the feature across frames and then estimating the depth. However, the solution is not that simple, the depth estimated by tracking the feature across frames depends on the location of the camera at those frames, and thus the state of the kalman filter. This creates a positive feedback loop where the kalman filter wrongly gains confidence in it's position because the feature position updates reinforce it.
+
+The solution is to use an [MSCKF](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf), which this library fully supports.
+
+### Rauch–Tung–Striebel smoothing
+When doing offline estimation with a kalman filter there can be an initialization period where states are badly estimated.
+Global estimators don't suffer from this, to make our kalman filter competitive with global optimizers we can run the filter
+backwards using an RTS smoother. Those combined with potentially multiple forward and backwards passes of the data should make
+performance very close to global optimization.
+
+### Mahalanobis distance outlier rejector
+A lot of measurements do not come from a Gaussian distribution and as such have outliers that do not fit the statistical model
+of the Kalman filter. This can cause a lot of performance issues if not dealt with. This library allows the use of a mahalanobis
+distance statistical test on the incoming measurements to deal with this. Note that good initialization is critical to prevent
+good measurements from being rejected.

artifacts/package_sources/rednose/SConstruct

@@ -0,0 +1,64 @@
+import os
+import platform
+import subprocess
+import sysconfig
+import numpy as np
+
+arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
+if platform.system() == "Darwin":
+  arch = "Darwin"
+
+common = ''
+
+python_path = sysconfig.get_paths()['include']
+cpppath = [
+  '#',
+  '#rednose',
+  '#rednose/examples/generated',
+  '/usr/lib/include',
+  python_path,
+  np.get_include(),
+]
+if platform.system() == "Darwin":
+  cpppath.append('/opt/homebrew/include')
+
+env = Environment(
+  ENV=os.environ,
+  CC='clang',
+  CXX='clang++',
+  CCFLAGS=[
+    "-g",
+    "-fPIC",
+    "-O2",
+    "-Werror=implicit-function-declaration",
+    "-Werror=incompatible-pointer-types",
+    "-Werror=int-conversion",
+    "-Werror=return-type",
+    "-Werror=format-extra-args",
+    "-Wshadow",
+  ],
+  LIBPATH=["#rednose/examples/generated"],
+  CFLAGS="-std=gnu11",
+  CXXFLAGS="-std=c++1z",
+  CPPPATH=cpppath,
+  REDNOSE_ROOT=Dir("#").abspath,
+  tools=["default", "cython", "rednose_filter"],
+)
+
+# Cython build enviroment
+envCython = env.Clone()
+envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"]
+
+envCython["LIBS"] = []
+if arch == "Darwin":
+  envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"]
+elif arch == "aarch64":
+  envCython["LINKFLAGS"] = ["-shared"]
+  envCython["LIBS"] = [os.path.basename(python_path)]
+else:
+  envCython["LINKFLAGS"] = ["-pthread", "-shared"]
+
+Export('env', 'envCython', 'common')
+
+SConscript(['#rednose/SConscript'])
+SConscript(['#examples/SConscript'])

artifacts/package_sources/rednose/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,
+)

artifacts/package_sources/rednose/examples/kinematic_kf.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python3
+import sys
+
+import numpy as np
+import sympy as sp
+
+from rednose.helpers.kalmanfilter import KalmanFilter
+
+if __name__ == '__main__':  # generating sympy code
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module
+
+
+class ObservationKind():
+  UNKNOWN = 0
+  NO_OBSERVATION = 1
+  POSITION = 1
+
+  names = [
+    'Unknown',
+    'No observation',
+    'Position'
+  ]
+
+  @classmethod
+  def to_string(cls, kind):
+    return cls.names[kind]
+
+
+class States():
+  POSITION = slice(0, 1)
+  VELOCITY = slice(1, 2)
+
+
+class KinematicKalman(KalmanFilter):
+  name = 'kinematic'
+
+  initial_x = np.array([0.5, 0.0])
+
+  # state covariance
+  initial_P_diag = np.array([1.0**2, 1.0**2])
+
+  # process noise
+  Q = np.diag([0.1**2, 2.0**2])
+
+  obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)}
+
+  @staticmethod
+  def generate_code(generated_dir):
+    name = KinematicKalman.name
+    dim_state = KinematicKalman.initial_x.shape[0]
+
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+
+    position = state[States.POSITION, :][0,:]
+    velocity = state[States.VELOCITY, :][0,:]
+
+    dt = sp.Symbol('dt')
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.POSITION.start, 0] = velocity
+    f_sym = state + dt * state_dot
+
+    obs_eqs = [
+      [sp.Matrix([position]), ObservationKind.POSITION, None],
+    ]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)
+
+  def __init__(self, generated_dir):
+    dim_state = self.initial_x.shape[0]
+    dim_state_err = self.initial_P_diag.shape[0]
+
+    # init filter
+    self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  KinematicKalman.generate_code(generated_dir)

artifacts/package_sources/rednose/examples/live_kf.py

@@ -0,0 +1,342 @@
+#!/usr/bin/env python3
+import sys
+import numpy as np
+
+from rednose.helpers import KalmanError
+
+if __name__ == '__main__':  # Generating sympy
+  import sympy as sp
+  from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx  # pylint: disable=no-name-in-module
+
+EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
+
+
+class ObservationKind():
+  UNKNOWN = 0
+  NO_OBSERVATION = 1
+  GPS_NED = 2
+  ODOMETRIC_SPEED = 3
+  PHONE_GYRO = 4
+  GPS_VEL = 5
+  PSEUDORANGE_GPS = 6
+  PSEUDORANGE_RATE_GPS = 7
+  SPEED = 8
+  NO_ROT = 9
+  PHONE_ACCEL = 10
+  ORB_POINT = 11
+  ECEF_POS = 12
+  CAMERA_ODO_TRANSLATION = 13
+  CAMERA_ODO_ROTATION = 14
+  ORB_FEATURES = 15
+  MSCKF_TEST = 16
+  FEATURE_TRACK_TEST = 17
+  LANE_PT = 18
+  IMU_FRAME = 19
+  PSEUDORANGE_GLONASS = 20
+  PSEUDORANGE_RATE_GLONASS = 21
+  PSEUDORANGE = 22
+  PSEUDORANGE_RATE = 23
+
+  names = [
+    'Unknown',
+    'No observation',
+    'GPS NED',
+    'Odometric speed',
+    'Phone gyro',
+    'GPS velocity',
+    'GPS pseudorange',
+    'GPS pseudorange rate',
+    'Speed',
+    'No rotation',
+    'Phone acceleration',
+    'ORB point',
+    'ECEF pos',
+    'camera odometric translation',
+    'camera odometric rotation',
+    'ORB features',
+    'MSCKF test',
+    'Feature track test',
+    'Lane ecef point',
+    'imu frame eulers',
+    'GLONASS pseudorange',
+    'GLONASS pseudorange rate',
+  ]
+
+  @classmethod
+  def to_string(cls, kind):
+    return cls.names[kind]
+
+
+class States():
+  ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters
+  ECEF_ORIENTATION = slice(3, 7)  # quat for pose of phone in ecef
+  ECEF_VELOCITY = slice(7, 10)  # ecef velocity in m/s
+  ANGULAR_VELOCITY = slice(10, 13)  # roll, pitch and yaw rates in device frame in radians/s
+  GYRO_BIAS = slice(13, 16)  # roll, pitch and yaw biases
+  ODO_SCALE = slice(16, 17)  # odometer scale
+  ACCELERATION = slice(17, 20)  # Acceleration in device frame in m/s**2
+  IMU_OFFSET = slice(20, 23)  # imu offset angles in radians
+
+  # Error-state has different slices because it is an ESKF
+  ECEF_POS_ERR = slice(0, 3)
+  ECEF_ORIENTATION_ERR = slice(3, 6)  # euler angles for orientation error
+  ECEF_VELOCITY_ERR = slice(6, 9)
+  ANGULAR_VELOCITY_ERR = slice(9, 12)
+  GYRO_BIAS_ERR = slice(12, 15)
+  ODO_SCALE_ERR = slice(15, 16)
+  ACCELERATION_ERR = slice(16, 19)
+  IMU_OFFSET_ERR = slice(19, 22)
+
+
+class LiveKalman():
+  name = 'live'
+
+  initial_x = np.array([-2.7e6, 4.2e6, 3.8e6,
+                        1, 0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0,
+                        1,
+                        0, 0, 0,
+                        0, 0, 0])
+
+  # state covariance
+  initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
+                             10**2, 10**2, 10**2,
+                             10**2, 10**2, 10**2,
+                             1**2, 1**2, 1**2,
+                             0.05**2, 0.05**2, 0.05**2,
+                             0.02**2,
+                             1**2, 1**2, 1**2,
+                             (0.01)**2, (0.01)**2, (0.01)**2])
+
+  # process noise
+  Q = np.diag([0.03**2, 0.03**2, 0.03**2,
+               0.0**2, 0.0**2, 0.0**2,
+               0.0**2, 0.0**2, 0.0**2,
+               0.1**2, 0.1**2, 0.1**2,
+               (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
+               (0.02 / 100)**2,
+               3**2, 3**2, 3**2,
+               (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
+
+  @staticmethod
+  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 = 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])
+
+    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(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
+
+  def __init__(self, generated_dir):
+    self.dim_state = self.initial_x.shape[0]
+    self.dim_state_err = self.initial_P_diag.shape[0]
+
+    self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
+                      ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
+                      ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
+                      ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
+                      ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
+                      ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
+                      ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
+
+    # init filter
+    self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
+
+  @property
+  def x(self):
+    return self.filter.state()
+
+  @property
+  def t(self):
+    return self.filter.filter_time
+
+  @property
+  def P(self):
+    return self.filter.covs()
+
+  def rts_smooth(self, estimates):
+    return self.filter.rts_smooth(estimates, norm_quats=True)
+
+  def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
+    if covs_diag is not None:
+      P = np.diag(covs_diag)
+    elif covs is not None:
+      P = covs
+    else:
+      P = self.filter.covs()
+    self.filter.init_state(state, P, filter_time)
+
+  def predict_and_observe(self, t, kind, data):
+    if len(data) > 0:
+      data = np.atleast_2d(data)
+    if kind == ObservationKind.CAMERA_ODO_TRANSLATION:
+      r = self.predict_and_update_odo_trans(data, t, kind)
+    elif kind == ObservationKind.CAMERA_ODO_ROTATION:
+      r = self.predict_and_update_odo_rot(data, t, kind)
+    elif kind == ObservationKind.ODOMETRIC_SPEED:
+      r = self.predict_and_update_odo_speed(data, t, kind)
+    else:
+      r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
+
+    # Normalize quats
+    quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
+
+    # Should not continue if the quats behave this weirdly
+    if not (0.1 < quat_norm < 10):
+      raise KalmanError("Kalman filter quaternions unstable")
+
+    self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
+
+    return r
+
+  def get_R(self, kind, n):
+    obs_noise = self.obs_noise[kind]
+    dim = obs_noise.shape[0]
+    R = np.zeros((n, dim, dim))
+    for i in range(n):
+      R[i, :, :] = obs_noise
+    return R
+
+  def predict_and_update_odo_speed(self, speed, t, kind):
+    z = np.array(speed)
+    R = np.zeros((len(speed), 1, 1))
+    for i, _ in enumerate(z):
+      R[i, :, :] = np.diag([0.2**2])
+    return self.filter.predict_and_update_batch(t, kind, z, R)
+
+  def predict_and_update_odo_trans(self, trans, t, kind):
+    z = trans[:, :3]
+    R = np.zeros((len(trans), 3, 3))
+    for i, _ in enumerate(z):
+        R[i, :, :] = np.diag(trans[i, 3:]**2)
+    return self.filter.predict_and_update_batch(t, kind, z, R)
+
+  def predict_and_update_odo_rot(self, rot, t, kind):
+    z = rot[:, :3]
+    R = np.zeros((len(rot), 3, 3))
+    for i, _ in enumerate(z):
+        R[i, :, :] = np.diag(rot[i, 3:]**2)
+    return self.filter.predict_and_update_batch(t, kind, z, R)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  LiveKalman.generate_code(generated_dir)

artifacts/package_sources/rednose/examples/test_compare.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+import pytest
+import os
+import sys
+import sympy as sp
+import numpy as np
+
+if __name__ == '__main__':  # generating sympy code
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module
+  from rednose.helpers.ekf_sym import EKF_sym as EKF_sym2
+
+
+GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
+
+
+class ObservationKind:
+  UNKNOWN = 0
+  NO_OBSERVATION = 1
+  POSITION = 1
+
+  names = [
+    'Unknown',
+    'No observation',
+    'Position'
+  ]
+
+  @classmethod
+  def to_string(cls, kind):
+    return cls.names[kind]
+
+
+class States:
+  POSITION = slice(0, 1)
+  VELOCITY = slice(1, 2)
+
+
+class CompareFilter:
+  name = "compare"
+
+  initial_x = np.array([0.5, 0.0])
+  initial_P_diag = np.array([1.0**2, 1.0**2])
+  Q = np.diag([0.1**2, 2.0**2])
+  obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)}
+
+  @staticmethod
+  def generate_code(generated_dir):
+    name = CompareFilter.name
+    dim_state = CompareFilter.initial_x.shape[0]
+
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+
+    position = state[States.POSITION, :][0,:]
+    velocity = state[States.VELOCITY, :][0,:]
+
+    dt = sp.Symbol('dt')
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.POSITION.start, 0] = velocity
+    f_sym = state + dt * state_dot
+
+    obs_eqs = [
+      [sp.Matrix([position]), ObservationKind.POSITION, None],
+    ]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)
+
+  def __init__(self, generated_dir):
+    dim_state = self.initial_x.shape[0]
+    dim_state_err = self.initial_P_diag.shape[0]
+
+    # init filter
+    self.filter_py = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
+    self.filter_pyx = EKF_sym2(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
+
+  def get_R(self, kind, n):
+    obs_noise = self.obs_noise[kind]
+    dim = obs_noise.shape[0]
+    R = np.zeros((n, dim, dim))
+    for i in range(n):
+      R[i, :, :] = obs_noise
+    return R
+
+
+class TestCompare:
+  def test_compare(self):
+    np.random.seed(0)
+
+    kf = CompareFilter(GENERATED_DIR)
+
+    # Simple simulation
+    dt = 0.01
+    ts = np.arange(0, 5, step=dt)
+    xs = np.empty(ts.shape)
+
+    # Simulate
+    x = 0.0
+    for i, v in enumerate(np.sin(ts * 5)):
+      xs[i] = x
+      x += v * dt
+
+    # insert late observation
+    switch = (20, 40)
+    ts[switch[0]], ts[switch[1]] = ts[switch[1]], ts[switch[0]]
+    xs[switch[0]], xs[switch[1]] = xs[switch[1]], xs[switch[0]]
+
+    for t, x in zip(ts, xs):
+      # get measurement
+      meas = np.random.normal(x, 0.1)
+      z = np.array([[meas]])
+      R = kf.get_R(ObservationKind.POSITION, 1)
+
+      # Update kf
+      kf.filter_py.predict_and_update_batch(t, ObservationKind.POSITION, z, R)
+      kf.filter_pyx.predict_and_update_batch(t, ObservationKind.POSITION, z, R)
+
+      assert kf.filter_py.get_filter_time() == pytest.approx(kf.filter_pyx.get_filter_time())
+      assert np.allclose(kf.filter_py.state(), kf.filter_pyx.state())
+      assert np.allclose(kf.filter_py.covs(), kf.filter_pyx.covs())
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  CompareFilter.generate_code(generated_dir)

artifacts/package_sources/rednose/examples/test_kinematic_kf.py

@@ -0,0 +1,82 @@
+import pytest
+import os
+import numpy as np
+
+from .kinematic_kf import KinematicKalman, ObservationKind, States
+
+GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
+
+class TestKinematic:
+  def test_kinematic_kf(self):
+    np.random.seed(0)
+
+    kf = KinematicKalman(GENERATED_DIR)
+
+    # Simple simulation
+    dt = 0.01
+    ts = np.arange(0, 5, step=dt)
+    vs = np.sin(ts * 5)
+
+    x = 0.0
+    xs = []
+
+    xs_meas = []
+
+    xs_kf = []
+    vs_kf = []
+
+    xs_kf_std = []
+    vs_kf_std = []
+
+    for t, v in zip(ts, vs):
+      xs.append(x)
+
+      # Update kf
+      meas = np.random.normal(x, 0.1)
+      xs_meas.append(meas)
+      kf.predict_and_observe(t, ObservationKind.POSITION, [meas])
+
+      # Retrieve kf values
+      state = kf.x
+      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].item()))
+      vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY].item()))
+
+      # Update simulation
+      x += v * dt
+
+    xs, xs_meas, xs_kf, vs_kf, xs_kf_std, vs_kf_std = (np.asarray(a) for a in (xs, xs_meas, xs_kf, vs_kf, xs_kf_std, vs_kf_std))
+
+    assert xs_kf[-1] == pytest.approx(-0.010866289677966417)
+    assert xs_kf_std[-1] == pytest.approx(0.04477103863330089)
+    assert vs_kf[-1] == pytest.approx(-0.8553720537261753)
+    assert vs_kf_std[-1] == pytest.approx(0.6695762270974388)
+
+    if "PLOT" in os.environ:
+      import matplotlib.pyplot as plt  # pylint: disable=import-error
+      plt.figure()
+      plt.subplot(2, 1, 1)
+      plt.plot(ts, xs, 'k', label='Simulation')
+      plt.plot(ts, xs_meas, 'k.', label='Measurements')
+      plt.plot(ts, xs_kf, label='KF')
+      ax = plt.gca()
+      ax.fill_between(ts, xs_kf - xs_kf_std, xs_kf + xs_kf_std, alpha=.2, color='C0')
+
+      plt.xlabel("Time [s]")
+      plt.ylabel("Position [m]")
+      plt.legend()
+
+      plt.subplot(2, 1, 2)
+      plt.plot(ts, vs, 'k', label='Simulation')
+      plt.plot(ts, vs_kf, label='KF')
+
+      ax = plt.gca()
+      ax.fill_between(ts, vs_kf - vs_kf_std, vs_kf + vs_kf_std, alpha=.2, color='C0')
+
+      plt.xlabel("Time [s]")
+      plt.ylabel("Velocity [m/s]")
+      plt.legend()
+
+      plt.show()

artifacts/package_sources/rednose/pyproject.toml

@@ -0,0 +1,37 @@
+[project]
+name = "rednose"
+version = "0.0.1"
+description = "Kalman filter library"
+requires-python = ">=3.11,<3.13"
+license = "MIT"
+dependencies = ["numpy", "cffi", "sympy"]
+
+[project.optional-dependencies]
+dev = ["scipy"]
+
+[build-system]
+requires = ["setuptools>=64", "Cython", "scons", "numpy"]
+build-backend = "setuptools.build_meta"
+
+[tool.setuptools.packages.find]
+include = ["rednose", "rednose.*"]
+
+[tool.setuptools.package-data]
+rednose = ["helpers/*.h", "helpers/*.a", "helpers/*.so", "helpers/*.dylib", "helpers/chi2_lookup_table.npy", "templates/*", "site_scons/site_tools/*.py"]
+
+# https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml
+[tool.ruff]
+line-length = 160
+target-version="py311"
+
+[tool.ruff.lint]
+select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF100", "A"]
+ignore = ["W292", "E741", "E402", "C408", "ISC003"]
+flake8-implicit-str-concat.allow-multiline=false
+
+[tool.ruff.lint.flake8-tidy-imports.banned-api]
+"pytest.main".msg = "pytest.main requires special handling that is easy to mess up!"
+"unittest".msg = "Use pytest"
+
+[tool.pytest.ini_options]
+addopts = "--durations=10 -n auto"

artifacts/package_sources/rednose/rednose/.gitignore

@@ -0,0 +1,5 @@
+# Cython intermediates
+*_pyx.cpp
+*_pyx.h
+*_pyx_api.h
+*.os

artifacts/package_sources/rednose/rednose/SConscript

@@ -0,0 +1,17 @@
+Import('env', 'envCython', 'common')
+
+cc_sources = [
+  "helpers/ekf_load.cc",
+  "helpers/ekf_sym.cc",
+]
+libs = ["dl"]
+if common != "":
+  # for SWAGLOG support
+  libs += [common, 'zmq']
+
+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('rednose', 'rednose_python')

artifacts/package_sources/rednose/rednose/__init__.py

@@ -0,0 +1,6 @@
+import os
+
+
+INCLUDE_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
+LIB_PATH = os.path.join(os.path.dirname(__file__), "helpers", "libekf_sym.a")
+SCONS_TOOL_PATH = os.path.join(os.path.dirname(__file__), "site_scons", "site_tools")

artifacts/package_sources/rednose/rednose/helpers/__init__.py

@@ -0,0 +1,35 @@
+import os
+import platform
+from cffi import FFI
+
+TEMPLATE_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'templates'))
+
+
+def write_code(folder, name, code, header):
+  if not os.path.exists(folder):
+    os.mkdir(folder)
+
+  with open(os.path.join(folder, f"{name}.cpp"), 'w', encoding='utf-8') as f:
+    f.write(code)
+  with open(os.path.join(folder, f"{name}.h"), 'w', encoding='utf-8') as f:
+    f.write(header)
+
+
+def load_code(folder, name):
+  shared_ext = "dylib" if platform.system() == "Darwin" else "so"
+  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:
+    header = f.read()
+
+  # is the only thing that can be parsed by cffi
+  header = "\n".join([line for line in header.split("\n") if line.startswith("void ")])
+
+  ffi = FFI()
+  ffi.cdef(header)
+  return (ffi, ffi.dlopen(shared_fn))
+
+
+class KalmanError(Exception):
+  pass

artifacts/package_sources/rednose/rednose/helpers/chi2_lookup.py

@@ -0,0 +1,22 @@
+import os
+
+import numpy as np
+
+
+def gen_chi2_ppf_lookup(max_dim=200):
+  from scipy.stats import chi2
+  table = np.zeros((max_dim, 98))
+  for dim in range(1, max_dim):
+    table[dim] = chi2.ppf(np.arange(.01, .99, .01), dim)
+
+  np.save('chi2_lookup_table', table)
+
+
+def chi2_ppf(p, dim):
+  table = np.load(os.path.dirname(os.path.realpath(__file__)) + '/chi2_lookup_table.npy')
+  result = np.interp(p, np.arange(.01, .99, .01), table[dim])
+  return result
+
+
+if __name__ == "__main__":
+  gen_chi2_ppf_lookup()

artifacts/package_sources/rednose/rednose/helpers/ekf.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <iostream>
+#include <cassert>
+#include <string>
+#include <vector>
+#include <deque>
+#include <unordered_map>
+#include <map>
+#include <cmath>
+
+#include <eigen3/Eigen/Dense>
+
+typedef void (*extra_routine_t)(double *, double *);
+
+struct EKF {
+  std::string name;
+  std::vector<int> kinds;
+  std::vector<int> feature_kinds;
+
+  void (*f_fun)(double *, double, double *);
+  void (*F_fun)(double *, double, double *);
+  void (*err_fun)(double *, double *, double *);
+  void (*inv_err_fun)(double *, double *, double *);
+  void (*H_mod_fun)(double *, double *);
+  void (*predict)(double *, double *, double *, double);
+  std::unordered_map<int, void (*)(double *, double *, double *)> hs = {};
+  std::unordered_map<int, void (*)(double *, double *, double *)> Hs = {};
+  std::unordered_map<int, void (*)(double *, double *, double *, double *, double *)> updates = {};
+  std::unordered_map<int, void (*)(double *, double *, double *)> Hes = {};
+  std::unordered_map<std::string, void (*)(double)> sets = {};
+  std::unordered_map<std::string, extra_routine_t> extra_routines = {};
+};
+
+#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) { \
+  if (ekf_register) ekf_register(&ekf); \
+}

artifacts/package_sources/rednose/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);
+}

artifacts/package_sources/rednose/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);

artifacts/package_sources/rednose/rednose/helpers/ekf_sym.cc

@@ -0,0 +1,223 @@
+#include "ekf_sym.h"
+#include "logger/logger.h"
+
+using namespace EKFS;
+using namespace Eigen;
+
+EKFSym::EKFSym(std::string name, Map<MatrixXdr> Q, Map<VectorXd> x_initial, Map<MatrixXdr> P_initial, int dim_main,
+    int dim_main_err, int N, int dim_augment, int dim_augment_err, std::vector<int> maha_test_kinds,
+    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);
+
+  this->msckf = N > 0;
+  this->N = N;
+  this->dim_augment = dim_augment;
+  this->dim_augment_err = dim_augment_err;
+  this->dim_main = dim_main;
+  this->dim_main_err = dim_main_err;
+
+  this->dim_x = x_initial.rows();
+  this->dim_err = P_initial.rows();
+
+  assert(dim_main + dim_augment * N == dim_x);
+  assert(dim_main_err + dim_augment_err * N == this->dim_err);
+  assert(Q.rows() == P_initial.rows() && Q.cols() == P_initial.cols());
+
+  // kinds that should get mahalanobis distance
+  // tested for outlier rejection
+  this->maha_test_kinds = maha_test_kinds;
+
+  // quaternions need normalization
+  this->quaternion_idxs = quaternion_idxs;
+
+  this->global_vars = global_vars;
+
+  // Process noise
+  this->Q = Q;
+
+  this->max_rewind_age = max_rewind_age;
+  this->init_state(x_initial, P_initial, NAN);
+}
+
+void EKFSym::init_state(Map<VectorXd> state, Map<MatrixXdr> covs, double init_filter_time) {
+  this->x = state;
+  this->P = covs;
+  this->filter_time = init_filter_time;
+  this->augment_times = VectorXd::Zero(this->N);
+  this->reset_rewind();
+}
+
+VectorXd EKFSym::state() {
+  return this->x;
+}
+
+MatrixXdr EKFSym::covs() {
+  return this->P;
+}
+
+void EKFSym::set_filter_time(double t) {
+  this->filter_time = t;
+}
+
+double EKFSym::get_filter_time() {
+  return this->filter_time;
+}
+
+void EKFSym::normalize_quaternions() {
+  for(std::size_t i = 0; i < this->quaternion_idxs.size(); ++i) {
+    this->normalize_slice(this->quaternion_idxs[i], this->quaternion_idxs[i] + 4);
+  }
+}
+
+void EKFSym::normalize_slice(int slice_start, int slice_end_ex) {
+  this->x.block(slice_start, 0, slice_end_ex - slice_start, this->x.cols()).normalize();
+}
+
+void EKFSym::set_global(std::string global_var, double val) {
+  this->ekf->sets.at(global_var)(val);
+}
+
+std::optional<Estimate> EKFSym::predict_and_update_batch(double t, int kind, std::vector<Map<VectorXd>> z_map,
+    std::vector<Map<MatrixXdr>> R_map, std::vector<std::vector<double>> extra_args, bool augment)
+{
+  // TODO handle rewinding at this level
+
+  std::deque<Observation> rewound;
+  if (!std::isnan(this->filter_time) && t < this->filter_time) {
+    if (this->rewind_t.empty() || t < this->rewind_t.front() || t < this->rewind_t.back() - this->max_rewind_age) {
+      LOGD("observation too old at %f with filter at %f, ignoring!", t, this->filter_time);
+      return std::nullopt;
+    }
+    rewound = this->rewind(t);
+  }
+
+  Observation obs;
+  obs.t = t;
+  obs.kind = kind;
+  obs.extra_args = extra_args;
+  for (Map<VectorXd> zi : z_map) {
+    obs.z.push_back(zi);
+  }
+  for (Map<MatrixXdr> Ri : R_map) {
+    obs.R.push_back(Ri);
+  }
+
+  std::optional<Estimate> res = std::make_optional(this->predict_and_update_batch(obs, augment));
+
+  // optional fast forward
+  while (!rewound.empty()) {
+    this->predict_and_update_batch(rewound.front(), false);
+    rewound.pop_front();
+  }
+
+  return res;
+}
+
+void EKFSym::reset_rewind() {
+  this->rewind_obscache.clear();
+  this->rewind_t.clear();
+  this->rewind_states.clear();
+}
+
+std::deque<Observation> EKFSym::rewind(double t) {
+  std::deque<Observation> rewound;
+
+  // rewind observations until t is after previous observation
+  while (this->rewind_t.back() > t) {
+    rewound.push_front(this->rewind_obscache.back());
+    this->rewind_t.pop_back();
+    this->rewind_states.pop_back();
+    this->rewind_obscache.pop_back();
+  }
+
+  // set the state to the time right before that
+  this->filter_time = this->rewind_t.back();
+  this->x = this->rewind_states.back().first;
+  this->P = this->rewind_states.back().second;
+
+  return rewound;
+}
+
+void EKFSym::checkpoint(Observation& obs) {
+  // push to rewinder
+  this->rewind_t.push_back(this->filter_time);
+  this->rewind_states.push_back(std::make_pair(this->x, this->P));
+  this->rewind_obscache.push_back(obs);
+
+  // only keep a certain number around
+  if (this->rewind_t.size() > REWIND_TO_KEEP) {
+    this->rewind_t.pop_front();
+    this->rewind_states.pop_front();
+    this->rewind_obscache.pop_front();
+  }
+}
+
+Estimate EKFSym::predict_and_update_batch(Observation& obs, bool augment) {
+  assert(obs.z.size() == obs.R.size());
+  assert(obs.z.size() == obs.extra_args.size());
+
+  this->predict(obs.t);
+
+  Estimate res;
+  res.t = obs.t;
+  res.kind = obs.kind;
+  res.z = obs.z;
+  res.extra_args = obs.extra_args;
+  res.xk1 = this->x;
+  res.Pk1 = this->P;
+
+  // update batch
+  std::vector<VectorXd> y;
+  for (int i = 0; i < obs.z.size(); i++) {
+    assert(obs.z[i].rows() == obs.R[i].rows());
+    assert(obs.z[i].rows() == obs.R[i].cols());
+
+    // update state
+    y.push_back(this->update(obs.kind, obs.z[i], obs.R[i], obs.extra_args[i]));
+  }
+
+  res.xk = this->x;
+  res.Pk = this->P;
+  res.y = y;
+
+  assert(!augment); // TODO
+  // if (augment) {
+  //   this->augment();
+  // }
+
+  this->checkpoint(obs);
+
+  return res;
+}
+
+void EKFSym::predict(double t) {
+  // initialize time
+  if (std::isnan(this->filter_time)) {
+    this->filter_time = t;
+  }
+
+  // predict
+  double dt = t - this->filter_time;
+  assert(dt >= 0.0);
+
+  this->ekf->predict(this->x.data(), this->P.data(), this->Q.data(), dt);
+  this->normalize_quaternions();
+  this->filter_time = t;
+}
+
+VectorXd EKFSym::update(int kind, VectorXd z, MatrixXdr R, std::vector<double> extra_args) {
+  this->ekf->updates.at(kind)(this->x.data(), this->P.data(), z.data(), R.data(), extra_args.data());
+  this->normalize_quaternions();
+
+  if (this->msckf && std::find(this->feature_track_kinds.begin(), this->feature_track_kinds.end(), kind) != this->feature_track_kinds.end()) {
+    return z.head(z.rows() - extra_args.size());
+  }
+  return z;
+}
+
+extra_routine_t EKFSym::get_extra_routine(const std::string& routine) {
+  return this->ekf->extra_routines.at(routine);
+}

artifacts/package_sources/rednose/rednose/helpers/ekf_sym.h

@@ -0,0 +1,113 @@
+#pragma once
+
+#include <iostream>
+#include <cassert>
+#include <string>
+#include <vector>
+#include <deque>
+#include <unordered_map>
+#include <map>
+#include <cmath>
+#include <optional>
+
+#include <eigen3/Eigen/Dense>
+
+#include "ekf.h"
+#include "ekf_load.h"
+
+#define REWIND_TO_KEEP 512
+
+namespace EKFS {
+
+typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXdr;
+
+typedef struct Observation {
+  double t;
+  int kind;
+  std::vector<Eigen::VectorXd> z;
+  std::vector<MatrixXdr> R;
+  std::vector<std::vector<double>> extra_args;
+} Observation;
+
+typedef struct Estimate {
+  Eigen::VectorXd xk1;
+  Eigen::VectorXd xk;
+  MatrixXdr Pk1;
+  MatrixXdr Pk;
+  double t;
+  int kind;
+  std::vector<Eigen::VectorXd> y;
+  std::vector<Eigen::VectorXd> z;
+  std::vector<std::vector<double>> extra_args;
+} Estimate;
+
+class EKFSym {
+public:
+  EKFSym(std::string name, Eigen::Map<MatrixXdr> Q, Eigen::Map<Eigen::VectorXd> x_initial,
+      Eigen::Map<MatrixXdr> P_initial, int dim_main, int dim_main_err, int N = 0, int dim_augment = 0,
+      int dim_augment_err = 0, std::vector<int> maha_test_kinds = std::vector<int>(),
+      std::vector<int> quaternion_idxs = std::vector<int>(),
+      std::vector<std::string> global_vars = std::vector<std::string>(), double max_rewind_age = 1.0);
+  void init_state(Eigen::Map<Eigen::VectorXd> state, Eigen::Map<MatrixXdr> covs, double filter_time);
+
+  Eigen::VectorXd state();
+  MatrixXdr covs();
+  void set_filter_time(double t);
+  double get_filter_time();
+  void normalize_quaternions();
+  void normalize_slice(int slice_start, int slice_end_ex);
+  void set_global(std::string global_var, double val);
+  void reset_rewind();
+
+  void predict(double t);
+  std::optional<Estimate> predict_and_update_batch(double t, int kind, std::vector<Eigen::Map<Eigen::VectorXd>> z,
+      std::vector<Eigen::Map<MatrixXdr>> R, std::vector<std::vector<double>> extra_args = {{}}, bool augment = false);
+
+  extra_routine_t get_extra_routine(const std::string& routine);
+
+private:
+  std::deque<Observation> rewind(double t);
+  void checkpoint(Observation& obs);
+
+  Estimate predict_and_update_batch(Observation& obs, bool augment);
+  Eigen::VectorXd update(int kind, Eigen::VectorXd z, MatrixXdr R, std::vector<double> extra_args);
+
+  // stuct with linked sympy generated functions
+  const EKF *ekf = NULL;
+
+  Eigen::VectorXd x;  // state
+  MatrixXdr P;  // covs
+
+  bool msckf;
+  int N;
+  int dim_augment;
+  int dim_augment_err;
+  int dim_main;
+  int dim_main_err;
+
+  // state
+  int dim_x;
+  int dim_err;
+
+  double filter_time;
+
+  std::vector<int> maha_test_kinds;
+  std::vector<int> quaternion_idxs;
+
+  std::vector<std::string> global_vars;
+
+  // process noise
+  MatrixXdr Q;
+
+  // rewind stuff
+  double max_rewind_age;
+  std::deque<double> rewind_t;
+  std::deque<std::pair<Eigen::VectorXd, MatrixXdr>> rewind_states;
+  std::deque<Observation> rewind_obscache;
+
+  Eigen::VectorXd augment_times;
+
+  std::vector<int> feature_track_kinds;
+};
+
+}

artifacts/package_sources/rednose/rednose/helpers/ekf_sym.py

@@ -0,0 +1,690 @@
+import os
+import logging
+from bisect import bisect_right
+
+import numpy as np
+import sympy as sp
+from numpy import dot
+
+from rednose.helpers.sympy_helpers import sympy_into_c
+from rednose.helpers import TEMPLATE_DIR, load_code
+from rednose.helpers.chi2_lookup import chi2_ppf
+
+
+def solve(a, b):
+  if a.shape[0] == 1 and a.shape[1] == 1:
+    return b / a[0][0]
+  else:
+    return np.linalg.solve(a, b)
+
+
+def null(H, eps=1e-12):
+  _, s, vh = np.linalg.svd(H)
+  padding = max(0, np.shape(H)[1] - np.shape(s)[0])
+  null_mask = np.concatenate(((s <= eps), np.ones((padding,), dtype=bool)), axis=0)
+  null_space = np.compress(null_mask, vh, axis=0)
+  return np.transpose(null_space)
+
+
+def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None,  # pylint: disable=dangerous-default-value
+             maha_test_kinds=[], quaternion_idxs=[], global_vars=None, extra_routines=[]):
+  # optional state transition matrix, H modifier
+  # and err_function if an error-state kalman filter (ESKF)
+  # is desired. Best described in "Quaternion kinematics
+  # for the error-state Kalman filter" by Joan Sola
+
+  if eskf_params:
+    err_eqs = eskf_params[0]
+    inv_err_eqs = eskf_params[1]
+    H_mod_sym = eskf_params[2]
+    f_err_sym = eskf_params[3]
+    x_err_sym = eskf_params[4]
+  else:
+    nom_x = sp.MatrixSymbol('nom_x', dim_x, 1)
+    true_x = sp.MatrixSymbol('true_x', dim_x, 1)
+    delta_x = sp.MatrixSymbol('delta_x', dim_x, 1)
+    err_function_sym = sp.Matrix(nom_x + delta_x)
+    inv_err_function_sym = sp.Matrix(true_x - nom_x)
+    err_eqs = [err_function_sym, nom_x, delta_x]
+    inv_err_eqs = [inv_err_function_sym, nom_x, true_x]
+
+    H_mod_sym = sp.Matrix(np.eye(dim_x))
+    f_err_sym = f_sym
+    x_err_sym = x_sym
+
+  # This configures the multi-state augmentation
+  # needed for EKF-SLAM with MSCKF (Mourikis et al 2007)
+  if msckf_params:
+    msckf = True
+    dim_main = msckf_params[0]      # size of the main state
+    dim_augment = msckf_params[1]   # size of one augment state chunk
+    dim_main_err = msckf_params[2]
+    dim_augment_err = msckf_params[3]
+    N = msckf_params[4]
+    feature_track_kinds = msckf_params[5]
+    assert dim_main + dim_augment * N == dim_x
+    assert dim_main_err + dim_augment_err * N == dim_err
+  else:
+    msckf = False
+    dim_main = dim_x
+    dim_augment = 0
+    dim_main_err = dim_err
+    dim_augment_err = 0
+    N = 0
+
+  # linearize with jacobians
+  F_sym = f_err_sym.jacobian(x_err_sym)
+
+  if eskf_params:
+    for sym in x_err_sym:
+      F_sym = F_sym.subs(sym, 0)
+
+  assert dt_sym in F_sym.free_symbols
+
+  for i in range(len(obs_eqs)):
+    obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym))
+    if msckf and obs_eqs[i][1] in feature_track_kinds:
+      obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2]))
+    else:
+      obs_eqs[i].append(None)
+
+  # collect sympy functions
+  sympy_functions = []
+
+  # extra routines
+  sympy_functions += extra_routines
+
+  # 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
+  sympy_header, code = sympy_into_c(sympy_functions, global_vars)
+
+  header = "#pragma once\n"
+  header += "#include \"rednose/helpers/ekf.h\"\n"
+  header += "extern \"C\" {\n"
+
+  pre_code = f"#include \"{name}.h\"\n"
+  pre_code += "\nnamespace {\n"
+  pre_code += "#define DIM %d\n" % dim_x
+  pre_code += "#define EDIM %d\n" % dim_err
+  pre_code += "#define MEDIM %d\n" % dim_main_err
+  pre_code += "typedef void (*Hfun)(double *, double *, double *);\n"
+
+  if global_vars is not None:
+    for var in global_vars:
+      pre_code += f"\ndouble {var.name};\n"
+      pre_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n"
+
+  post_code = "\n}\n" # namespace
+  post_code += "extern \"C\" {\n\n"
+
+  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
+
+    pre_code += f"const static double MAHA_THRESH_{kind} = {maha_thresh};\n"
+
+    header += f"void {name}_update_{kind}(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);\n"
+    post_code += f"void {name}_update_{kind}(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {{\n"
+    post_code += f"  update<{h_sym.shape[0]}, 3, {int(maha_test)}>(in_x, in_P, h_{kind}, H_{kind}, {He_str}, in_z, in_R, in_ea, MAHA_THRESH_{kind});\n"
+    post_code += "}\n"
+
+  # For ffi loading of specific functions
+  for line in sympy_header.split("\n"):
+    if line.startswith("void "):  # sympy functions
+      func_call = line[5: line.index(')') + 1]
+      header += f"void {name}_{func_call};\n"
+      post_code += f"void {name}_{func_call} {{\n"
+      post_code += f"  {func_call.replace('double *', '').replace('double', '')};\n"
+      post_code += "}\n"
+  header += f"void {name}_predict(double *in_x, double *in_P, double *in_Q, double dt);\n"
+  post_code += f"void {name}_predict(double *in_x, double *in_P, double *in_Q, double dt) {{\n"
+  post_code += "  predict(in_x, in_P, in_Q, dt);\n"
+  post_code += "}\n"
+  if global_vars is not None:
+    for var in global_vars:
+      header += f"void {name}_set_{var.name}(double x);\n"
+      post_code += f"void {name}_set_{var.name}(double x) {{\n"
+      post_code += f"  set_{var.name}(x);\n"
+      post_code += "}\n"
+
+  post_code += "}\n\n" # extern c
+
+  funcs = ['f_fun', 'F_fun', 'err_fun', 'inv_err_fun', 'H_mod_fun', 'predict']
+  func_lists = {
+    'h': [kind for _, kind, _, _, _ in obs_eqs],
+    'H': [kind for _, kind, _, _, _ in obs_eqs],
+    'update': [kind for _, kind, _, _, _ in obs_eqs],
+    'He': [kind for _, kind, _, _, _ in obs_eqs if msckf and kind in feature_track_kinds],
+    'set': [var.name for var in global_vars] if global_vars is not None else [],
+  }
+  func_extra = [x[0] for x in extra_routines]
+
+  # For dynamic loading of specific functions
+  post_code += f"const EKF {name} = {{\n"
+  post_code += f"  .name = \"{name}\",\n"
+  post_code += f"  .kinds = {{ {', '.join([str(kind) for _, kind, _, _, _ in obs_eqs])} }},\n"
+  post_code += f"  .feature_kinds = {{ {', '.join([str(kind) for _, kind, _, _, _ in obs_eqs if msckf and kind in feature_track_kinds])} }},\n"
+  for func in funcs:
+    post_code += f"  .{func} = {name}_{func},\n"
+  for group, kinds in func_lists.items():
+    post_code += f"  .{group}s = {{\n"
+    for kind in kinds:
+      str_kind = f"\"{kind}\"" if isinstance(kind, str) else kind
+      post_code += f"    {{ {str_kind}, {name}_{group}_{kind} }},\n"
+    post_code += "  },\n"
+  post_code += "  .extra_routines = {\n"
+  for f in func_extra:
+    post_code += f"    {{ \"{f}\", {name}_{f} }},\n"
+  post_code += "  },\n"
+  post_code += "};\n\n"
+  post_code += f"ekf_lib_init({name})\n"
+
+  # merge code blocks
+  header += "}"
+  with open(os.path.join(TEMPLATE_DIR, "ekf_c.c"), encoding='utf-8') as f:
+    code = "\n".join([pre_code, code, f.read(), post_code])
+
+  # write to file
+  if not os.path.exists(folder):
+    os.mkdir(folder)
+
+  with open(os.path.join(folder, f"{name}.h"), 'w', encoding='utf-8') as f:
+    f.write(header)  # header is used for ffi import
+  with open(os.path.join(folder, f"{name}.cpp"), 'w', encoding='utf-8') as f:
+    f.write(code)
+
+
+class EKF_sym():
+  def __init__(self, folder, name, Q, x_initial, P_initial, dim_main, dim_main_err,  # pylint: disable=dangerous-default-value
+               N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], quaternion_idxs=[], global_vars=None, max_rewind_age=1.0, logger=logging):
+    """Generates process function and all observation functions for the kalman filter."""
+    self.msckf = N > 0
+    self.N = N
+    self.dim_augment = dim_augment
+    self.dim_augment_err = dim_augment_err
+    self.dim_main = dim_main
+    self.dim_main_err = dim_main_err
+
+    self.logger = logger
+
+    # state
+    x_initial = x_initial.reshape((-1, 1))
+    self.dim_x = x_initial.shape[0]
+    self.dim_err = P_initial.shape[0]
+    assert dim_main + dim_augment * N == self.dim_x
+    assert dim_main_err + dim_augment_err * N == self.dim_err
+    assert Q.shape == P_initial.shape
+
+    # kinds that should get mahalanobis distance
+    # tested for outlier rejection
+    self.maha_test_kinds = maha_test_kinds
+
+    # quaternions need normalization
+    self.quaternion_idxs = quaternion_idxs
+
+    # process noise
+    self.Q = Q
+
+    # rewind stuff
+    self.max_rewind_age = max_rewind_age
+    self.rewind_t = []
+    self.rewind_states = []
+    self.rewind_obscache = []
+    self.init_state(x_initial, P_initial, None)
+
+    ffi, lib = load_code(folder, name)
+    kinds, self.feature_track_kinds = [], []
+    for func in dir(lib):
+      if func[:len(name) + 3] == f'{name}_h_':
+        kinds.append(int(func[len(name) + 3:]))
+      if func[:len(name) + 4] == f'{name}_He_':
+        self.feature_track_kinds.append(int(func[len(name) + 4:]))
+
+    # wrap all the sympy functions
+    def wrap_1lists(func_name):
+      func = eval(f"lib.{name}_{func_name}", {"lib": lib})  # pylint: disable=eval-used
+
+      def ret(lst1, out):
+        func(ffi.cast("double *", lst1.ctypes.data),
+             ffi.cast("double *", out.ctypes.data))
+      return ret
+
+    def wrap_2lists(func_name):
+      func = eval(f"lib.{name}_{func_name}", {"lib": lib})  # pylint: disable=eval-used
+
+      def ret(lst1, lst2, out):
+        func(ffi.cast("double *", lst1.ctypes.data),
+             ffi.cast("double *", lst2.ctypes.data),
+             ffi.cast("double *", out.ctypes.data))
+      return ret
+
+    def wrap_1list_1float(func_name):
+      func = eval(f"lib.{name}_{func_name}", {"lib": lib})  # pylint: disable=eval-used
+
+      def ret(lst1, fl, out):
+        func(ffi.cast("double *", lst1.ctypes.data),
+             ffi.cast("double", fl),
+             ffi.cast("double *", out.ctypes.data))
+      return ret
+
+    self.f = wrap_1list_1float("f_fun")
+    self.F = wrap_1list_1float("F_fun")
+
+    self.err_function = wrap_2lists("err_fun")
+    self.inv_err_function = wrap_2lists("inv_err_fun")
+    self.H_mod = wrap_1lists("H_mod_fun")
+
+    self.hs, self.Hs, self.Hes = {}, {}, {}
+    for kind in kinds:
+      self.hs[kind] = wrap_2lists(f"h_{kind}")
+      self.Hs[kind] = wrap_2lists(f"H_{kind}")
+      if self.msckf and kind in self.feature_track_kinds:
+        self.Hes[kind] = wrap_2lists(f"He_{kind}")
+
+    self.set_globals = {}
+    if global_vars is not None:
+      for global_var in global_vars:
+        self.set_globals[global_var] = getattr(lib, f"{name}_set_{global_var}")
+
+    # wrap the C++ predict function
+    def _predict_blas(x, P, dt):
+      func = eval(f"lib.{name}_predict", {"lib": lib})  # pylint: disable=eval-used
+      func(ffi.cast("double *", x.ctypes.data),
+           ffi.cast("double *", P.ctypes.data),
+           ffi.cast("double *", self.Q.ctypes.data),
+           ffi.cast("double", dt))
+      return x, P
+
+    # wrap the C++ update function
+    def fun_wrapper(f, kind):
+      f = eval(f"lib.{name}_{f}", {"lib": lib})  # pylint: disable=eval-used
+
+      def _update_inner_blas(x, P, z, R, extra_args):
+        f(ffi.cast("double *", x.ctypes.data),
+          ffi.cast("double *", P.ctypes.data),
+          ffi.cast("double *", z.ctypes.data),
+          ffi.cast("double *", R.ctypes.data),
+          ffi.cast("double *", extra_args.ctypes.data))
+        if self.msckf and kind in self.feature_track_kinds:
+          y = z[:-len(extra_args)]
+        else:
+          y = z
+        return x, P, y
+      return _update_inner_blas
+
+    self._updates = {}
+    for kind in kinds:
+      self._updates[kind] = fun_wrapper("update_%d" % kind, kind)
+
+    def _update_blas(x, P, kind, z, R, extra_args=[]):  # pylint: disable=dangerous-default-value
+        return self._updates[kind](x, P, z, R, extra_args)
+
+    # assign the functions
+    self._predict = _predict_blas
+    # self._predict = self._predict_python
+    self._update = _update_blas
+    # self._update = self._update_python
+
+  def init_state(self, state, covs, filter_time):
+    self.x = np.array(state.reshape((-1, 1))).astype(np.float64)
+    self.P = np.array(covs).astype(np.float64)
+    self.filter_time = filter_time
+    self.augment_times = [0] * self.N
+    self.rewind_obscache = []
+    self.rewind_t = []
+    self.rewind_states = []
+
+  def reset_rewind(self):
+    self.rewind_obscache = []
+    self.rewind_t = []
+    self.rewind_states = []
+
+  def augment(self):
+    # TODO this is not a generalized way of doing this and implies that the augmented states
+    # are simply the first (dim_augment_state) elements of the main state.
+    assert self.msckf
+    d1 = self.dim_main
+    d2 = self.dim_main_err
+    d3 = self.dim_augment
+    d4 = self.dim_augment_err
+
+    # push through augmented states
+    self.x[d1:-d3] = self.x[d1 + d3:]
+    self.x[-d3:] = self.x[:d3]
+    assert self.x.shape == (self.dim_x, 1)
+
+    # push through augmented covs
+    assert self.P.shape == (self.dim_err, self.dim_err)
+    P_reduced = self.P
+    P_reduced = np.delete(P_reduced, np.s_[d2:d2 + d4], axis=1)
+    P_reduced = np.delete(P_reduced, np.s_[d2:d2 + d4], axis=0)
+    assert P_reduced.shape == (self.dim_err - d4, self.dim_err - d4)
+    to_mult = np.zeros((self.dim_err, self.dim_err - d4))
+    to_mult[:-d4, :] = np.eye(self.dim_err - d4)
+    to_mult[-d4:, :d4] = np.eye(d4)
+    self.P = to_mult.dot(P_reduced.dot(to_mult.T))
+    self.augment_times = self.augment_times[1:]
+    self.augment_times.append(self.filter_time)
+    assert self.P.shape == (self.dim_err, self.dim_err)
+
+  def state(self):
+    return np.array(self.x).flatten()
+
+  def covs(self):
+    return self.P
+
+  def set_filter_time(self, t):
+    self.filter_time = t
+
+  def get_filter_time(self):
+    return self.filter_time
+
+  def normalize_quaternions(self):
+    for idx in self.quaternion_idxs:
+      self.normalize_slice(idx, idx+4)
+
+  def normalize_slice(self, slice_start, slice_end_ex):
+    self.x[slice_start:slice_end_ex] /= np.linalg.norm(self.x[slice_start:slice_end_ex])
+
+  def get_augment_times(self):
+    return self.augment_times
+
+  def set_global(self, global_var, val):
+    self.set_globals[global_var](val)
+
+  def rewind(self, t):
+    # find where we are rewinding to
+    idx = bisect_right(self.rewind_t, t)
+    assert self.rewind_t[idx - 1] <= t
+    assert self.rewind_t[idx] > t    # must be true, or rewind wouldn't be called
+
+    # set the state to the time right before that
+    self.filter_time = self.rewind_t[idx - 1]
+    self.x[:] = self.rewind_states[idx - 1][0]
+    self.P[:] = self.rewind_states[idx - 1][1]
+
+    # return the observations we rewound over for fast forwarding
+    ret = self.rewind_obscache[idx:]
+
+    # throw away the old future
+    # TODO: is this making a copy?
+    self.rewind_t = self.rewind_t[:idx]
+    self.rewind_states = self.rewind_states[:idx]
+    self.rewind_obscache = self.rewind_obscache[:idx]
+
+    return ret
+
+  def checkpoint(self, obs):
+    # push to rewinder
+    self.rewind_t.append(self.filter_time)
+    self.rewind_states.append((np.copy(self.x), np.copy(self.P)))
+    self.rewind_obscache.append(obs)
+
+    # only keep a certain number around
+    REWIND_TO_KEEP = 512
+    self.rewind_t = self.rewind_t[-REWIND_TO_KEEP:]
+    self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:]
+    self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:]
+
+  def predict(self, t):
+    # initialize time
+    if self.filter_time is None:
+      self.filter_time = t
+
+    # predict
+    dt = t - self.filter_time
+    assert dt >= 0
+    self.x, self.P = self._predict(self.x, self.P, dt)
+    self.normalize_quaternions()
+    self.filter_time = t
+
+  def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False):  # pylint: disable=dangerous-default-value
+    # TODO handle rewinding at this level"
+
+    # rewind
+    if self.filter_time is not None and t < self.filter_time:
+      if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] - self.max_rewind_age:
+        self.logger.error(f"observation too old at {t:.3f} with filter at {self.filter_time:.3f}, ignoring")
+        return None
+      rewound = self.rewind(t)
+    else:
+      rewound = []
+
+    ret = self._predict_and_update_batch(t, kind, z, R, extra_args, augment)
+
+    # optional fast forward
+    for r in rewound:
+      self._predict_and_update_batch(*r)
+
+    return ret
+
+  def _predict_and_update_batch(self, t, kind, z, R, extra_args, augment=False):
+    """The main kalman filter function
+    Predicts the state and then updates a batch of observations
+    dim_x: dimensionality of the state space
+    dim_z: dimensionality of the observation and depends on kind
+    n: number of observations
+    Args:
+      t                 (float): Time of observation
+      kind                (int): Type of observation
+      z         (vec [n,dim_z]): Measurements
+      R  (mat [n,dim_z, dim_z]): Measurement Noise
+      extra_args    (list, [n]): Values used in H computations
+    """
+    assert z.shape[0] == R.shape[0]
+    assert z.shape[1] == R.shape[1]
+    assert z.shape[1] == R.shape[2]
+
+    # initialize time
+    if self.filter_time is None:
+      self.filter_time = t
+
+    # predict
+    dt = t - self.filter_time
+    assert dt >= 0
+    self.x, self.P = self._predict(self.x, self.P, dt)
+    self.filter_time = t
+    xk_km1, Pk_km1 = np.copy(self.x).flatten(), np.copy(self.P)
+
+    # update batch
+    y = []
+    for i in range(len(z)):
+      # these are from the user, so we canonicalize them
+      z_i = np.array(z[i], dtype=np.float64, order='F')
+      R_i = np.array(R[i], dtype=np.float64, order='F')
+      extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F')
+      # update
+      self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i)
+      self.normalize_quaternions()
+      y.append(y_i)
+    xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P)
+
+    if augment:
+      self.augment()
+
+    # checkpoint
+    self.checkpoint((t, kind, z, R, extra_args))
+
+    return xk_km1, xk_k, Pk_km1, Pk_k, t, kind, y, z, extra_args
+
+  def _predict_python(self, x, P, dt):
+    x_new = np.zeros(x.shape, dtype=np.float64)
+    self.f(x, dt, x_new)
+
+    F = np.zeros(P.shape, dtype=np.float64)
+    self.F(x, dt, F)
+
+    if not self.msckf:
+      P = dot(dot(F, P), F.T)
+    else:
+      # Update the predicted state covariance:
+      #  Pk+1|k   =  |F*Pii*FT + Q*dt   F*Pij |
+      #              |PijT*FT           Pjj   |
+      # Where F is the jacobian of the main state
+      # predict function, Pii is the main state's
+      # covariance and Q its process noise. Pij
+      # is the covariance between the augmented
+      # states and the main state.
+      #
+      d2 = self.dim_main_err    # known at compile time
+      F_curr = F[:d2, :d2]
+      P[:d2, :d2] = (F_curr.dot(P[:d2, :d2])).dot(F_curr.T)
+      P[:d2, d2:] = F_curr.dot(P[:d2, d2:])
+      P[d2:, :d2] = P[d2:, :d2].dot(F_curr.T)
+
+    P += dt * self.Q
+    return x_new, P
+
+  def _update_python(self, x, P, kind, z, R, extra_args=[]):  # pylint: disable=dangerous-default-value
+    # init vars
+    z = z.reshape((-1, 1))
+    h = np.zeros(z.shape, dtype=np.float64)
+    H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
+
+    # C functions
+    self.hs[kind](x, extra_args, h)
+    self.Hs[kind](x, extra_args, H)
+
+    # y is the "loss"
+    y = z - h
+
+    # *** same above this line ***
+
+    if self.msckf and kind in self.Hes:
+      # Do some algebraic magic to decorrelate
+      He = np.zeros((z.shape[0], len(extra_args)), dtype=np.float64)
+      self.Hes[kind](x, extra_args, He)
+
+      # TODO: Don't call a function here, do projection locally
+      A = null(He.T)
+
+      y = A.T.dot(y)
+      H = A.T.dot(H)
+      R = A.T.dot(R.dot(A))
+
+      # TODO If nullspace isn't the dimension we want
+      if A.shape[1] + He.shape[1] != A.shape[0]:
+        self.logger.warning('Warning: null space projection failed, measurement ignored')
+        return x, P, np.zeros(A.shape[0] - He.shape[1])
+
+    # if using eskf
+    H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
+    self.H_mod(x, H_mod)
+    H = H.dot(H_mod)
+
+    # Do mahalobis distance test
+    # currently just runs on msckf observations
+    # could run on anything if needed
+    if self.msckf and kind in self.maha_test_kinds:
+      a = np.linalg.inv(H.dot(P).dot(H.T) + R)
+      maha_dist = y.T.dot(a.dot(y))
+      if maha_dist > chi2_ppf(0.95, y.shape[0]):
+        R = 10e16 * R
+
+    # *** same below this line ***
+
+    # Outlier resilient weighting as described in:
+    # "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..."
+    weight = 1  # (1.5)/(1 + np.sum(y**2)/np.sum(R))
+
+    S = dot(dot(H, P), H.T) + R / weight
+    K = solve(S, dot(H, P.T)).T
+    I_KH = np.eye(P.shape[0]) - dot(K, H)
+
+    # update actual state
+    delta_x = dot(K, y)
+    P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T)
+
+    # inject observed error into state
+    x_new = np.zeros(x.shape, dtype=np.float64)
+    self.err_function(x, delta_x, x_new)
+    return x_new, P, y.flatten()
+
+  def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95):  # pylint: disable=dangerous-default-value
+    # init vars
+    z = z.reshape((-1, 1))
+    h = np.zeros(z.shape, dtype=np.float64)
+    H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
+
+    # C functions
+    self.hs[kind](x, extra_args, h)
+    self.Hs[kind](x, extra_args, H)
+
+    # y is the "loss"
+    y = z - h
+
+    # if using eskf
+    H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
+    self.H_mod(x, H_mod)
+    H = H.dot(H_mod)
+
+    a = np.linalg.inv(H.dot(P).dot(H.T) + R)
+    maha_dist = y.T.dot(a.dot(y))
+    if maha_dist > chi2_ppf(maha_thresh, y.shape[0]):
+      return False
+    else:
+      return True
+
+  def rts_smooth(self, estimates, norm_quats=False):
+    '''
+    Returns rts smoothed results of
+    kalman filter estimates
+    If the kalman state is augmented with
+    old states only the main state is smoothed
+    '''
+    xk_n = estimates[-1][0]
+    Pk_n = estimates[-1][2]
+    Fk_1 = np.zeros(Pk_n.shape, dtype=np.float64)
+
+    states_smoothed = [xk_n]
+    covs_smoothed = [Pk_n]
+    for k in range(len(estimates) - 2, -1, -1):
+      xk1_n = xk_n
+      if norm_quats:
+        xk1_n[3:7] /= np.linalg.norm(xk1_n[3:7])
+      Pk1_n = Pk_n
+
+      xk1_k, _, Pk1_k, _, t2, _, _, _, _ = estimates[k + 1]
+      _, xk_k, _, Pk_k, t1, _, _, _, _ = estimates[k]
+      dt = t2 - t1
+      self.F(xk_k, dt, Fk_1)
+
+      d1 = self.dim_main
+      d2 = self.dim_main_err
+      Ck = np.linalg.solve(Pk1_k[:d2, :d2], Fk_1[:d2, :d2].dot(Pk_k[:d2, :d2].T)).T
+      xk_n = xk_k
+      delta_x = np.zeros((Pk_n.shape[0], 1), dtype=np.float64)
+      self.inv_err_function(xk1_k, xk1_n, delta_x)
+      delta_x[:d2] = Ck.dot(delta_x[:d2])
+      x_new = np.zeros((xk_n.shape[0], 1), dtype=np.float64)
+      self.err_function(xk_k, delta_x, x_new)
+      xk_n[:d1] = x_new[:d1, 0]
+      Pk_n = Pk_k
+      Pk_n[:d2, :d2] = Pk_k[:d2, :d2] + Ck.dot(Pk1_n[:d2, :d2] - Pk1_k[:d2, :d2]).dot(Ck.T)
+      states_smoothed.append(xk_n)
+      covs_smoothed.append(Pk_n)
+
+    return np.flipud(np.vstack(states_smoothed)), np.stack(covs_smoothed, 0)[::-1]

artifacts/package_sources/rednose/rednose/helpers/ekf_sym_pyx.pyx

@@ -0,0 +1,195 @@
+# cython: language_level=3
+# cython: profile=True
+# distutils: language = c++
+
+cimport cython
+
+from libcpp.string cimport string
+from libcpp.vector cimport vector
+from libcpp cimport bool
+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)
+
+  cdef cppclass MapMatrixXdr "Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >":
+    MapMatrixXdr(double*, int, int)
+
+  cdef cppclass VectorXd "Eigen::VectorXd":
+    VectorXd()
+    double* data()
+    int rows()
+
+  cdef cppclass MatrixXdr "Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>":
+    MatrixXdr()
+    double* data()
+    int rows()
+    int cols()
+
+  ctypedef struct Estimate:
+    VectorXd xk1
+    VectorXd xk
+    MatrixXdr Pk1
+    MatrixXdr Pk
+    double t
+    int kind
+    vector[VectorXd] y
+    vector[VectorXd] z
+    vector[vector[double]] extra_args
+
+  cdef cppclass EKFSym:
+    EKFSym(string name, MapMatrixXdr Q, MapVectorXd x_initial, MapMatrixXdr P_initial, int dim_main,
+        int dim_main_err, int N, int dim_augment, int dim_augment_err, vector[int] maha_test_kinds,
+        vector[int] quaternion_idxs, vector[string] global_vars, double max_rewind_age)
+    void init_state(MapVectorXd state, MapMatrixXdr covs, double filter_time)
+
+    VectorXd state()
+    MatrixXdr covs()
+    void set_filter_time(double t)
+    double get_filter_time()
+    void set_global(string name, double val)
+    void reset_rewind()
+
+    void predict(double t)
+    optional[Estimate] predict_and_update_batch(double t, int kind, vector[MapVectorXd] z, vector[MapMatrixXdr] z,
+        vector[vector[double]] extra_args, bool augment)
+
+# Functions like `numpy_to_matrix` are not possible, cython requires default
+# constructor for return variable types which aren't available with Eigen::Map
+
+@cython.wraparound(False)
+@cython.boundscheck(False)
+cdef np.ndarray[np.float64_t, ndim=2, mode="c"] matrix_to_numpy(MatrixXdr arr):
+  cdef double[:,:] mem_view = <double[:arr.rows(),:arr.cols()]>arr.data()
+  return np.copy(np.asarray(mem_view, dtype=np.double, order="C"))
+
+@cython.wraparound(False)
+@cython.boundscheck(False)
+cdef np.ndarray[np.float64_t, ndim=1, mode="c"] vector_to_numpy(VectorXd arr):
+  cdef double[:] mem_view = <double[:arr.rows()]>arr.data()
+  return np.copy(np.asarray(mem_view, dtype=np.double, order="C"))
+
+cdef class EKF_sym_pyx:
+  cdef EKFSym* ekf
+  def __cinit__(self, str gen_dir, str name, np.ndarray[np.float64_t, ndim=2] Q,
+      np.ndarray[np.float64_t, ndim=1] x_initial, np.ndarray[np.float64_t, ndim=2] P_initial, int dim_main,
+      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)
+    cdef np.ndarray[np.float64_t, ndim=2, mode='c'] P_initial_b = np.ascontiguousarray(P_initial, dtype=np.double)
+    self.ekf = new EKFSym(
+      name.encode('utf8'),
+      MapMatrixXdr(<double*> Q_b.data, Q.shape[0], Q.shape[1]),
+      MapVectorXd(<double*> x_initial_b.data, x_initial.shape[0]),
+      MapMatrixXdr(<double*> P_initial_b.data, P_initial.shape[0], P_initial.shape[1]),
+      dim_main,
+      dim_main_err,
+      N,
+      dim_augment,
+      dim_augment_err,
+      maha_test_kinds,
+      quaternion_idxs,
+      [x.encode('utf8') for x in global_vars],
+      max_rewind_age
+    )
+
+  def init_state(self, np.ndarray[np.float64_t, ndim=1] state, np.ndarray[np.float64_t, ndim=2] covs, filter_time):
+    cdef np.ndarray[np.float64_t, ndim=1, mode='c'] state_b = np.ascontiguousarray(state, dtype=np.double)
+    cdef np.ndarray[np.float64_t, ndim=2, mode='c'] covs_b = np.ascontiguousarray(covs, dtype=np.double)
+    self.ekf.init_state(
+      MapVectorXd(<double*> state_b.data, state.shape[0]),
+      MapMatrixXdr(<double*> covs_b.data, covs.shape[0], covs.shape[1]),
+      np.nan if filter_time is None else filter_time
+    )
+
+  def state(self):
+    cdef np.ndarray res = vector_to_numpy(self.ekf.state())
+    return res
+
+  def covs(self):
+    return matrix_to_numpy(self.ekf.covs())
+
+  def set_filter_time(self, double t):
+    self.ekf.set_filter_time(t)
+
+  def get_filter_time(self):
+    return self.ekf.get_filter_time()
+
+  def set_global(self, str global_var, double val):
+    self.ekf.set_global(global_var.encode('utf8'), val)
+
+  def reset_rewind(self):
+    self.ekf.reset_rewind()
+
+  def predict(self, double t):
+    self.ekf.predict(t)
+
+  def predict_and_update_batch(self, double t, int kind, z, R, extra_args=[[]], bool augment=False):
+    cdef vector[MapVectorXd] z_map
+    cdef np.ndarray[np.float64_t, ndim=1, mode='c'] zi_b
+    for zi in z:
+      zi_b = np.ascontiguousarray(zi, dtype=np.double)
+      z_map.push_back(MapVectorXd(<double*> zi_b.data, zi.shape[0]))
+
+    cdef vector[MapMatrixXdr] R_map
+    cdef np.ndarray[np.float64_t, ndim=2, mode='c'] Ri_b
+    for Ri in R:
+      Ri_b = np.ascontiguousarray(Ri, dtype=np.double)
+      R_map.push_back(MapMatrixXdr(<double*> Ri_b.data, Ri.shape[0], Ri.shape[1]))
+
+    cdef vector[vector[double]] extra_args_map
+    cdef vector[double] args_map
+    for args in extra_args:
+      args_map.clear()
+      for a in args:
+        args_map.push_back(a)
+      extra_args_map.push_back(args_map)
+
+    cdef optional[Estimate] res = self.ekf.predict_and_update_batch(t, kind, z_map, R_map, extra_args_map, augment)
+    if not res.has_value():
+      return None
+
+    cdef VectorXd tmpvec
+    return (
+      vector_to_numpy(res.value().xk1),
+      vector_to_numpy(res.value().xk),
+      matrix_to_numpy(res.value().Pk1),
+      matrix_to_numpy(res.value().Pk),
+      res.value().t,
+      res.value().kind,
+      [vector_to_numpy(tmpvec) for tmpvec in res.value().y],
+      z,  # TODO: take return values?
+      extra_args,
+    )
+
+  def augment(self):
+    raise NotImplementedError()  # TODO
+
+  def get_augment_times(self):
+    raise NotImplementedError()  # TODO
+
+  def rts_smooth(self, estimates, norm_quats=False):
+    raise NotImplementedError()  # TODO
+
+  def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95):
+    raise NotImplementedError()  # TODO
+
+  def __dealloc__(self):
+    del self.ekf

artifacts/package_sources/rednose/rednose/helpers/kalmanfilter.py

@@ -0,0 +1,52 @@
+from typing import Any
+
+import numpy as np
+
+
+class KalmanFilter:
+  name = "<name>"
+  initial_x = np.zeros((0, 0))
+  initial_P_diag = np.zeros((0, 0))
+  Q = np.zeros((0, 0))
+  obs_noise: dict[int, Any] = {}
+
+  # Should be initialized when initializating a KalmanFilter implementation
+  filter = None
+
+  @property
+  def x(self):
+    return self.filter.state()
+
+  @property
+  def t(self):
+    return self.filter.get_filter_time()
+
+  @property
+  def P(self):
+    return self.filter.covs()
+
+  def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
+    if covs_diag is not None:
+      P = np.diag(covs_diag)
+    elif covs is not None:
+      P = covs
+    else:
+      P = self.filter.covs()
+    self.filter.init_state(state, P, filter_time)
+
+  def get_R(self, kind, n):
+    obs_noise = self.obs_noise[kind]
+    dim = obs_noise.shape[0]
+    R = np.zeros((n, dim, dim))
+    for i in range(n):
+      R[i, :, :] = obs_noise
+    return R
+
+  def predict_and_observe(self, t, kind, data, R=None):
+    if len(data) > 0:
+      data = np.atleast_2d(data)
+
+    if R is None:
+      R = self.get_R(kind, len(data))
+
+    self.filter.predict_and_update_batch(t, kind, data, R)

artifacts/package_sources/rednose/rednose/helpers/sympy_helpers.py

@@ -0,0 +1,162 @@
+import sympy as sp
+import numpy as np
+
+# TODO: remove code duplication between openpilot.common.orientation
+def quat2rot(quats):
+  quats = np.array(quats)
+  input_shape = quats.shape
+  quats = np.atleast_2d(quats)
+  Rs = np.zeros((quats.shape[0], 3, 3))
+  q0 = quats[:, 0]
+  q1 = quats[:, 1]
+  q2 = quats[:, 2]
+  q3 = quats[:, 3]
+  Rs[:, 0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
+  Rs[:, 0, 1] = 2 * (q1 * q2 - q0 * q3)
+  Rs[:, 0, 2] = 2 * (q0 * q2 + q1 * q3)
+  Rs[:, 1, 0] = 2 * (q1 * q2 + q0 * q3)
+  Rs[:, 1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3
+  Rs[:, 1, 2] = 2 * (q2 * q3 - q0 * q1)
+  Rs[:, 2, 0] = 2 * (q1 * q3 - q0 * q2)
+  Rs[:, 2, 1] = 2 * (q0 * q1 + q2 * q3)
+  Rs[:, 2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
+
+  if len(input_shape) < 2:
+    return Rs[0]
+  else:
+    return Rs
+
+
+def euler2quat(eulers):
+  eulers = np.array(eulers)
+  if len(eulers.shape) > 1:
+    output_shape = (-1,4)
+  else:
+    output_shape = (4,)
+  eulers = np.atleast_2d(eulers)
+  gamma, theta, psi = eulers[:,0],  eulers[:,1],  eulers[:,2]
+
+  q0 = np.cos(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q1 = np.sin(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) - \
+       np.cos(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q2 = np.cos(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2)
+  q3 = np.cos(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2) - \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2)
+
+  quats = np.array([q0, q1, q2, q3]).T
+  for i in range(len(quats)):
+    if quats[i,0] < 0:  # pylint: disable=unsubscriptable-object
+      quats[i] = -quats[i]  # pylint: disable=unsupported-assignment-operation,unsubscriptable-object
+  return quats.reshape(output_shape)
+
+
+def euler2rot(eulers):
+  return quat2rot(euler2quat(eulers))
+
+
+rotations_from_quats = quat2rot
+
+
+def cross(x):
+  ret = sp.Matrix(np.zeros((3, 3)))
+  ret[0, 1], ret[0, 2] = -x[2], x[1]
+  ret[1, 0], ret[1, 2] = x[2], -x[0]
+  ret[2, 0], ret[2, 1] = -x[1], x[0]
+  return ret
+
+
+def rot_to_euler(R):
+  gamma = sp.atan2(R[2, 1], R[2, 2])
+  theta = sp.asin(-R[2, 0])
+  psi = sp.atan2(R[1, 0], R[0, 0])
+  return sp.Matrix([gamma, theta, psi])
+
+
+def rot_matrix(roll, pitch, yaw):
+  cr, sr = np.cos(roll), np.sin(roll)
+  cp, sp = np.cos(pitch), np.sin(pitch)
+  cy, sy = np.cos(yaw), np.sin(yaw)
+  rr = np.array([[1,0,0],[0, cr,-sr],[0, sr, cr]])
+  rp = np.array([[cp,0,sp],[0, 1,0],[-sp, 0, cp]])
+  ry = np.array([[cy,-sy,0],[sy, cy,0],[0, 0, 1]])
+  return ry.dot(rp.dot(rr))
+
+
+def euler_rotate(roll, pitch, yaw):
+  # make symbolic rotation matrix from eulers
+  matrix_roll = sp.Matrix([[1, 0, 0],
+                           [0, sp.cos(roll), -sp.sin(roll)],
+                           [0, sp.sin(roll), sp.cos(roll)]])
+  matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)],
+                            [0, 1, 0],
+                            [-sp.sin(pitch), 0, sp.cos(pitch)]])
+  matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0],
+                          [sp.sin(yaw), sp.cos(yaw), 0],
+                          [0, 0, 1]])
+  return matrix_yaw * matrix_pitch * matrix_roll
+
+
+def quat_rotate(q0, q1, q2, q3):
+  # make symbolic rotation matrix from quat
+  return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 + q0 * q3), 2 * (q1 * q3 - q0 * q2)],
+                    [2 * (q1 * q2 - q0 * q3), q0**2 - q1**2 + q2**2 - q3**2, 2 * (q2 * q3 + q0 * q1)],
+                    [2 * (q1 * q3 + q0 * q2), 2 * (q2 * q3 - q0 * q1), q0**2 - q1**2 - q2**2 + q3**2]]).T
+
+
+def quat_matrix_l(p):
+  return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
+                    [p[1],  p[0], -p[3],  p[2]],
+                    [p[2],  p[3],  p[0], -p[1]],
+                    [p[3], -p[2],  p[1],  p[0]]])
+
+
+def quat_matrix_r(p):
+  return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
+                    [p[1],  p[0],  p[3], -p[2]],
+                    [p[2], -p[3],  p[0],  p[1]],
+                    [p[3],  p[2], -p[1],  p[0]]])
+
+
+def sympy_into_c(sympy_functions, global_vars=None):
+  from sympy.utilities import codegen
+  routines = []
+  for name, expr, args in sympy_functions:
+    r = codegen.make_routine(name, expr, language="C99", global_vars=global_vars)
+
+    # argument ordering input to sympy is broken with function with output arguments
+    nargs = []
+
+    # reorder the input arguments
+    for aa in args:
+      if aa is None:
+        nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1, 1]))
+        continue
+      found = False
+      for a in r.arguments:
+        if str(aa.name) == str(a.name):
+          nargs.append(a)
+          found = True
+          break
+      if not found:
+        # [1,1] is a hack for Matrices
+        nargs.append(codegen.InputArgument(aa, dimensions=[1, 1]))
+
+    # add the output arguments
+    for a in r.arguments:
+      if type(a) == codegen.OutputArgument:
+        nargs.append(a)
+
+    # assert len(r.arguments) == len(args)+1
+    r.arguments = nargs
+
+    # add routine to list
+    routines.append(r)
+
+  [(_, c_code), (_, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf")
+  c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
+
+  c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#')
+
+  return c_header, c_code

artifacts/package_sources/rednose/rednose/logger/logger.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#ifdef SWAGLOG
+#include SWAGLOG
+#else
+
+#define CLOUDLOG_DEBUG 10
+#define CLOUDLOG_INFO 20
+#define CLOUDLOG_WARNING 30
+#define CLOUDLOG_ERROR 40
+#define CLOUDLOG_CRITICAL 50
+
+#define cloudlog(lvl, fmt, ...) printf(fmt "\n", ## __VA_ARGS__)
+
+#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
+#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
+#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
+#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
+
+#endif

artifacts/package_sources/rednose/rednose/site_scons/site_tools/rednose_filter.py

@@ -0,0 +1,40 @@
+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)
+  action = f"{File(generator_file).relpath} {target} {Dir(output_dir).relpath}"
+  if hasattr(env, 'PrettyAction'):
+    action = env.PrettyAction(action, 'GEN')
+  env.Command(generated_src_files + extra_generated_files, [generator_file] + script_deps, action)
+  return File(generated_src_files[:1])
+
+
+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"]
+    return env.SharedLibrary(f'{output_dir}/{target}', [self.base_cc_deps, objects], LINKFLAGS=linker_flags)
+
+
+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")
+  env.AddMethod(CompileFilterMethod(templates + [sympy_helpers, ekf_sym], []), "RednoseCompileFilter")
+
+
+def exists(env):
+  return True

artifacts/package_sources/rednose/rednose/templates/compute_pos.c

@@ -0,0 +1,52 @@
+#include <eigen3/Eigen/QR>
+#include <eigen3/Eigen/Dense>
+#include <iostream>
+
+typedef Eigen::Matrix<double, KDIM*2, 3, Eigen::RowMajor> R3M;
+typedef Eigen::Matrix<double, KDIM*2, 1> R1M;
+typedef Eigen::Matrix<double, 3, 1> O1M;
+typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> M3D;
+
+void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) {
+
+  double res[KDIM*2] = {0};
+  double jac[KDIM*6] = {0};
+
+  O1M x(in_x);
+  O1M delta;
+  int counter = 0;
+  while ((delta.squaredNorm() > 0.0001 and counter < 30) or counter == 0){
+    res_fun(in_x, in_poses, in_img_positions, res);
+    jac_fun(in_x, in_poses, in_img_positions, jac);
+    R1M E(res); R3M J(jac);
+    delta = (J.transpose()*J).inverse() * J.transpose() * E;
+    x = x - delta;
+    memcpy(in_x, x.data(), 3 * sizeof(double));
+    counter = counter + 1;
+  }
+}
+
+
+void compute_pos(double *to_c, double *poses, double *img_positions, double *param, double *pos) {
+    param[0] = img_positions[KDIM*2-2];
+    param[1] = img_positions[KDIM*2-1];
+    param[2] = 0.1;
+    gauss_newton(param, poses, img_positions);
+
+    Eigen::Quaterniond q;
+    q.w() = poses[KDIM*7-4];
+    q.x() = poses[KDIM*7-3];
+    q.y() = poses[KDIM*7-2];
+    q.z() = poses[KDIM*7-1];
+    M3D RC(to_c);
+    Eigen::Matrix3d R = q.normalized().toRotationMatrix();
+    Eigen::Matrix3d rot = R * RC.transpose();
+
+    pos[0] = param[0]/param[2];
+    pos[1] = param[1]/param[2];
+    pos[2] = 1.0/param[2];
+    O1M ecef_offset(poses + KDIM*7-7);
+    O1M ecef_output(pos);
+    ecef_output = rot*ecef_output + ecef_offset;
+    memcpy(pos, ecef_output.data(), 3 * sizeof(double));
+}

artifacts/package_sources/rednose/rednose/templates/ekf_c.c

@@ -0,0 +1,123 @@
+#include <eigen3/Eigen/Dense>
+#include <iostream>
+
+typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM;
+typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM;
+typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM;
+
+void predict(double *in_x, double *in_P, double *in_Q, double dt) {
+  typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> RRM;
+
+  double nx[DIM] = {0};
+  double in_F[EDIM*EDIM] = {0};
+
+  // functions from sympy
+  f_fun(in_x, dt, nx);
+  F_fun(in_x, dt, in_F);
+
+
+  EEM F(in_F);
+  EEM P(in_P);
+  EEM Q(in_Q);
+
+  RRM F_main = F.topLeftCorner(MEDIM, MEDIM);
+  P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose();
+  P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM);
+  P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose();
+
+  P = P + dt*Q;
+
+  // copy out state
+  memcpy(in_x, nx, DIM * sizeof(double));
+  memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
+}
+
+// note: extra_args dim only correct when null space projecting
+// otherwise 1
+template <int ZDIM, int EADIM, bool MAHA_TEST>
+void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) {
+  typedef Eigen::Matrix<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM;
+  typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM;
+  typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM;
+  //typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM;
+  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M;
+  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXM;
+
+  double in_hx[ZDIM] = {0};
+  double in_H[ZDIM * DIM] = {0};
+  double in_H_mod[EDIM * DIM] = {0};
+  double delta_x[EDIM] = {0};
+  double x_new[DIM] = {0};
+
+
+  // state x, P
+  Eigen::Matrix<double, ZDIM, 1> z(in_z);
+  EEM P(in_P);
+  ZZM pre_R(in_R);
+
+  // functions from sympy
+  h_fun(in_x, in_ea, in_hx);
+  H_fun(in_x, in_ea, in_H);
+  ZDM pre_H(in_H);
+
+  // get y (y = z - hx)
+  Eigen::Matrix<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y;
+  X1M y; XXM H; XXM R;
+  if (Hea_fun){
+    typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> ZAM;
+    double in_Hea[ZDIM * EADIM] = {0};
+    Hea_fun(in_x, in_ea, in_Hea);
+    ZAM Hea(in_Hea);
+    XXM A = Hea.transpose().fullPivLu().kernel();
+
+
+    y = A.transpose() * pre_y;
+    H = A.transpose() * pre_H;
+    R = A.transpose() * pre_R * A;
+  } else {
+    y = pre_y;
+    H = pre_H;
+    R = pre_R;
+  }
+  // get modified H
+  H_mod_fun(in_x, in_H_mod);
+  DEM H_mod(in_H_mod);
+  XEM H_err = H * H_mod;
+
+  // Do mahalobis distance test
+  if (MAHA_TEST){
+    XXM a = (H_err * P * H_err.transpose() + R).inverse();
+    double maha_dist = y.transpose() * a * y;
+    if (maha_dist > MAHA_THRESHOLD){
+      R = 1.0e16 * R;
+    }
+  }
+
+  // Outlier resilient weighting.
+  double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());
+
+  // kalman gains and I_KH
+  XXM S = ((H_err * P) * H_err.transpose()) + R/weight;
+  XEM KT = S.fullPivLu().solve(H_err * P.transpose());
+  //EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
+  //EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
+  //std::cout << "Here is the matrix rot:\n" << K << std::endl;
+  EEM I_KH = Eigen::Matrix<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err);
+
+  // update state by injecting dx
+  Eigen::Matrix<double, EDIM, 1> dx(delta_x);
+  dx  = (KT.transpose() * y);
+  memcpy(delta_x, dx.data(), EDIM * sizeof(double));
+  err_fun(in_x, delta_x, x_new);
+  Eigen::Matrix<double, DIM, 1> x(x_new);
+
+  // update cov
+  P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT);
+
+  // copy out state
+  memcpy(in_x, x.data(), DIM * sizeof(double));
+  memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
+  memcpy(in_z, y.data(), y.rows() * sizeof(double));
+}
+
+

artifacts/package_sources/rednose/rednose/templates/feature_handler.c

@@ -0,0 +1,56 @@
+bool sane(double track [K + 1][5]) {
+  double diffs_x [K-1];
+  double diffs_y [K-1];
+  int i;
+  for (i = 0; i < K-1; i++) {
+    diffs_x[i] = fabs(track[i+2][2] - track[i+1][2]);
+    diffs_y[i] = fabs(track[i+2][3] - track[i+1][3]);
+  }
+  for (i = 1; i < K-1; i++) {
+    if (((diffs_x[i] > 0.05 or diffs_x[i-1] > 0.05) and
+         (diffs_x[i] > 2*diffs_x[i-1] or
+          diffs_x[i] < .5*diffs_x[i-1])) or
+        ((diffs_y[i] > 0.05 or diffs_y[i-1] > 0.05) and
+	 (diffs_y[i] > 2*diffs_y[i-1] or
+          diffs_y[i] < .5*diffs_y[i-1]))){
+      return false;
+    }
+  }
+  return true;
+}
+
+void merge_features(double *tracks, double *features, long long *empty_idxs) {
+  double feature_arr [3000][5];
+  memcpy(feature_arr, features, 3000 * 5 * sizeof(double));
+  double track_arr [6000][K + 1][5];
+  memcpy(track_arr, tracks, (K+1) * 6000 * 5 * sizeof(double));
+  int match;
+  int empty_idx = 0;
+  int idx;
+  for (int i = 0; i < 3000; i++) {
+    match = feature_arr[i][4];
+    if (track_arr[match][0][1] == match and track_arr[match][0][2] == 0){
+      track_arr[match][0][0] = track_arr[match][0][0] + 1;
+      track_arr[match][0][1] = feature_arr[i][1];
+      track_arr[match][0][2] = 1;
+      idx = track_arr[match][0][0];
+      memcpy(track_arr[match][idx], feature_arr[i], 5 * sizeof(double));
+      if (idx == K){
+        // label complete
+        track_arr[match][0][3] = 1;
+	if (sane(track_arr[match])){
+          // label valid
+          track_arr[match][0][4] = 1;
+	}
+      }
+    } else {
+      // gen new track with this feature
+      track_arr[empty_idxs[empty_idx]][0][0] = 1;
+      track_arr[empty_idxs[empty_idx]][0][1] = feature_arr[i][1];
+      track_arr[empty_idxs[empty_idx]][0][2] = 1;
+      memcpy(track_arr[empty_idxs[empty_idx]][1], feature_arr[i], 5 * sizeof(double));
+      empty_idx = empty_idx + 1;
+    }
+  }
+  memcpy(tracks, track_arr, (K+1) * 6000 * 5 * sizeof(double));
+}

artifacts/package_sources/rednose/requirements.txt

@@ -0,0 +1,10 @@
+ruff
+sympy
+numpy
+scipy
+cffi
+scons
+pre-commit
+Cython
+pytest
+pytest-xdist

artifacts/package_sources/rednose/setup.py

@@ -0,0 +1,19 @@
+import os
+import subprocess
+
+from setuptools import Distribution, setup
+from setuptools.command.build_py import build_py
+
+
+class BinaryDistribution(Distribution):
+  def has_ext_modules(self):
+    return True
+
+
+class BuildPyWithScons(build_py):
+  def run(self):
+    subprocess.check_call(["scons", f"-j{os.cpu_count() or 1}", "rednose"], cwd=os.path.dirname(os.path.abspath(__file__)))
+    super().run()
+
+
+setup(cmdclass={"build_py": BuildPyWithScons}, distclass=BinaryDistribution)

artifacts/package_sources/rednose/site_scons/site_tools/cython.py

@@ -0,0 +1,72 @@
+import re
+import SCons
+from SCons.Action import Action
+from SCons.Scanner import Scanner
+
+pyx_from_import_re = re.compile(r'^from\s+(\S+)\s+cimport', re.M)
+pyx_import_re = re.compile(r'^cimport\s+(\S+)', re.M)
+cdef_import_re = re.compile(r'^cdef extern from\s+.(\S+).:', re.M)
+
+
+def pyx_scan(node, env, path, arg=None):
+  contents = node.get_text_contents()
+
+  # from <module> cimport ...
+  matches = pyx_from_import_re.findall(contents)
+  # cimport <module>
+  matches += pyx_import_re.findall(contents)
+
+  # Modules can be either .pxd or .pyx files
+  files = [m.replace('.', '/') + '.pxd' for m in matches]
+  files += [m.replace('.', '/') + '.pyx' for m in matches]
+
+  # cdef extern from <file>
+  files += cdef_import_re.findall(contents)
+
+  # Handle relative imports
+  cur_dir = str(node.get_dir())
+  files = [cur_dir + f if f.startswith('/') else f for f in files]
+
+  # Filter out non-existing files (probably system imports)
+  files = [f for f in files if env.File(f).exists()]
+  return env.File(files)
+
+
+pyxscanner = Scanner(function=pyx_scan, skeys=['.pyx', '.pxd'], recursive=True)
+cythonAction = Action("$CYTHONCOM")
+
+
+def create_builder(env):
+  try:
+    cython = env['BUILDERS']['Cython']
+  except KeyError:
+    cython = SCons.Builder.Builder(
+      action=cythonAction,
+      emitter={},
+      suffix=cython_suffix_emitter,
+      single_source=1
+    )
+    env.Append(SCANNERS=pyxscanner)
+    env['BUILDERS']['Cython'] = cython
+  return cython
+
+def cython_suffix_emitter(env, source):
+  return "$CYTHONCFILESUFFIX"
+
+def generate(env):
+  env["CYTHON"] = "cythonize"
+  env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE"
+  env["CYTHONCFILESUFFIX"] = ".cpp"
+
+  c_file, _ = SCons.Tool.createCFileBuilders(env)
+
+  c_file.suffix['.pyx'] = cython_suffix_emitter
+  c_file.add_action('.pyx', cythonAction)
+
+  c_file.suffix['.py'] = cython_suffix_emitter
+  c_file.add_action('.py', cythonAction)
+
+  create_builder(env)
+
+def exists(env):
+  return True

artifacts/package_sources/rednose/site_scons/site_tools/rednose_filter.py

@@ -0,0 +1,51 @@
+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)
+
+  action = f"{File(generator_file).relpath} {target} {Dir(output_dir).relpath}"
+  if hasattr(env, 'PrettyAction'):  # short colored line when the top-level pretty tool is present
+    action = env.PrettyAction(action, 'GEN')
+  env.Command(generated_src_files + extra_generated_files,
+              [generator_file] + script_deps, action)
+
+  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