diff --git a/rednose/helpers/ekf_sym.cc b/rednose/helpers/ekf_sym.cc index 38d419d..0de90a3 100644 --- a/rednose/helpers/ekf_sym.cc +++ b/rednose/helpers/ekf_sym.cc @@ -4,9 +4,9 @@ using namespace EKFS; using namespace Eigen; -EKFSym::EKFSym(std::string name, Map Q, Map x_initial, Map P_initial, int dim_main, - int dim_main_err, int N, int dim_augment, int dim_augment_err, std::vector maha_test_kinds, - std::vector quaternion_idxs, std::vector global_vars, double max_rewind_age) +EKFSym::EKFSym(const std::string &name, const Map &Q, const Map &x_initial, const Map &P_initial, int dim_main, + int dim_main_err, int N, int dim_augment, int dim_augment_err, const std::vector &maha_test_kinds, + const std::vector &quaternion_idxs, const std::vector &global_vars, double max_rewind_age) { // TODO: add logger this->ekf = ekf_lookup(name); @@ -42,7 +42,7 @@ EKFSym::EKFSym(std::string name, Map Q, Map x_initial, Map< this->init_state(x_initial, P_initial, NAN); } -void EKFSym::init_state(Map state, Map covs, double init_filter_time) { +void EKFSym::init_state(const Map &state, const Map &covs, double init_filter_time) { this->x = state; this->P = covs; this->filter_time = init_filter_time; @@ -80,8 +80,8 @@ void EKFSym::set_global(std::string global_var, double val) { this->ekf->sets.at(global_var)(val); } -std::optional EKFSym::predict_and_update_batch(double t, int kind, std::vector> z_map, - std::vector> R_map, std::vector> extra_args, bool augment) +std::optional EKFSym::predict_and_update_batch(double t, int kind, const std::vector> &z_map, + const std::vector> &R_map, const std::vector> &extra_args, bool augment) { // TODO handle rewinding at this level @@ -141,7 +141,7 @@ std::deque EKFSym::rewind(double t) { return rewound; } -void EKFSym::checkpoint(Observation& obs) { +void EKFSym::checkpoint(const 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)); @@ -155,7 +155,7 @@ void EKFSym::checkpoint(Observation& obs) { } } -Estimate EKFSym::predict_and_update_batch(Observation& obs, bool augment) { +Estimate EKFSym::predict_and_update_batch(const Observation &obs, bool augment) { assert(obs.z.size() == obs.R.size()); assert(obs.z.size() == obs.extra_args.size()); diff --git a/rednose/helpers/ekf_sym.h b/rednose/helpers/ekf_sym.h index b83d7f3..318963e 100644 --- a/rednose/helpers/ekf_sym.h +++ b/rednose/helpers/ekf_sym.h @@ -43,12 +43,12 @@ typedef struct Estimate { class EKFSym { public: - EKFSym(std::string name, Eigen::Map Q, Eigen::Map x_initial, - Eigen::Map P_initial, int dim_main, int dim_main_err, int N = 0, int dim_augment = 0, - int dim_augment_err = 0, std::vector maha_test_kinds = std::vector(), - std::vector quaternion_idxs = std::vector(), - std::vector global_vars = std::vector(), double max_rewind_age = 1.0); - void init_state(Eigen::Map state, Eigen::Map covs, double filter_time); + EKFSym(const std::string &name, const Eigen::Map &Q, const Eigen::Map &x_initial, + const Eigen::Map &P_initial, int dim_main, int dim_main_err, int N = 0, int dim_augment = 0, + int dim_augment_err = 0, const std::vector &maha_test_kinds = std::vector(), + const std::vector &quaternion_idxs = std::vector(), + const std::vector &global_vars = std::vector(), double max_rewind_age = 1.0); + void init_state(const Eigen::Map &state, const Eigen::Map &covs, double filter_time); Eigen::VectorXd state(); MatrixXdr covs(); @@ -60,16 +60,16 @@ class EKFSym { void reset_rewind(); void predict(double t); - std::optional predict_and_update_batch(double t, int kind, std::vector> z, - std::vector> R, std::vector> extra_args = {{}}, bool augment = false); + std::optional predict_and_update_batch(double t, int kind, const std::vector> &z, + const std::vector> &R, const std::vector> &extra_args = {{}}, bool augment = false); extra_routine_t get_extra_routine(const std::string& routine); private: std::deque rewind(double t); - void checkpoint(Observation& obs); + void checkpoint(const Observation &obs); - Estimate predict_and_update_batch(Observation& obs, bool augment); + Estimate predict_and_update_batch(const Observation &obs, bool augment); Eigen::VectorXd update(int kind, Eigen::VectorXd z, MatrixXdr R, std::vector extra_args); // stuct with linked sympy generated functions diff --git a/rednose/helpers/ekf_sym_pyx.pyx b/rednose/helpers/ekf_sym_pyx.pyx index f552ea7..bbeb884 100644 --- a/rednose/helpers/ekf_sym_pyx.pyx +++ b/rednose/helpers/ekf_sym_pyx.pyx @@ -51,10 +51,10 @@ cdef extern from "rednose/helpers/ekf_sym.h" namespace "EKFS": 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) + EKFSym(const string &name, const MapMatrixXdr &Q, const MapVectorXd &x_initial, const MapMatrixXdr &P_initial, int dim_main, + int dim_main_err, int N, int dim_augment, int dim_augment_err, const vector[int] &maha_test_kinds, + const vector[int] &quaternion_idxs, const vector[string] &global_vars, double max_rewind_age) + void init_state(const MapVectorXd &state, const MapMatrixXdr &covs, double filter_time) VectorXd state() MatrixXdr covs() @@ -64,8 +64,8 @@ cdef extern from "rednose/helpers/ekf_sym.h" namespace "EKFS": 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) + optional[Estimate] predict_and_update_batch(double t, int kind, const vector[MapVectorXd] &z, const vector[MapMatrixXdr] &z, + const 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