Skip to content

Commit

Permalink
pass parameters by reference
Browse files Browse the repository at this point in the history
  • Loading branch information
@deanlee
deanlee committed Sep 10, 2024
1 parent 05fd602 commit 6fefee8
Show file tree
Hide file tree
Showing 3 changed files with 24 additions and 24 deletions.
16 changes: 8 additions & 8 deletions rednose/helpers/ekf_sym.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,9 @@
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)
EKFSym::EKFSym(const std::string &name, const Map<MatrixXdr> &Q, const Map<VectorXd> &x_initial, const Map<MatrixXdr> &P_initial, int dim_main,
int dim_main_err, int N, int dim_augment, int dim_augment_err, const std::vector<int> &maha_test_kinds,
const std::vector<int> &quaternion_idxs, const std::vector<std::string> &global_vars, double max_rewind_age)
{
// TODO: add logger
this->ekf = ekf_lookup(name);
Expand Down Expand Up @@ -42,7 +42,7 @@ EKFSym::EKFSym(std::string name, Map<MatrixXdr> Q, Map<VectorXd> x_initial, Map<
this->init_state(x_initial, P_initial, NAN);
}

void EKFSym::init_state(Map<VectorXd> state, Map<MatrixXdr> covs, double init_filter_time) {
void EKFSym::init_state(const Map<VectorXd> &state, const Map<MatrixXdr> &covs, double init_filter_time) {
this->x = state;
this->P = covs;
this->filter_time = init_filter_time;
Expand Down Expand Up @@ -80,8 +80,8 @@ 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)
std::optional<Estimate> EKFSym::predict_and_update_batch(double t, int kind, const std::vector<Map<VectorXd>> &z_map,
const std::vector<Map<MatrixXdr>> &R_map, const std::vector<std::vector<double>> &extra_args, bool augment)
{
// TODO handle rewinding at this level

Expand Down Expand Up @@ -141,7 +141,7 @@ std::deque<Observation> 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));
Expand All @@ -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());

Expand Down
20 changes: 10 additions & 10 deletions rednose/helpers/ekf_sym.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,12 +43,12 @@ typedef struct 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);
EKFSym(const std::string &name, const Eigen::Map<MatrixXdr> &Q, const Eigen::Map<Eigen::VectorXd> &x_initial,
const Eigen::Map<MatrixXdr> &P_initial, int dim_main, int dim_main_err, int N = 0, int dim_augment = 0,
int dim_augment_err = 0, const std::vector<int> &maha_test_kinds = std::vector<int>(),
const std::vector<int> &quaternion_idxs = std::vector<int>(),
const std::vector<std::string> &global_vars = std::vector<std::string>(), double max_rewind_age = 1.0);
void init_state(const Eigen::Map<Eigen::VectorXd> &state, const Eigen::Map<MatrixXdr> &covs, double filter_time);

Eigen::VectorXd state();
MatrixXdr covs();
Expand All @@ -60,16 +60,16 @@ class EKFSym {
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);
std::optional<Estimate> predict_and_update_batch(double t, int kind, const std::vector<Eigen::Map<Eigen::VectorXd>> &z,
const std::vector<Eigen::Map<MatrixXdr>> &R, const 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);
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<double> extra_args);

// stuct with linked sympy generated functions
Expand Down
12 changes: 6 additions & 6 deletions rednose/helpers/ekf_sym_pyx.pyx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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
Expand Down

0 comments on commit 6fefee8

Please sign in to comment.