diff --git a/examples/kinematic_kf.py b/examples/kinematic_kf.py index 29049a7..47ffd74 100755 --- a/examples/kinematic_kf.py +++ b/examples/kinematic_kf.py @@ -9,7 +9,7 @@ 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 import EKFSym class ObservationKind(): @@ -36,13 +36,13 @@ class States(): class KinematicKalman(KalmanFilter): name = 'kinematic' - initial_x = np.array([0.5, 0.0]) + initial_x: np.ndarray = np.array([0.5, 0.0]) # state covariance - initial_P_diag = np.array([1.0**2, 1.0**2]) + initial_P_diag: np.ndarray = np.array([1.0**2, 1.0**2]) # process noise - Q = np.diag([0.1**2, 2.0**2]) + Q: np.ndarray = np.diag([0.1**2, 2.0**2]) obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)} @@ -73,7 +73,7 @@ def __init__(self, generated_dir): 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) + self.filter = EKFSym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err) if __name__ == "__main__": diff --git a/examples/live_kf.py b/examples/live_kf.py index 6e3c21d..11bfc66 100755 --- a/examples/live_kf.py +++ b/examples/live_kf.py @@ -9,7 +9,7 @@ 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 + from rednose.helpers import EKFSym EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) @@ -258,7 +258,7 @@ def __init__(self, generated_dir): 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) + self.filter = EKFSym(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): diff --git a/examples/test_compare.py b/examples/test_compare.py index 291f29e..026f733 100755 --- a/examples/test_compare.py +++ b/examples/test_compare.py @@ -8,7 +8,7 @@ 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 import EKFSym from rednose.helpers.ekf_sym import EKF_sym as EKF_sym2 @@ -71,7 +71,7 @@ def __init__(self, generated_dir): 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_py = EKFSym(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): diff --git a/examples/test_kinematic_kf.py b/examples/test_kinematic_kf.py index 05d4d3e..1483478 100644 --- a/examples/test_kinematic_kf.py +++ b/examples/test_kinematic_kf.py @@ -7,11 +7,12 @@ GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated')) class TestKinematic: + def setup_method(self): + self.kf = KinematicKalman(GENERATED_DIR) + 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) @@ -34,13 +35,13 @@ def test_kinematic_kf(self): # Update kf meas = np.random.normal(x, 0.1) xs_meas.append(meas) - kf.predict_and_observe(t, ObservationKind.POSITION, [meas]) + self.kf.predict_and_observe(t, ObservationKind.POSITION, [meas]) # Retrieve kf values - state = kf.x + state = self.kf.x xs_kf.append(float(state[States.POSITION].item())) vs_kf.append(float(state[States.VELOCITY].item())) - std = np.sqrt(kf.P) + std = np.sqrt(self.kf.P) xs_kf_std.append(float(std[States.POSITION, States.POSITION].item())) vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY].item())) @@ -80,3 +81,47 @@ def test_kinematic_kf(self): plt.legend() plt.show() + + def test_init_state(self): + init_x = self.kf.x + + dim_state_err = self.kf.initial_P_diag.shape[0] + + new_x = np.copy(init_x) + new_x[States.POSITION] = 100.0 + new_x[States.VELOCITY] = 5.0 + + new_P = np.eye(dim_state_err) * 0.5 + + self.kf.init_state(new_x, covs=new_P, filter_time=1.0) + + assert np.allclose(self.kf.x, new_x) + assert np.allclose(self.kf.P, new_P) + assert self.kf.t == 1.0 + + def test_set_filter_time(self): + assert np.isnan(self.kf.t) + + self.kf.filter.set_filter_time(10.5) + assert self.kf.t == 10.5 + + def test_predict(self): + dim_state = self.kf.initial_x.shape[0] + + x0 = np.zeros(dim_state) + x0[States.VELOCITY] = 10.0 + self.kf.init_state(x0, filter_time=0.0) + + t0 = self.kf.t + dt = 0.1 + + self.kf.filter.predict(t0 + dt) + + assert self.kf.t == pytest.approx(t0 + dt) + assert self.kf.x[States.POSITION].item() == pytest.approx(1.0) + + def test_rewind(self): + try: + self.kf.filter.reset_rewind() + except Exception as e: + pytest.fail(f"reset_rewind raised exception: {e}") diff --git a/rednose/SConscript b/rednose/SConscript index 52e36e0..b24d940 100644 --- a/rednose/SConscript +++ b/rednose/SConscript @@ -11,7 +11,7 @@ if common != "": 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"]) +rednose_python = envCython.SharedLibrary("helpers/_ekf_sym_module.so", ["helpers/ekf_sym_module.cc", ekf_objects], + LIBS=libs + envCython["LIBS"], SHLIBPREFIX="", CPPPATH=envCython["CPPPATH"] + [Dir('.').abspath]) Export('rednose', 'rednose_python') diff --git a/rednose/helpers/__init__.py b/rednose/helpers/__init__.py index 3acc14a..c52ad8a 100644 --- a/rednose/helpers/__init__.py +++ b/rednose/helpers/__init__.py @@ -33,3 +33,5 @@ def load_code(folder, name): class KalmanError(Exception): pass + +from ._ekf_sym_module import EKFSym # noqa: F401 diff --git a/rednose/helpers/ekf_sym_module.cc b/rednose/helpers/ekf_sym_module.cc new file mode 100644 index 0000000..bda4d16 --- /dev/null +++ b/rednose/helpers/ekf_sym_module.cc @@ -0,0 +1,402 @@ + +#define PY_SSIZE_T_CLEAN +#include +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include + +#include "helpers/ekf_sym.h" + +#include +#include +#include + +using namespace EKFS; + +// --- Helper Functions --- + +static PyObject* vector_to_numpy(const Eigen::VectorXd& vec) { + npy_intp dims[1] = { (npy_intp)vec.size() }; + PyObject* arr = PyArray_SimpleNew(1, dims, NPY_DOUBLE); + if (!arr) return NULL; + + double* data = (double*)PyArray_DATA((PyArrayObject*)arr); + Eigen::Map(data, vec.size()) = vec; + + return arr; +} + +static PyObject* matrix_to_numpy(const MatrixXdr& mat) { + npy_intp dims[2] = { (npy_intp)mat.rows(), (npy_intp)mat.cols() }; + PyObject* arr = PyArray_SimpleNew(2, dims, NPY_DOUBLE); + if (!arr) return NULL; + + double* data = (double*)PyArray_DATA((PyArrayObject*)arr); + Eigen::Map(data, mat.rows(), mat.cols()) = mat; + + return arr; +} + +static PyArrayObject* get_contiguous_double_array(PyObject* obj, int min_depth, int max_depth) { + return (PyArrayObject*)PyArray_ContiguousFromAny(obj, NPY_DOUBLE, min_depth, max_depth); +} + +// --- EKFSym Wrapper --- + +typedef struct { + PyObject_HEAD + EKFSym* ekf; +} EKFSymObject; + +static void EKFSym_dealloc(EKFSymObject* self) { + if (self->ekf) { + delete self->ekf; + } + Py_TYPE(self)->tp_free((PyObject*)self); +} + +static int EKFSym_init(EKFSymObject* self, PyObject* args, PyObject* kwds) { + + + char *gen_dir_str, *name_str; + PyObject *Q_obj, *x_init_obj, *P_init_obj; + int dim_main, dim_main_err; + int N = 0; + int dim_augment = 0; + int dim_augment_err = 0; + PyObject *maha_test_kinds_obj = NULL; + PyObject *quaternion_idxs_obj = NULL; + PyObject *global_vars_obj = NULL; + double max_rewind_age = 1.0; + PyObject *logger_obj = NULL; + + static char* kwlist[] = { + (char*)"gen_dir", (char*)"name", (char*)"Q", (char*)"x_initial", (char*)"P_initial", + (char*)"dim_main", (char*)"dim_main_err", (char*)"N", (char*)"dim_augment", (char*)"dim_augment_err", + (char*)"maha_test_kinds", (char*)"quaternion_idxs", (char*)"global_vars", (char*)"max_rewind_age", (char*)"logger", NULL + }; + + if (!PyArg_ParseTupleAndKeywords(args, kwds, "ssOOOii|iiiOOOdO", kwlist, + &gen_dir_str, &name_str, &Q_obj, &x_init_obj, &P_init_obj, + &dim_main, &dim_main_err, &N, &dim_augment, &dim_augment_err, + &maha_test_kinds_obj, &quaternion_idxs_obj, &global_vars_obj, &max_rewind_age, &logger_obj)) { + return -1; + } + + ekf_load_and_register(std::string(gen_dir_str), std::string(name_str)); + + PyArrayObject *Q_arr = get_contiguous_double_array(Q_obj, 2, 2); + PyArrayObject *x_init_arr = get_contiguous_double_array(x_init_obj, 1, 1); + PyArrayObject *P_init_arr = get_contiguous_double_array(P_init_obj, 2, 2); + + if (!Q_arr || !x_init_arr || !P_init_arr) { + Py_XDECREF(Q_arr); Py_XDECREF(x_init_arr); Py_XDECREF(P_init_arr); + return -1; + } + + std::vector maha, quat_idxs; + std::vector globals; + + if (maha_test_kinds_obj) { + PyObject *iter = PyObject_GetIter(maha_test_kinds_obj); + if (!iter) goto fail; + PyObject *item; + while ((item = PyIter_Next(iter))) { + maha.push_back((int)PyLong_AsLong(item)); + Py_DECREF(item); + } + Py_DECREF(iter); + if (PyErr_Occurred()) goto fail; + } + + if (quaternion_idxs_obj) { + PyObject *iter = PyObject_GetIter(quaternion_idxs_obj); + if (!iter) goto fail; + PyObject *item; + while ((item = PyIter_Next(iter))) { + quat_idxs.push_back((int)PyLong_AsLong(item)); + Py_DECREF(item); + } + Py_DECREF(iter); + if (PyErr_Occurred()) goto fail; + } + + if (global_vars_obj) { + PyObject *iter = PyObject_GetIter(global_vars_obj); + if (!iter) goto fail; + PyObject *item; + while ((item = PyIter_Next(iter))) { + const char* s = PyUnicode_AsUTF8(item); + if (s) globals.push_back(std::string(s)); + Py_DECREF(item); + } + Py_DECREF(iter); + if (PyErr_Occurred()) goto fail; + } + + { + Eigen::Map Q_map((double*)PyArray_DATA(Q_arr), PyArray_DIM(Q_arr, 0), PyArray_DIM(Q_arr, 1)); + Eigen::Map x_map((double*)PyArray_DATA(x_init_arr), PyArray_DIM(x_init_arr, 0)); + Eigen::Map P_map((double*)PyArray_DATA(P_init_arr), PyArray_DIM(P_init_arr, 0), PyArray_DIM(P_init_arr, 1)); + + self->ekf = new EKFSym( + std::string(name_str), Q_map, x_map, P_map, + dim_main, dim_main_err, N, dim_augment, dim_augment_err, + maha, quat_idxs, globals, max_rewind_age + ); + } + + Py_DECREF(Q_arr); + Py_DECREF(x_init_arr); + Py_DECREF(P_init_arr); + return 0; + +fail: + Py_XDECREF(Q_arr); + Py_XDECREF(x_init_arr); + Py_XDECREF(P_init_arr); + return -1; +} + +static PyObject* EKFSym_init_state(EKFSymObject* self, PyObject* args) { + PyObject *state_obj, *covs_obj; + double filter_time; + if (!PyArg_ParseTuple(args, "OOd", &state_obj, &covs_obj, &filter_time)) return NULL; + + PyArrayObject *state_arr = get_contiguous_double_array(state_obj, 1, 1); + PyArrayObject *covs_arr = get_contiguous_double_array(covs_obj, 2, 2); + if (!state_arr || !covs_arr) { + Py_XDECREF(state_arr); Py_XDECREF(covs_arr); + return NULL; + } + + Eigen::Map state_map((double*)PyArray_DATA(state_arr), PyArray_DIM(state_arr, 0)); + Eigen::Map covs_map((double*)PyArray_DATA(covs_arr), PyArray_DIM(covs_arr, 0), PyArray_DIM(covs_arr, 1)); + + self->ekf->init_state(state_map, covs_map, filter_time); + + Py_DECREF(state_arr); + Py_DECREF(covs_arr); + Py_RETURN_NONE; +} + +static PyObject* EKFSym_state(EKFSymObject* self, PyObject* args) { + return vector_to_numpy(self->ekf->state()); +} + +static PyObject* EKFSym_covs(EKFSymObject* self, PyObject* args) { + return matrix_to_numpy(self->ekf->covs()); +} + +static PyObject* EKFSym_set_filter_time(EKFSymObject* self, PyObject* args) { + double t; + if (!PyArg_ParseTuple(args, "d", &t)) return NULL; + self->ekf->set_filter_time(t); + Py_RETURN_NONE; +} + +static PyObject* EKFSym_get_filter_time(EKFSymObject* self, PyObject* args) { + return PyFloat_FromDouble(self->ekf->get_filter_time()); +} + +static PyObject* EKFSym_set_global(EKFSymObject* self, PyObject* args) { + char* name; + double val; + if (!PyArg_ParseTuple(args, "sd", &name, &val)) return NULL; + self->ekf->set_global(std::string(name), val); + Py_RETURN_NONE; +} + +static PyObject* EKFSym_reset_rewind(EKFSymObject* self, PyObject* args) { + self->ekf->reset_rewind(); + Py_RETURN_NONE; +} + +static PyObject* EKFSym_predict(EKFSymObject* self, PyObject* args) { + double t; + if (!PyArg_ParseTuple(args, "d", &t)) return NULL; + self->ekf->predict(t); + Py_RETURN_NONE; +} + +// predict_and_update_batch(double t, int kind, vector[MapVectorXd] z, vector[MapMatrixXdr] R, vector[vector[double]] extra_args, bool augment) +static PyObject* EKFSym_predict_and_update_batch(EKFSymObject* self, PyObject* args, PyObject* kwds) { + double t; + int kind; + PyObject *z_obj, *R_obj; + PyObject *extra_args_obj = NULL; + int augment = 0; + + static char* kwlist[] = { + (char*)"t", (char*)"kind", (char*)"z", (char*)"R", (char*)"extra_args", (char*)"augment", NULL + }; + + if (!PyArg_ParseTupleAndKeywords(args, kwds, "diOO|Oi", kwlist, &t, &kind, &z_obj, &R_obj, &extra_args_obj, &augment)) { + return NULL; + } + + std::vector z_arrays; + std::vector R_arrays; + std::vector> z_maps; + std::vector> R_maps; + std::vector> extra_args_cpp; + + auto cleanup = [&]() { + for (auto p : z_arrays) Py_DECREF(p); + for (auto p : R_arrays) Py_DECREF(p); + }; + + PyObject *z_iter = PyObject_GetIter(z_obj); + if (z_iter) { + PyObject *item; + while ((item = PyIter_Next(z_iter))) { + PyArrayObject* arr = get_contiguous_double_array(item, 1, 1); + Py_DECREF(item); + if (!arr) { Py_DECREF(z_iter); cleanup(); return NULL; } + z_arrays.push_back(arr); + z_maps.emplace_back((double*)PyArray_DATA(arr), PyArray_DIM(arr, 0)); + } + Py_DECREF(z_iter); + if (PyErr_Occurred()) { cleanup(); return NULL; } + } else { + PyErr_SetString(PyExc_TypeError, "z must be iterable"); + cleanup(); return NULL; + } + + PyObject *R_iter = PyObject_GetIter(R_obj); + if (R_iter) { + PyObject *item; + while ((item = PyIter_Next(R_iter))) { + PyArrayObject* arr = get_contiguous_double_array(item, 2, 2); + Py_DECREF(item); + if (!arr) { Py_DECREF(R_iter); cleanup(); return NULL; } + R_arrays.push_back(arr); + R_maps.emplace_back((double*)PyArray_DATA(arr), PyArray_DIM(arr, 0), PyArray_DIM(arr, 1)); + } + Py_DECREF(R_iter); + if (PyErr_Occurred()) { cleanup(); return NULL; } + } else { + PyErr_SetString(PyExc_TypeError, "R must be iterable"); + cleanup(); return NULL; + } + + if (extra_args_obj) { + PyObject *ea_iter = PyObject_GetIter(extra_args_obj); + if (ea_iter) { + PyObject *inner; + while ((inner = PyIter_Next(ea_iter))) { + std::vector ea; + PyObject *inner_iter = PyObject_GetIter(inner); + if (inner_iter) { + PyObject *val; + while ((val = PyIter_Next(inner_iter))) { + ea.push_back(PyFloat_AsDouble(val)); + Py_DECREF(val); + if (PyErr_Occurred()) { Py_DECREF(inner_iter); Py_DECREF(inner); Py_DECREF(ea_iter); cleanup(); return NULL; } + } + Py_DECREF(inner_iter); + } else { + PyErr_Clear(); + } + extra_args_cpp.push_back(ea); + Py_DECREF(inner); + } + Py_DECREF(ea_iter); + if (PyErr_Occurred()) { cleanup(); return NULL; } + } + } else { + extra_args_cpp.push_back({}); + } + if (extra_args_cpp.empty()) extra_args_cpp.push_back({}); + + std::optional res = self->ekf->predict_and_update_batch(t, kind, z_maps, R_maps, extra_args_cpp, (bool)augment); + + cleanup(); // arrays no longer needed after call returns result copy + + if (!res.has_value()) { + Py_RETURN_NONE; + } + + Estimate& est = res.value(); + + PyObject* res_tuple = PyTuple_New(9); + + PyTuple_SetItem(res_tuple, 0, vector_to_numpy(est.xk1)); + PyTuple_SetItem(res_tuple, 1, vector_to_numpy(est.xk)); + PyTuple_SetItem(res_tuple, 2, matrix_to_numpy(est.Pk1)); + PyTuple_SetItem(res_tuple, 3, matrix_to_numpy(est.Pk)); + PyTuple_SetItem(res_tuple, 4, PyFloat_FromDouble(est.t)); + PyTuple_SetItem(res_tuple, 5, PyLong_FromLong(est.kind)); + + PyObject* y_list = PyList_New(est.y.size()); + for(size_t i=0; i" 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": - MapVectorXd(double*, int) - - cdef cppclass MapMatrixXdr "Eigen::Map >": - MapMatrixXdr(double*, int, int) - - cdef cppclass VectorXd "Eigen::VectorXd": - VectorXd() - double* data() - int rows() - - cdef cppclass MatrixXdr "Eigen::Matrix": - 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 = 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 = 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( Q_b.data, Q.shape[0], Q.shape[1]), - MapVectorXd( x_initial_b.data, x_initial.shape[0]), - MapMatrixXdr( 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( state_b.data, state.shape[0]), - MapMatrixXdr( 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( 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( 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 diff --git a/site_scons/site_tools/rednose_filter.py b/site_scons/site_tools/rednose_filter.py index 1a26a3b..d49ff02 100644 --- a/site_scons/site_tools/rednose_filter.py +++ b/site_scons/site_tools/rednose_filter.py @@ -9,7 +9,7 @@ def compile_single_filter(env, target, filter_gen_script, output_dir, extra_gen_ generator_file = File(filter_gen_script) env.Command(generated_src_files + extra_generated_files, - [generator_file] + script_deps, f"{File(generator_file).relpath} {target} {Dir(output_dir).relpath}") + [generator_file] + script_deps, f"PYTHONPATH={env['REDNOSE_ROOT']}:$PYTHONPATH {File(generator_file).relpath} {target} {Dir(output_dir).relpath}") generated_cc_file = File(generated_src_files[:1]) @@ -37,8 +37,9 @@ 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") + ekf_sym_lib = env.File("$REDNOSE_ROOT/rednose/helpers/_ekf_sym_module.so") - gen_script_deps = templates + [sympy_helpers, ekf_sym] + gen_script_deps = templates + [sympy_helpers, ekf_sym, ekf_sym_lib] filter_lib_deps = [] env.AddMethod(CompileFilterMethod(gen_script_deps, filter_lib_deps), "RednoseCompileFilter")