diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 8bb25c437970d14dc7cb48b30567b990c0ae386b..1b76c1b4800156c25a1bf932c6721456d6055e91 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -129,7 +129,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh protected: Eigen::MatrixXd noise_cov_; // cov matrix of noise - std::map<char, Eigen::MatrixXd> drift_rate_covs_; // covariance rate of the drift of dynamic state blocks + std::map<char, Eigen::MatrixXd> drift_covs_; // covariance of the drift of dynamic state blocks [unit²/s] void setProblem(ProblemPtr _problem) override final; virtual void loadParams(ParamsSensorBasePtr _params); @@ -257,12 +257,12 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh void setNoiseStd(const Eigen::VectorXd & _noise_std); void setNoiseCov(const Eigen::MatrixXd & _noise_cov); - void setDriftRateStd(char, const Eigen::VectorXd & _drift_rate_std); - void setDriftRateCov(char, const Eigen::MatrixXd & _drift_rate_cov); + void setDriftStd(char, const Eigen::VectorXd & _drift_rate_std); + void setDriftCov(char, const Eigen::MatrixXd & _drift_rate_cov); Eigen::VectorXd getNoiseStd() const; Eigen::MatrixXd getNoiseCov() const; - Eigen::VectorXd getDriftRateStd(char) const; - Eigen::MatrixXd getDriftRateCov(char) const; + Eigen::VectorXd getDriftStd(char) const; + Eigen::MatrixXd getDriftCov(char) const; virtual void printHeader(int depth, // bool constr_by, // @@ -362,20 +362,20 @@ inline Eigen::MatrixXd SensorBase::getNoiseCov() const return noise_cov_; } -inline Eigen::VectorXd SensorBase::getDriftRateStd(char _key) const +inline Eigen::VectorXd SensorBase::getDriftStd(char _key) const { - if (drift_rate_covs_.count(_key) == 0) + if (drift_covs_.count(_key) == 0) return Eigen::MatrixXd(0,0); else - return drift_rate_covs_.at(_key).diagonal().cwiseSqrt(); + return drift_covs_.at(_key).diagonal().cwiseSqrt(); } -inline Eigen::MatrixXd SensorBase::getDriftRateCov(char _key) const +inline Eigen::MatrixXd SensorBase::getDriftCov(char _key) const { - if (drift_rate_covs_.count(_key) == 0) + if (drift_covs_.count(_key) == 0) return Eigen::MatrixXd(0,0); else - return drift_rate_covs_.at(_key); + return drift_covs_.at(_key); } void setDriftRateStd(char, const Eigen::VectorXd & _drift_rate_std); diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h index 8f422da10bb8ad37fa2714c4db7a3c025cc33b6f..1dc7b33839282579e412bc0928a882d28cb121a8 100644 --- a/include/core/state_block/prior.h +++ b/include/core/state_block/prior.h @@ -28,94 +28,66 @@ using std::to_string; -namespace wolf +namespace wolf { - class ParamsServer; class Prior; typedef std::unordered_map<char, Prior> Priors; class Prior { - private: - std::string type_; // State type - std::string mode_; // Prior mode. Can be 'initial_guess', 'fix' and 'factor' - Eigen::VectorXd state_; // state values - Eigen::VectorXd sigma_std_; // factor sigmas (only filled from server if mode == 'factor') - bool dynamic_; // State dynamic - Eigen::VectorXd sigma_drift_std_; // drift of the state (only filled from server if dynamic) - - public: + private: + std::string type_; // State type + std::string mode_; // Prior mode. Can be 'initial_guess', 'fix' or 'factor' + Eigen::VectorXd state_; // state values + Eigen::VectorXd noise_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix (ONLY IF mode == 'factor') + bool dynamic_; // State dynamic + Eigen::VectorXd drift_std_; // OPTIONAL std of the state drift [in units/sqrt(s)] diagonal elements (ONLY IF if dynamic) + + public: Prior() = default; - Prior(const std::string& _type, + Prior(const std::string& _type, const Eigen::VectorXd& _state, - const std::string& _mode = "fix", - const Eigen::VectorXd& _sigma_std = Eigen::VectorXd(0), - bool _dynamic = false, - const Eigen::VectorXd& _sigma_drift_std = Eigen::VectorXd(0)); + const std::string& _mode = "fix", + const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0), + bool _dynamic = false, + const Eigen::VectorXd& _drift_std = Eigen::VectorXd(0)); Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server); virtual ~Prior() = default; - const std::string& getType() const; - const std::string& getMode() const; + const std::string& getType() const; + const std::string& getMode() const; const Eigen::VectorXd& getState() const; - const Eigen::VectorXd& getSigmaStd() const; - bool isDynamic() const; - bool isFixed() const; - bool isInitialGuess() const; - bool isFactor() const; - const Eigen::VectorXd& getSigmaDriftStd() const; + const Eigen::VectorXd& getNoiseStd() const; + bool isDynamic() const; + bool isFixed() const; + bool isInitialGuess() const; + bool isFactor() const; + const Eigen::VectorXd& getDriftStd() const; void check() const; virtual std::string print() const final; }; -inline const std::string& Prior::getType() const -{ - return type_; -} +inline const std::string& Prior::getType() const { return type_; } -inline const std::string& Prior::getMode() const -{ - return mode_; -} +inline const std::string& Prior::getMode() const { return mode_; } -inline const Eigen::VectorXd& Prior::getState() const -{ - return state_; -} +inline const Eigen::VectorXd& Prior::getState() const { return state_; } -inline const Eigen::VectorXd& Prior::getSigmaStd() const -{ - return sigma_std_; -} +inline const Eigen::VectorXd& Prior::getNoiseStd() const { return noise_std_; } -inline bool Prior::isDynamic() const -{ - return dynamic_; -} +inline bool Prior::isDynamic() const { return dynamic_; } -inline bool Prior::isFixed() const -{ - return mode_ == "fix"; -} +inline bool Prior::isFixed() const { return mode_ == "fix"; } -inline bool Prior::isInitialGuess() const -{ - return mode_ == "initial_guess"; -} +inline bool Prior::isInitialGuess() const { return mode_ == "initial_guess"; } -inline bool Prior::isFactor() const -{ - return mode_ == "factor"; -} +inline bool Prior::isFactor() const { return mode_ == "factor"; } -inline const Eigen::VectorXd& Prior::getSigmaDriftStd() const -{ - return sigma_drift_std_; -} -} +inline const Eigen::VectorXd& Prior::getDriftStd() const { return drift_std_; } +} // namespace wolf #endif diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h index 23e2d82be991fee2a9ee1dcc0ae4616e2fe20d51..a94a0320418eb8cc8c4125d3cd3536d50689db73 100644 --- a/include/core/utils/params_server.h +++ b/include/core/utils/params_server.h @@ -35,39 +35,37 @@ public: MissingValueException(std::string _msg) : std::runtime_error(_msg) {} }; -class ParamsServer{ - std::map<std::string, std::string> params_; -public: - ParamsServer(); - ParamsServer(std::map<std::string, std::string> _params); - ~ParamsServer(){ - // - } - - void print(); - - - void addParam(std::string _key, std::string _value); - - void addParams(std::map<std::string, std::string> _params); - - // template<typename T> - // T getParam(std::string key, std::string def_value) const { - // if(params_.find(key) != params_.end()){ - // return converter<T>::convert(params_.find(key)->second); - // }else{ - // return converter<T>::convert(def_value); - // } - // } - - template<typename T> - T getParam(std::string _key) const { - if(params_.find(_key) != params_.end()){ - return converter<T>::convert(params_.find(_key)->second); - }else{ - throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server."); +class ParamsServer +{ + private: + + std::map<std::string, std::string> params_; + + public: + + ParamsServer(); + ParamsServer(std::map<std::string, std::string> _params); + ~ParamsServer() + { + // + } + + void print(); + + bool hasParam(std::string _key) const; + + void addParam(std::string _key, std::string _value); + + void addParams(std::map<std::string, std::string> _params); + + template<typename T> + T getParam(std::string _key) const + { + if(params_.count(_key)) + return converter<T>::convert(params_.find(_key)->second); + else + throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server."); } - } }; @@ -84,6 +82,11 @@ std::string to_string(const Eigen::DenseBase<Derived>& mat) std::string to_string(bool _arg); +inline bool ParamsServer::hasParam(std::string _key) const +{ + return params_.count(_key); +} + } #endif diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 8282a9c5c1a4fae1ac5b09585a827c93f3ea5cde..9b8f70f7ba6b6943c4a45d04386ea4cedb1042c3 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -138,11 +138,11 @@ void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim) // Factor if (prior.isFactor()) - addPriorParameter(key, prior.getState(), prior.getSigmaStd().cwiseAbs2().asDiagonal()); + addPriorParameter(key, prior.getState(), prior.getNoiseStd().cwiseAbs2().asDiagonal()); // Drift covariance rates if (prior.isDynamic()) - setDriftRateStd(key, prior.getSigmaDriftStd().cwiseAbs2().asDiagonal()); + setDriftStd(key, prior.getDriftStd().cwiseAbs2().asDiagonal()); } if (not hasStateBlock('P') or not hasStateBlock('O')) @@ -292,17 +292,16 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) noise_cov_ = _noise_cov; } -void SensorBase::setDriftRateStd(char _key, const Eigen::VectorXd & _drift_rate_std) +void SensorBase::setDriftStd(char _key, const Eigen::VectorXd & _drift_rate_std) { - drift_rate_covs_[_key] = _drift_rate_std.cwiseAbs2().asDiagonal(); + drift_covs_[_key] = _drift_rate_std.cwiseAbs2().asDiagonal(); } -void SensorBase::setDriftRateCov(char _key, const Eigen::MatrixXd & _drift_rate_cov) +void SensorBase::setDriftCov(char _key, const Eigen::MatrixXd & _drift_rate_cov) { - drift_rate_covs_[_key] = _drift_rate_cov; + drift_covs_[_key] = _drift_rate_cov; } - void SensorBase::setLastCapture(CaptureBasePtr cap) { assert(cap); diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp index 7517c27f3e7f238a9b2e3373fd499c1a7b7a4696..3849ccb814b9fc2aad7ed3a36ab289fb7e86f621 100644 --- a/src/state_block/prior.cpp +++ b/src/state_block/prior.cpp @@ -24,86 +24,82 @@ #include "core/state_block/factory_state_block.h" #include "core/utils/params_server.h" -namespace wolf { - -Prior::Prior(const std::string& _type, - const Eigen::VectorXd& _state, - const std::string& _mode, - const Eigen::VectorXd& _sigma, - bool _dynamic, - const Eigen::VectorXd& _sigma_drift_std) : - type_(_type), - mode_(_mode), - state_(_state), - sigma_std_(_sigma), - dynamic_(_dynamic), - sigma_drift_std_(_sigma_drift_std) +namespace wolf +{ +Prior::Prior(const std::string& _type, + const Eigen::VectorXd& _state, + const std::string& _mode, + const Eigen::VectorXd& _noise_std, + bool _dynamic, + const Eigen::VectorXd& _drift_std) + : type_(_type), mode_(_mode), state_(_state), noise_std_(_noise_std), dynamic_(_dynamic), drift_std_(_drift_std) { - check(); + check(); } Prior::Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server) { - type_ = _type; - state_ = _server.getParam<Eigen::VectorXd>(_prefix + "/state"); - mode_ = _server.getParam<std::string>(_prefix + "/mode"); + type_ = _type; + state_ = _server.getParam<Eigen::VectorXd>(_prefix + "/state"); + mode_ = _server.getParam<std::string>(_prefix + "/mode"); - if (mode_ == "factor") - sigma_std_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma"); - else - sigma_std_ = Eigen::VectorXd(0); + if (mode_ == "factor") + noise_std_ = _server.getParam<Eigen::VectorXd>(_prefix + "/noise_std"); + else + noise_std_ = Eigen::VectorXd(0); - dynamic_ = _server.getParam<bool>(_prefix + "/dynamic"); + dynamic_ = _server.getParam<bool>(_prefix + "/dynamic"); - if (dynamic_) - sigma_drift_std_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma_drift_std"); - else - sigma_drift_std_ = Eigen::VectorXd(0); + if (dynamic_ and _server.hasParam(_prefix + "/drift_std")) + drift_std_ = _server.getParam<Eigen::VectorXd>(_prefix + "/drift_std"); + else + drift_std_ = Eigen::VectorXd(0); - check(); + check(); } void Prior::check() const { - if (mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor") - throw std::runtime_error("Prior::check() wrong mode value: " + mode_ + ", it should be: 'initial_guess', 'fix' or 'factor'. " + print()); + if (mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor") + throw std::runtime_error("Prior::check() wrong mode value: " + mode_ + + ", it should be: 'initial_guess', 'fix' or 'factor'. " + print()); // try to create a state block and check for local parameterization and dimensions consistency StateBlockPtr sb; - try - { - sb = FactoryStateBlock::create(type_, state_, mode_ == "fix"); - } - catch(const std::exception& e) - { - throw std::runtime_error("Prior::check() could not create state block from prior with error: " + std::string(e.what()) + print()); + try { + sb = FactoryStateBlock::create(type_, state_, mode_ == "fix"); + } catch (const std::exception& e) { + throw std::runtime_error( + "Prior::check() could not create state block from prior with error: " + std::string(e.what()) + print()); } // check local param if (not sb->isValid()) - throw std::runtime_error("Prior::check() Prior " + type_ + " state is not valid (local param violation). " + print()); + throw std::runtime_error("Prior::check() Prior " + type_ + " state is not valid (local param violation). " + + print()); // check state size if (state_.size() != sb->getSize()) - throw std::runtime_error("Prior::check() Prior " + type_ + " state size different of state size. " + print()); + throw std::runtime_error("Prior::check() Prior " + type_ + " state size different of StateBlock size. " + print()); // check sigma size - if (mode_ == "factor" and sigma_std_.size() != sb->getLocalSize()) - throw std::runtime_error("Prior::check() Prior " + type_ + " sigma size different of state local size. " + print()); + if (mode_ == "factor" and noise_std_.size() != sb->getLocalSize()) + throw std::runtime_error("Prior::check() Prior " + type_ + " noise_std size different of StateBlock local size. " + + print()); // check sigma size - if (dynamic_ and sigma_drift_std_.size() != sb->getLocalSize()) - throw std::runtime_error("Prior::check() Prior " + type_ + " sigma_drift_std size different of state local size. " + print()); + if (dynamic_ and drift_std_.size() > 0 and drift_std_.size() != sb->getLocalSize()) + throw std::runtime_error("Prior::check() Prior " + type_ + + " drift_std size different of StateBlock local size. " + print()); } std::string Prior::print() const { - return "Prior " + type_ + "\n" - + "mode: " + mode_ + "\n" - + "state: " + to_string(state_) + "\n" - + (mode_ == "factor" ? "sigma: " + to_string(sigma_std_) + "\n" : "") - + "dynamic: " + to_string(dynamic_) + "\n" - + (dynamic_ ? "sigma_drift_std: "+ to_string(sigma_drift_std_) + "\n" : ""); + return "Prior " + type_ + "\n" + + "mode: " + mode_ + "\n" + + "state: " + to_string(state_) + "\n" + + (mode_ == "factor" ? "noise_std: " + to_string(noise_std_) + "\n" : "") + "dynamic: " + to_string(dynamic_) + + "\n" + (dynamic_ ? "drift_std: " + to_string(drift_std_) + "\n" : ""); } -} // namespace wolf \ No newline at end of file +} // namespace wolf \ No newline at end of file diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp index 5121b7acfaf85988282654259a33904f459a6dae..990c07d5dfe9caaa2a56df3618600987781c2d4f 100644 --- a/test/gtest_prior.cpp +++ b/test/gtest_prior.cpp @@ -43,9 +43,9 @@ struct PriorAsStruct std::string type; std::string mode; VectorXd state; - VectorXd sigma; + VectorXd noise_std; bool dynamic; - VectorXd sigma_drift; + VectorXd drift_std; }; void testPrior(const Prior& P, @@ -57,22 +57,18 @@ void testPrior(const Prior& P, if (pstruct.mode == "factor") { - ASSERT_MATRIX_APPROX(pstruct.sigma, P.getSigmaStd(), 1e-8); + ASSERT_MATRIX_APPROX(pstruct.noise_std, P.getNoiseStd(), 1e-8); } else { - ASSERT_EQ(0,P.getSigmaStd().size()); + ASSERT_EQ(0,P.getNoiseStd().size()); } ASSERT_EQ(pstruct.dynamic, P.isDynamic()); - if (pstruct.dynamic) + if (pstruct.drift_std.size() != 0) { - ASSERT_MATRIX_APPROX(pstruct.sigma_drift, P.getSigmaDriftStd(), 1e-8); - } - else - { - ASSERT_EQ(0,P.getSigmaDriftStd().size()); + ASSERT_MATRIX_APPROX(pstruct.drift_std, P.getDriftStd(), 1e-8); } }; @@ -85,14 +81,14 @@ void testPriors(const std::vector<PriorAsStruct>& setups, bool should_work) "\n\ttype: ", setup.type, "\n\tmode: ", setup.mode, "\n\tstate: ", setup.state.transpose(), - "\n\tsigma: ", setup.sigma.transpose(), + "\n\tnoise_std: ", setup.noise_std.transpose(), "\n\tdynamic: ", setup.dynamic, - "\n\tsigma_drift: ", setup.sigma_drift.transpose()); + "\n\tdrift_std: ", setup.drift_std.transpose()); if (should_work) - testPrior(Prior(setup.type, setup.state, setup.mode, setup.sigma, setup.dynamic, setup.sigma_drift), setup); + testPrior(Prior(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std), setup); else { - ASSERT_THROW(testPrior(Prior(setup.type, setup.state, setup.mode, setup.sigma, setup.dynamic, setup.sigma_drift), setup),std::runtime_error); + ASSERT_THROW(testPrior(Prior(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std), setup),std::runtime_error); } } } @@ -107,6 +103,8 @@ TEST(Prior, P) setups_death.push_back(PriorAsStruct({"P","initial_guess",vector4,vector0,false})); // wrong state size // Initial guess - dynamic + setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,true})); + setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector0})); setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,true,vector2})); setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector3})); setups_death.push_back(PriorAsStruct({"P","initial_guess",vector4,vector0,true,vector4})); // wrong state size @@ -118,6 +116,8 @@ TEST(Prior, P) setups_death.push_back(PriorAsStruct({"P","fix",vector4,vector0,false})); // wrong size // Fix - dynamic + setups_ok .push_back(PriorAsStruct({"P","fix",vector2,vector0,true,vector0})); + setups_ok .push_back(PriorAsStruct({"P","fix",vector3,vector0,true})); setups_ok .push_back(PriorAsStruct({"P","fix",vector2,vector0,true,vector2})); setups_ok .push_back(PriorAsStruct({"P","fix",vector3,vector0,true,vector3})); setups_death.push_back(PriorAsStruct({"P","fix",vector4,vector0,true,vector4})); // wrong state size @@ -130,6 +130,8 @@ TEST(Prior, P) setups_death.push_back(PriorAsStruct({"P","factor",vector2,vector3,false})); // inconsistent size // Factor - dynamic + setups_ok .push_back(PriorAsStruct({"P","factor",vector2,vector2,true})); + setups_ok .push_back(PriorAsStruct({"P","factor",vector3,vector3,true})); setups_ok .push_back(PriorAsStruct({"P","factor",vector2,vector2,true,vector2})); setups_ok .push_back(PriorAsStruct({"P","factor",vector3,vector3,true,vector3})); setups_death.push_back(PriorAsStruct({"P","factor",vector4,vector4,true,vector4})); // wrong state size @@ -152,6 +154,8 @@ TEST(Prior, O) setups_death.push_back(PriorAsStruct({"O","initial_guess",vector3,vector0,false})); // wrong size // Initial guess - dynamic + setups_ok .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,true})); + setups_ok .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector0})); setups_ok .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,true,vector1})); setups_ok .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector3})); setups_death.push_back(PriorAsStruct({"O","initial_guess",vector4,vector0,true,vector3})); // not normalized @@ -165,6 +169,8 @@ TEST(Prior, O) setups_death.push_back(PriorAsStruct({"O","fix",vector4,vector0,false})); // not normalized // Fix - dynamic + setups_ok .push_back(PriorAsStruct({"O","fix",vector1,vector0,true,vector0})); + setups_ok .push_back(PriorAsStruct({"O","fix",vectorq,vector0,true})); setups_ok .push_back(PriorAsStruct({"O","fix",vector1,vector0,true,vector1})); setups_ok .push_back(PriorAsStruct({"O","fix",vectorq,vector0,true,vector3})); setups_death.push_back(PriorAsStruct({"O","fix",vector4,vector0,true,vector3})); // not normalized @@ -179,6 +185,8 @@ TEST(Prior, O) setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector4,false})); // inconsistent size // Factor - dynamic + setups_ok .push_back(PriorAsStruct({"O","factor",vector1,vector1,true})); + setups_ok .push_back(PriorAsStruct({"O","factor",vectorq,vector3,true})); setups_ok .push_back(PriorAsStruct({"O","factor",vector1,vector1,true,vector1})); setups_ok .push_back(PriorAsStruct({"O","factor",vectorq,vector3,true,vector3})); setups_death.push_back(PriorAsStruct({"O","factor",vector4,vector3,true,vector3})); // not normalized @@ -204,6 +212,7 @@ TEST(Prior, StateBlock) setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector4,vector0,false})); // Initial guess - dynamic + setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,true})); setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,true,vector2})); setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,true,vector3})); setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector4,vector0,true,vector4})); @@ -215,6 +224,7 @@ TEST(Prior, StateBlock) setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector4,vector0,false})); // wrong size // Fix - dynamic + setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector2,vector0,true,vector0})); setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector2,vector0,true,vector2})); setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector3,vector0,true,vector3})); setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector4,vector0,true,vector4})); @@ -227,6 +237,7 @@ TEST(Prior, StateBlock) setups_death.push_back(PriorAsStruct({"StateBlock","factor",vector2,vector3,false})); // inconsistent size // Factor - dynamic + setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector2,vector2,true})); setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector2,vector2,true,vector2})); setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector3,vector3,true,vector3})); setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector4,vector4,true,vector4})); @@ -246,6 +257,7 @@ TEST(Prior, StateAngle) setups_death.push_back(PriorAsStruct({"StateAngle","initial_guess",vector3,vector0,false})); // wrong size // Initial guess - dynamic + setups_ok .push_back(PriorAsStruct({"StateAngle","initial_guess",vector1,vector0,true})); setups_ok .push_back(PriorAsStruct({"StateAngle","initial_guess",vector1,vector0,true,vector1})); setups_death.push_back(PriorAsStruct({"StateAngle","initial_guess",vector3,vector0,true,vector3})); // wrong size setups_death.push_back(PriorAsStruct({"StateAngle","initial_guess",vector1,vector0,true,vector4})); // inconsistent size @@ -255,6 +267,7 @@ TEST(Prior, StateAngle) setups_death.push_back(PriorAsStruct({"StateAngle","fix",vector3,vector0,false})); // wrong size // Fix - dynamic + setups_ok .push_back(PriorAsStruct({"StateAngle","fix",vector1,vector0,true,vector0})); setups_ok .push_back(PriorAsStruct({"StateAngle","fix",vector1,vector0,true,vector1})); setups_death.push_back(PriorAsStruct({"StateAngle","fix",vector3,vector0,true,vector3})); // wrong size setups_death.push_back(PriorAsStruct({"StateAngle","fix",vector1,vector0,true,vector4})); // inconsistent size @@ -265,6 +278,7 @@ TEST(Prior, StateAngle) setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector1,vector4,false})); // inconsistent size // Factor - dynamic + setups_ok .push_back(PriorAsStruct({"StateAngle","factor",vector1,vector1,true})); setups_ok .push_back(PriorAsStruct({"StateAngle","factor",vector1,vector1,true,vector1})); setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector3,vector3,true,vector3})); // wrong size setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector1,vector3,true,vector1})); // inconsistent size @@ -285,6 +299,7 @@ TEST(Prior, StateQuaternion) setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector3,vector0,false})); // wrong size // Initial guess - dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","initial_guess",vectorq,vector0,true})); setups_ok .push_back(PriorAsStruct({"StateQuaternion","initial_guess",vectorq,vector0,true,vector3})); setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector4,vector0,true,vector3})); // not normalized setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector3,vector0,true,vector3})); // wrong size @@ -296,6 +311,7 @@ TEST(Prior, StateQuaternion) setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector4,vector0,false})); // not normalized // Fix - dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","fix",vectorq,vector0,true,vector0})); setups_ok .push_back(PriorAsStruct({"StateQuaternion","fix",vectorq,vector0,true,vector3})); setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector4,vector0,true,vector3})); // not normalized setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector3,vector0,true,vector3})); // wrong size @@ -308,6 +324,7 @@ TEST(Prior, StateQuaternion) setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector4,false})); // inconsistent size // Factor - dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector3,true})); setups_ok .push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector3,true,vector3})); setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vector4,vector3,true,vector3})); // not normalized setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vector3,vector3,true,vector3})); // wrong size @@ -328,66 +345,77 @@ TEST(Prior, ParamsServer) std::vector<int> dims({2,3}); std::vector<std::string> modes({"initial_guess","fix","factor"}); std::vector<bool> dynamics({false,true}); + std::vector<bool> drifts({false,true}); - VectorXd p_state(4), o_state(4), po_sigma(4); + VectorXd p_state(4), o_state(4), po_std(4); p_state << 1, 2, 3, 4; o_state << 1, 0, 0, 0; - po_sigma << 0.1, 0.2, 0.3, 0.4; + po_std << 0.1, 0.2, 0.3, 0.4; // P & O for (auto key : keys) for (auto dim : dims) for (auto mode : modes) for (auto dynamic : dynamics) - { - std::string prefix = key + to_string(dim) + "_" + mode + (dynamic ? "_dynamic" : ""); - - WOLF_INFO("Creating prior from prefix ", prefix); - auto P = Prior(prefix, key, server); - - // Checks - ASSERT_EQ(P.getMode(), mode); - ASSERT_EQ(P.isDynamic(), dynamic); - - VectorXd state = p_state.head(dim); - VectorXd sigma = po_sigma.head(dim); - if (key == "O") + for (auto drift : drifts) { - state = o_state.head(dim == 2 ? 1 : 4); - sigma = po_sigma.head(dim == 2 ? 1 : 3); + // nonsense combination + if (not dynamic and drift) + continue; + + std::string prefix = key + to_string(dim) + "_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : ""); + + WOLF_INFO("Creating prior from prefix ", prefix); + auto P = Prior(prefix, key, server); + + // Checks + ASSERT_EQ(P.getMode(), mode); + ASSERT_EQ(P.isDynamic(), dynamic); + + VectorXd state = p_state.head(dim); + VectorXd noise_std = po_std.head(dim); + if (key == "O") + { + state = o_state.head(dim == 2 ? 1 : 4); + noise_std = po_std.head(dim == 2 ? 1 : 3); + } + + ASSERT_MATRIX_APPROX(P.getState(),state,wolf::Constants::EPS); + + if (drift) + { + ASSERT_MATRIX_APPROX(P.getDriftStd(),noise_std,wolf::Constants::EPS); + } + if (mode == "factor") + { + ASSERT_MATRIX_APPROX(P.getNoiseStd(),noise_std,wolf::Constants::EPS); + } } - ASSERT_MATRIX_APPROX(P.getState(),state,wolf::Constants::EPS); - - if (dynamic) - { - ASSERT_MATRIX_APPROX(P.getSigmaDriftStd(),sigma,wolf::Constants::EPS); - } - if (mode == "factor") - { - ASSERT_MATRIX_APPROX(P.getSigmaStd(),sigma,wolf::Constants::EPS); - } - } - // I for (auto mode : modes) for (auto dynamic : dynamics) - { - std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : ""); - WOLF_INFO("Creating prior from prefix ", prefix); - auto P = Prior(prefix, "StateBlock", server); - ASSERT_EQ(P.getMode(), mode); - ASSERT_EQ(P.isDynamic(), dynamic); - ASSERT_MATRIX_APPROX(P.getState(),p_state,wolf::Constants::EPS); - if (dynamic) - { - ASSERT_MATRIX_APPROX(P.getSigmaDriftStd(),po_sigma,wolf::Constants::EPS); - } - if (mode == "factor") + for (auto drift : drifts) { - ASSERT_MATRIX_APPROX(P.getSigmaStd(),po_sigma,wolf::Constants::EPS); + // nonsense combination + if (not dynamic and drift) + continue; + + std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : ""); + WOLF_INFO("Creating prior from prefix ", prefix); + auto P = Prior(prefix, "StateBlock", server); + ASSERT_EQ(P.getMode(), mode); + ASSERT_EQ(P.isDynamic(), dynamic); + ASSERT_MATRIX_APPROX(P.getState(),p_state,wolf::Constants::EPS); + if (drift) + { + ASSERT_MATRIX_APPROX(P.getDriftStd(),po_std,wolf::Constants::EPS); + } + if (mode == "factor") + { + ASSERT_MATRIX_APPROX(P.getNoiseStd(),po_std,wolf::Constants::EPS); + } } - } } TEST(Prior, ParamsServerWrong) @@ -399,32 +427,38 @@ TEST(Prior, ParamsServerWrong) std::vector<int> dims({2,3}); std::vector<std::string> modes({"initial_guess","fix","factor"}); std::vector<bool> dynamics({false,true}); - - VectorXd p_state(4), o_state(4), po_sigma(4); - p_state << 1, 2, 3, 4; - o_state << 1, 0, 0, 0; - po_sigma << 0.1, 0.2, 0.3, 0.4; + std::vector<bool> drifts({false,true}); // P & O for (auto key : keys) for (auto dim : dims) for (auto mode : modes) for (auto dynamic : dynamics) - { - std::string prefix = key + to_string(dim) + "_" + mode + (dynamic ? "_dynamic" : ""); + for (auto drift : drifts) + { + // nonsense combination + if (not dynamic and drift) + continue; - WOLF_INFO("Creating prior from prefix ", prefix); - ASSERT_THROW(auto P = Prior(prefix, key, server),std::runtime_error); - } + std::string prefix = key + to_string(dim) + "_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : ""); + + WOLF_INFO("Creating prior from prefix ", prefix); + ASSERT_THROW(auto P = Prior(prefix, key, server),std::runtime_error); + } // I for (auto mode : modes) for (auto dynamic : dynamics) - { - std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : ""); - WOLF_INFO("Creating prior from prefix ", prefix); - ASSERT_THROW(auto P = Prior(prefix, "StateBlock", server),std::runtime_error); - } + for (auto drift : drifts) + { + // nonsense combination + if (not dynamic and drift) + continue; + + std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : ""); + WOLF_INFO("Creating prior from prefix ", prefix); + ASSERT_THROW(auto P = Prior(prefix, "StateBlock", server),std::runtime_error); + } } int main(int argc, char **argv) diff --git a/test/yaml/params_prior.yaml b/test/yaml/params_prior.yaml index 17b77f93c4b8bc8aacc91db7f5ff2ed148bbbece..00c6f18916822a0f62a9436b2aff54ea14f1dcb7 100644 --- a/test/yaml/params_prior.yaml +++ b/test/yaml/params_prior.yaml @@ -16,27 +16,43 @@ config: P2_factor: mode: factor state: [1, 2] - sigma: [0.1, 0.2] + noise_std: [0.1, 0.2] dynamic: false + P2_initial_guess_dynamic_drift: + mode: initial_guess + state: [1, 2] + dynamic: true + drift_std: [0.1, 0.2] + + P2_fix_dynamic_drift: + mode: fix + state: [1, 2] + dynamic: true + drift_std: [0.1, 0.2] + + P2_factor_dynamic_drift: + mode: factor + state: [1, 2] + noise_std: [0.1, 0.2] + dynamic: true + drift_std: [0.1, 0.2] + P2_initial_guess_dynamic: mode: initial_guess state: [1, 2] dynamic: true - sigma_drift: [0.1, 0.2] P2_fix_dynamic: mode: fix state: [1, 2] dynamic: true - sigma_drift: [0.1, 0.2] P2_factor_dynamic: mode: factor state: [1, 2] - sigma: [0.1, 0.2] + noise_std: [0.1, 0.2] dynamic: true - sigma_drift: [0.1, 0.2] # P in 3D P3_initial_guess: @@ -52,27 +68,43 @@ config: P3_factor: mode: factor state: [1, 2, 3] - sigma: [0.1, 0.2, 0.3] + noise_std: [0.1, 0.2, 0.3] dynamic: false + P3_initial_guess_dynamic_drift: + mode: initial_guess + state: [1, 2, 3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + + P3_fix_dynamic_drift: + mode: fix + state: [1, 2, 3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + + P3_factor_dynamic_drift: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + P3_initial_guess_dynamic: mode: initial_guess state: [1, 2, 3] dynamic: true - sigma_drift: [0.1, 0.2, 0.3] P3_fix_dynamic: mode: fix state: [1, 2, 3] dynamic: true - sigma_drift: [0.1, 0.2, 0.3] P3_factor_dynamic: mode: factor state: [1, 2, 3] - sigma: [0.1, 0.2, 0.3] + noise_std: [0.1, 0.2, 0.3] dynamic: true - sigma_drift: [0.1, 0.2, 0.3] # O in 2D O2_initial_guess: @@ -88,27 +120,43 @@ config: O2_factor: mode: factor state: [1] - sigma: [0.1] + noise_std: [0.1] dynamic: false + O2_initial_guess_dynamic_drift: + mode: initial_guess + state: [1] + dynamic: true + drift_std: [0.1] + + O2_fix_dynamic_drift: + mode: fix + state: [1] + dynamic: true + drift_std: [0.1] + + O2_factor_dynamic_drift: + mode: factor + state: [1] + noise_std: [0.1] + dynamic: true + drift_std: [0.1] + O2_initial_guess_dynamic: mode: initial_guess state: [1] dynamic: true - sigma_drift: [0.1] O2_fix_dynamic: mode: fix state: [1] dynamic: true - sigma_drift: [0.1] O2_factor_dynamic: mode: factor state: [1] - sigma: [0.1] + noise_std: [0.1] dynamic: true - sigma_drift: [0.1] # O in 3D O3_initial_guess: @@ -124,27 +172,43 @@ config: O3_factor: mode: factor state: [1, 0, 0, 0] - sigma: [0.1, 0.2, 0.3] + noise_std: [0.1, 0.2, 0.3] dynamic: false + O3_initial_guess_dynamic_drift: + mode: initial_guess + state: [1, 0, 0, 0] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + + O3_fix_dynamic_drift: + mode: fix + state: [1, 0, 0, 0] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + + O3_factor_dynamic_drift: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + O3_initial_guess_dynamic: mode: initial_guess state: [1, 0, 0, 0] dynamic: true - sigma_drift: [0.1, 0.2, 0.3] O3_fix_dynamic: mode: fix state: [1, 0, 0, 0] dynamic: true - sigma_drift: [0.1, 0.2, 0.3] O3_factor_dynamic: mode: factor state: [1, 0, 0, 0] - sigma: [0.1, 0.2, 0.3] + noise_std: [0.1, 0.2, 0.3] dynamic: true - sigma_drift: [0.1, 0.2, 0.3] # I I_initial_guess: @@ -160,24 +224,40 @@ config: I_factor: mode: factor state: [1, 2, 3, 4] - sigma: [0.1, 0.2, 0.3, 0.4] + noise_std: [0.1, 0.2, 0.3, 0.4] dynamic: false + I_initial_guess_dynamic_drift: + mode: initial_guess + state: [1, 2, 3, 4] + dynamic: true + drift_std: [0.1, 0.2, 0.3, 0.4] + + I_fix_dynamic_drift: + mode: fix + state: [1, 2, 3, 4] + dynamic: true + drift_std: [0.1, 0.2, 0.3, 0.4] + + I_factor_dynamic_drift: + mode: factor + state: [1, 2, 3, 4] + noise_std: [0.1, 0.2, 0.3, 0.4] + dynamic: true + drift_std: [0.1, 0.2, 0.3, 0.4] + I_initial_guess_dynamic: mode: initial_guess state: [1, 2, 3, 4] dynamic: true - sigma_drift: [0.1, 0.2, 0.3, 0.4] I_fix_dynamic: mode: fix state: [1, 2, 3, 4] dynamic: true - sigma_drift: [0.1, 0.2, 0.3, 0.4] I_factor_dynamic: mode: factor state: [1, 2, 3, 4] - sigma: [0.1, 0.2, 0.3, 0.4] - dynamic: true - sigma_drift: [0.1, 0.2, 0.3, 0.4] \ No newline at end of file + noise_std: [0.1, 0.2, 0.3, 0.4] + dynamic: true \ No newline at end of file diff --git a/test/yaml/params_prior_wrong.yaml b/test/yaml/params_prior_wrong.yaml index 1d83bad82680c0deb719c1c4948bbbbb73449a44..31b919571be9a6e9bfa0cae6255d077ce2e037b5 100644 --- a/test/yaml/params_prior_wrong.yaml +++ b/test/yaml/params_prior_wrong.yaml @@ -16,27 +16,43 @@ config: P2_factor: mode: factor state: [1, 2] - sigma: [0.1, 0.2, 0.3] # wrong size + noise_std: [0.1, 0.2, 0.3] # wrong size dynamic: false P2_initial_guess_dynamic: mode: initial_guess state: [1, 2] - dynamic: true - #sigma_drift: [0.1, 0.2] #missing + #dynamic: true #missing P2_fix_dynamic: mode: fix - state: [1, 2] + state: [1] # wrong size dynamic: true - sigma_drift: [0.1, 0.2, 0.3] # wrong size P2_factor_dynamic: mode: factor state: [1, 2] - #sigma: [0.1, 0.2] # missing + #noise_std: [0.1, 0.2] # missing + dynamic: true + + P2_initial_guess_dynamic_drift: + mode: initial_guess + state: [1, 2] + dynamic: true + #drift_std: [0.1, 0.2] #missing + + P2_fix_dynamic_drift: + mode: fix + state: [1, 2] + dynamic: true + drift_std: [0.1, 0.2, 0.3] # wrong size + + P2_factor_dynamic_drift: + mode: factor + state: [1, 2] + #noise_std: [0.1, 0.2] # missing dynamic: true - sigma_drift: [0.1, 0.2] + drift_std: [0.1, 0.2] # P in 3D P3_initial_guess: @@ -52,27 +68,46 @@ config: P3_factor: mode: factor state: [1, 2, 3] - sigma: [0.1, 0.2] # wrong size + noise_std: [0.1, 0.2] # wrong size dynamic: false P3_initial_guess_dynamic: mode: initial_guess state: [1, 2, 3] dynamic: true - sigma_drift: [0.1, 0.2] # wrong size + drift_std: [0.1, 0.2] # wrong size P3_fix_dynamic: mode: fix state: [1, 2, 3] dynamic: true - #sigma_drift: [0.1, 0.2] # missing + #drift_std: [0.1, 0.2] # missing P3_factor_dynamic: mode: factor state: [1, 2, 3] - sigma: [0.1, 0.2] # wrong size + noise_std: [0.1, 0.2] # wrong size dynamic: true - sigma_drift: [0.1, 0.2, 0.3] + drift_std: [0.1, 0.2, 0.3] + + P3_initial_guess_dynamic_drift: + mode: initial_guess + state: [1, 2, 3] + dynamic: true + drift_std: [0.1, 0.2] # wrong size + + P3_fix_dynamic_drift: + mode: fix + state: [1, 2, 3] + dynamic: true + #drift_std: [0.1, 0.2] # missing + + P3_factor_dynamic_drift: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2] # wrong size + dynamic: true + drift_std: [0.1, 0.2, 0.3] # O in 2D O2_initial_guess: @@ -88,27 +123,46 @@ config: O2_factor: mode: factor state: [1] - sigma: [0.1, 0.2] # wrong size + noise_std: [0.1, 0.2] # wrong size dynamic: false O2_initial_guess_dynamic: mode: initial_guess state: [1] dynamic: true - #sigma_drift: [0.1] # missing + #drift_std: [0.1] # missing O2_fix_dynamic: mode: fix state: [1 2] # wrong size dynamic: true - sigma_drift: [0.1] + drift_std: [0.1] O2_factor_dynamic: mode: factor state: [1] - sigma: [0.1 0.2] # wrong size + noise_std: [0.1 0.2] # wrong size + dynamic: true + drift_std: [0.1] + + O2_initial_guess_dynamic_drift: + mode: initial_guess + state: [1] + dynamic: true + #drift_std: [0.1] # missing + + O2_fix_dynamic_drift: + mode: fix + state: [1 2] # wrong size + dynamic: true + drift_std: [0.1] + + O2_factor_dynamic_drift: + mode: factor + state: [1] + noise_std: [0.1 0.2] # wrong size dynamic: true - sigma_drift: [0.1] + drift_std: [0.1] # O in 3D O3_initial_guess: @@ -124,27 +178,46 @@ config: O3_factor: mode: factor state: [1, 0, 0, 0] - sigma: [0.1, 0.2] # wrong size + noise_std: [0.1, 0.2] # wrong size dynamic: false O3_initial_guess_dynamic: mode: initial_guess state: [1, 0, 0] # wrong size dynamic: true - sigma_drift: [0.1, 0.2, 0.3] + drift_std: [0.1, 0.2, 0.3] O3_fix_dynamic: mode: fix state: [1, 0, 0, 0] dynamic: true - #sigma_drift: [0.1, 0.2, 0.3] # missing + #drift_std: [0.1, 0.2, 0.3] # missing O3_factor_dynamic: mode: factor state: [1, 0, 0, 0] - sigma: [0.1, 0.2, 0.3] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + drift_std: [0.1, 0.2] # wrong size + + O3_initial_guess_dynamic_drift: + mode: initial_guess + state: [1, 0, 0] # wrong size + dynamic: true + drift_std: [0.1, 0.2, 0.3] + + O3_fix_dynamic_drift: + mode: fix + state: [1, 0, 0, 0] dynamic: true - sigma_drift: [0.1, 0.2] # wrong size + #drift_std: [0.1, 0.2, 0.3] # missing + + O3_factor_dynamic_drift: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + drift_std: [0.1, 0.2] # wrong size # I I_initial_guess: @@ -160,24 +233,43 @@ config: I_factor: mode: factor state: [1, 2, 3, 4] - sigma: [0.1, 0.2, 0.3] # wrong size + noise_std: [0.1, 0.2, 0.3] # wrong size dynamic: false I_initial_guess_dynamic: mode: initial_guess state: [1, 2, 3, 4] dynamic: true - sigma_drift: [0.1, 0.2, 0.3] # wrong size + drift_std: [0.1, 0.2, 0.3] # wrong size I_fix_dynamic: mode: fix state: [1, 2, 3] # wrong size dynamic: true - sigma_drift: [0.1, 0.2, 0.3, 0.4] + drift_std: [0.1, 0.2, 0.3, 0.4] I_factor_dynamic: mode: factor state: [1, 2, 3, 4] - sigma: [0.1, 0.2, 0.3] # wrong size + noise_std: [0.1, 0.2, 0.3] # wrong size + dynamic: true + drift_std: [0.1, 0.2, 0.3, 0.4] + + I_initial_guess_dynamic_drift: + mode: initial_guess + state: [1, 2, 3, 4] + dynamic: true + drift_std: [0.1, 0.2, 0.3] # wrong size + + I_fix_dynamic_drift: + mode: fix + state: [1, 2, 3] # wrong size + dynamic: true + drift_std: [0.1, 0.2, 0.3, 0.4] + + I_factor_dynamic_drift: + mode: factor + state: [1, 2, 3, 4] + noise_std: [0.1, 0.2, 0.3] # wrong size dynamic: true - sigma_drift: [0.1, 0.2, 0.3, 0.4] \ No newline at end of file + drift_std: [0.1, 0.2, 0.3, 0.4] \ No newline at end of file