Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

passes parameters by reference #47

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

passes parameters by reference #47

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 8 additions & 8 deletions rednose/helpers/ekf_sym.cc
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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