diff --git a/include/core/math/covariance.h b/include/core/math/covariance.h index acb4c086282ea48d37c0394aab2c18b75daaab0d..06ae067ccb0e7be12ead33c56839efc15256d19e 100644 --- a/include/core/math/covariance.h +++ b/include/core/math/covariance.h @@ -37,6 +37,8 @@ template <typename T, int N, int RC> inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M, const T eps = wolf::Constants::EPS) { + if (M.cols() != M.rows()) + return false; return M.isApprox(M.transpose(), eps); } diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index f57725decd14bc50d4bec57936a8d2de5bf8c316..8bb25c437970d14dc7cb48b30567b990c0ae386b 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -128,8 +128,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree) protected: - Eigen::VectorXd noise_std_; // std of sensor noise - Eigen::MatrixXd noise_cov_; // cov matrix of noise + 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 void setProblem(ProblemPtr _problem) override final; virtual void loadParams(ParamsSensorBasePtr _params); @@ -163,7 +163,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh const std::string& _unique_name, const SizeEigen& _dim, const ParamsServer& _server, - std::unordered_map<char, std::string> _keys_types); + const std::unordered_map<char, std::string>& _keys_types_apart_from_PO={}); ~SensorBase() override; @@ -245,23 +245,24 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh void addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, - unsigned int _start_idx = 0, - int _size = -1); + unsigned int _start_idx = 0); void addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, - unsigned int _start_idx = 0, - int _size = -1); + unsigned int _start_idx = 0); void addPriorO(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov); void addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, - unsigned int _start_idx = 0, - int _size = -1); + unsigned int _start_idx = 0); void setNoiseStd(const Eigen::VectorXd & _noise_std); - void setNoiseCov(const Eigen::MatrixXd & _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); Eigen::VectorXd getNoiseStd() const; Eigen::MatrixXd getNoiseCov() const; + Eigen::VectorXd getDriftRateStd(char) const; + Eigen::MatrixXd getDriftRateCov(char) const; virtual void printHeader(int depth, // bool constr_by, // @@ -353,7 +354,7 @@ inline bool SensorBase::isStateBlockDynamic(const char& _key) const inline Eigen::VectorXd SensorBase::getNoiseStd() const { - return noise_std_; + return noise_cov_.diagonal().cwiseSqrt(); } inline Eigen::MatrixXd SensorBase::getNoiseCov() const @@ -361,6 +362,25 @@ inline Eigen::MatrixXd SensorBase::getNoiseCov() const return noise_cov_; } +inline Eigen::VectorXd SensorBase::getDriftRateStd(char _key) const +{ + if (drift_rate_covs_.count(_key) == 0) + return Eigen::MatrixXd(0,0); + else + return drift_rate_covs_.at(_key).diagonal().cwiseSqrt(); +} + +inline Eigen::MatrixXd SensorBase::getDriftRateCov(char _key) const +{ + if (drift_rate_covs_.count(_key) == 0) + return Eigen::MatrixXd(0,0); + else + return drift_rate_covs_.at(_key); +} + +void setDriftRateStd(char, const Eigen::VectorXd & _drift_rate_std); +void setDriftRateCov(char, const Eigen::MatrixXd & _drift_rate_cov); + inline HardwareBaseConstPtr SensorBase::getHardware() const { return hardware_ptr_.lock(); @@ -376,9 +396,9 @@ inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr) hardware_ptr_ = _hw_ptr; } -inline void SensorBase::addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size) +inline void SensorBase::addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx) { - addPriorParameter('P', _x, _cov, _start_idx, _size); + addPriorParameter('P', _x, _cov, _start_idx); } inline void SensorBase::addPriorO(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov) @@ -386,9 +406,9 @@ inline void SensorBase::addPriorO(const Eigen::VectorXd& _x, const Eigen::Matrix addPriorParameter('O', _x, _cov); } -inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size) +inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx) { - addPriorParameter('I', _x, _cov); + addPriorParameter('I', _x, _cov, _start_idx); } } // namespace wolf diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h index e1bab00d989329004bef89c074b9c95e07be7ced..8f422da10bb8ad37fa2714c4db7a3c025cc33b6f 100644 --- a/include/core/state_block/prior.h +++ b/include/core/state_block/prior.h @@ -38,21 +38,21 @@ typedef std::unordered_map<char, Prior> Priors; class Prior { private: - std::string type_; // State type - std::string mode_; // Prior mode. Can be 'nothing', 'initial_guess', 'fix' and 'factor' - Eigen::VectorXd state_; // state values (only filled from server if mode != 'nothing') - Eigen::VectorXd sigma_; // factor sigmas (only filled from server if mode == 'factor') - bool dynamic_; // State dynamic - Eigen::VectorXd sigma_drift_; // drift of the state (only filled from server if dynamic) + 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: Prior() = default; Prior(const std::string& _type, - const std::string& _mode, - const Eigen::VectorXd& _state, - const Eigen::VectorXd& _sigma = Eigen::VectorXd(0), - bool _dynamic = false, - const Eigen::VectorXd& _sigma_drift = Eigen::VectorXd(0)); + 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)); Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server); @@ -61,12 +61,12 @@ class Prior const std::string& getType() const; const std::string& getMode() const; const Eigen::VectorXd& getState() const; - const Eigen::VectorXd& getSigma() const; + const Eigen::VectorXd& getSigmaStd() const; bool isDynamic() const; bool isFixed() const; bool isInitialGuess() const; bool isFactor() const; - const Eigen::VectorXd& getSigmaDrift() const; + const Eigen::VectorXd& getSigmaDriftStd() const; void check() const; @@ -88,9 +88,9 @@ inline const Eigen::VectorXd& Prior::getState() const return state_; } -inline const Eigen::VectorXd& Prior::getSigma() const +inline const Eigen::VectorXd& Prior::getSigmaStd() const { - return sigma_; + return sigma_std_; } inline bool Prior::isDynamic() const @@ -113,9 +113,9 @@ inline bool Prior::isFactor() const return mode_ == "factor"; } -inline const Eigen::VectorXd& Prior::getSigmaDrift() const +inline const Eigen::VectorXd& Prior::getSigmaDriftStd() const { - return sigma_drift_; + return sigma_drift_std_; } } #endif diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index dc92cbe1d59a68c106d041fa0049e07f65a08903..8282a9c5c1a4fae1ac5b09585a827c93f3ea5cde 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -27,6 +27,7 @@ #include "core/state_block/state_quaternion.h" #include "core/factor/factor_block_absolute.h" #include "core/factor/factor_quaternion_absolute.h" +#include "core/math/covariance.h" namespace wolf { @@ -58,7 +59,7 @@ SensorBase::SensorBase(const std::string& _type, const std::string& _unique_name, const SizeEigen& _dim, const ParamsServer& _server, - std::unordered_map<char, std::string> _keys_types) : + const std::unordered_map<char, std::string>& _keys_types_apart_from_PO) : NodeBase("SENSOR", _type, _unique_name), HasStateBlocks(""), hardware_ptr_(), @@ -67,6 +68,10 @@ SensorBase::SensorBase(const std::string& _type, params_prior_map_(), last_capture_(nullptr) { + assert(_keys_types_apart_from_PO.count('P') == 0 and + _keys_types_apart_from_PO.count('O') == 0 and + "SensorBase: _keys_types_apart_from_PO should not contain 'P' or 'O' keys"); + // create params auto params = std::make_shared<ParamsSensorBase>("sensor/" + _unique_name, _server); @@ -74,8 +79,11 @@ SensorBase::SensorBase(const std::string& _type, loadParams(params); // create priors + std::unordered_map<char, std::string> keys_types = _keys_types_apart_from_PO; + keys_types['P'] = "P"; // equivalent to "StateBlock"; + keys_types['O'] = "O"; // equivalent to (_dim == 2 ? "StateAngle" : "StateQuaternion"); Priors priors; - for (auto pair : _keys_types) + for (auto pair : keys_types) priors[pair.first] = Prior("sensor/" + _unique_name, pair.second, _server); // load priors @@ -102,29 +110,43 @@ void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim) auto key = prior_pair.first; const Prior& prior = prior_pair.second; - // Checks - if (key == 'P' and prior.getState().size() != _dim) - throw std::runtime_error("Prior state for P has wrong size"); + // Check consistency of keys + if (key == 'P' and prior.getType() != "P" and prior.getType() != "StateBlock") + throw std::runtime_error("Prior type for key P has to be 'P' or 'StateBlock'"); + if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateBlock") + throw std::runtime_error("Prior type for key V has to be 'V' or 'StateBlock'"); + if (key == 'O' and _dim == 2 and prior.getType() != "O" and prior.getType() != "StateAngle") + throw std::runtime_error("Prior type for key O in 2D has to be 'O' or 'StateAngle'"); + if (key == 'O' and _dim == 3 and prior.getType() != "O" and prior.getType() != "StateQuaternion") + throw std::runtime_error("Prior type for key O in 3D has to be 'O' or 'StateQuaternion'"); + + // Checks state size (P, O, V) + if ((key == 'P' or key == 'V') and prior.getState().size() != _dim) + throw std::runtime_error("Prior state for P or V has wrong size: " + to_string(prior.getState().size()) + " in " + (_dim == 2 ? "2D" : "3D")); if (key == '0' and prior.getState().size() != (_dim == 2 ? 1 : 4)) - throw std::runtime_error("Prior state for O has wrong size"); + throw std::runtime_error("Prior state for O has wrong size: " + to_string(prior.getState().size()) + " in " + (_dim == 2 ? "2D" : "3D")); // create state block auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed()); - // check local param - if (not sb->isValid()) - { - WOLF_ERROR("The created stateblock ", prior.getType(), " for key ", std::string(1,key), " is not valid (local param violation)"); - throw std::runtime_error("StateBlock not valid"); - } + // check local param + if (not sb->isValid()) // always false I think... + throw std::runtime_error("The created stateblock " + prior.getType() + " for key " + std::string(1,key) + " is not valid (local param violation)"); // Add state block addStateBlock(key, sb, prior.isDynamic()); // Factor if (prior.isFactor()) - addPriorParameter(key, prior.getState(), prior.getSigma()); + addPriorParameter(key, prior.getState(), prior.getSigmaStd().cwiseAbs2().asDiagonal()); + + // Drift covariance rates + if (prior.isDynamic()) + setDriftRateStd(key, prior.getSigmaDriftStd().cwiseAbs2().asDiagonal()); } + + if (not hasStateBlock('P') or not hasStateBlock('O')) + throw std::runtime_error("SensorBase prior should have at least 'P' and 'O' keys defined"); } void SensorBase::fixExtrinsics() @@ -173,26 +195,54 @@ void SensorBase::unfixIntrinsics() } } -void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size) +void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx) { - assert(!isStateBlockInCapture(_key) && "SensorBase::addPriorParameter only allowed for static parameters"); + assert(isCovariance(_cov) and "SensorBase::addPriorParameter: provided _cov is not a covariance matrix"); StateBlockPtr _sb = getStateBlock(_key); - bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr); - assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) || - (is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions"); - assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize())); - assert(_size == -1 || _size == _x.size()); - assert(!(_size != -1 && is_quaternion) && "prior of a segment of state not available for quaternion"); + // CHECKS + // missing key + if (not _sb) + throw std::runtime_error("SensorBase::addPriorParameter: missing key " + std::string(1,_key)); + + // Overparametrized states + if (_sb->getSize() != _sb->getLocalSize()) + { + // not segment of state + if (_start_idx != 0) + throw std::runtime_error("SensorBase::addPriorParameter: Prior for a segment of a state with local size different from size not allowed"); + // state size + if (_x.size() != _sb->getSize()) + throw std::runtime_error("SensorBase::addPriorParameter: Bad state size"); + // cov size + if (_cov.cols() != _sb->getLocalSize()) + throw std::runtime_error("SensorBase::addPriorParameter: Bad covariance size"); + } + else + { + // cov and state size + if (_cov.cols() != _x.size()) + throw std::runtime_error("SensorBase::addPriorParameter: Inconsistend measurement and covariance sizes"); + + // state size + if (_x.size() + _start_idx > _sb->getSize()) + throw std::runtime_error("SensorBase::addPriorParameter: Prior for a segment of a state with local size different from size not allowed"); + } + + // dynamic state blocks + if (isStateBlockInCapture(_key)) + throw std::runtime_error("SensorBase::addPriorParameter only allowed for static parameters"); + + // APPLY PRIOR // set StateBlock state - if (_size == -1) + if (_x.size() == _sb->getSize()) _sb->setState(_x); else { Eigen::VectorXd new_x = _sb->getState(); - new_x.segment(_start_idx,_size) = _x; + new_x.segment(_start_idx,_x.size()) = _x; _sb->setState(new_x); } @@ -201,16 +251,17 @@ void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, params_prior_map_[_key]->remove(); // create feature - FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov); //std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov); + FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov); // set feature problem - if(getProblem()) ftr_prior->setProblem(getProblem()); + ftr_prior->setProblem(getProblem()); // create & add factor absolute + bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr); if (is_quaternion) FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, ftr_prior, _sb, nullptr, false); else - FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, ftr_prior, _sb, _start_idx, _size, nullptr, false); + FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, ftr_prior, _sb, _start_idx, _x.size(), nullptr, false); // store feature in params_prior_map_ params_prior_map_[_key] = ftr_prior; @@ -233,16 +284,25 @@ void SensorBase::registerNewStateBlocks(ProblemPtr _problem) void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std) { - noise_std_ = _noise_std; noise_cov_ = _noise_std.array().square().matrix().asDiagonal(); } void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) { - noise_std_ = _noise_cov.diagonal().array().sqrt(); noise_cov_ = _noise_cov; } +void SensorBase::setDriftRateStd(char _key, const Eigen::VectorXd & _drift_rate_std) +{ + drift_rate_covs_[_key] = _drift_rate_std.cwiseAbs2().asDiagonal(); +} + +void SensorBase::setDriftRateCov(char _key, const Eigen::MatrixXd & _drift_rate_cov) +{ + drift_rate_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 172d49aaa05c816cbf19bacac25c8a372118fbd7..7517c27f3e7f238a9b2e3373fd499c1a7b7a4696 100644 --- a/src/state_block/prior.cpp +++ b/src/state_block/prior.cpp @@ -27,17 +27,17 @@ namespace wolf { Prior::Prior(const std::string& _type, - const std::string& _mode, const Eigen::VectorXd& _state, + const std::string& _mode, const Eigen::VectorXd& _sigma, bool _dynamic, - const Eigen::VectorXd& _sigma_drift) : + const Eigen::VectorXd& _sigma_drift_std) : type_(_type), mode_(_mode), state_(_state), - sigma_(_sigma), + sigma_std_(_sigma), dynamic_(_dynamic), - sigma_drift_(_sigma_drift) + sigma_drift_std_(_sigma_drift_std) { check(); } @@ -45,35 +45,28 @@ Prior::Prior(const std::string& _type, 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"); - if (mode_ != "nothing") - state_ = _server.getParam<Eigen::VectorXd>(_prefix + "/state"); - else - state_ = Eigen::VectorXd(0); - if (mode_ == "factor") - sigma_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma"); + sigma_std_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma"); else - sigma_ = Eigen::VectorXd(0); + sigma_std_ = Eigen::VectorXd(0); dynamic_ = _server.getParam<bool>(_prefix + "/dynamic"); if (dynamic_) - sigma_drift_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma_drift"); + sigma_drift_std_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma_drift_std"); else - sigma_drift_ = Eigen::VectorXd(0); + sigma_drift_std_ = Eigen::VectorXd(0); check(); } void Prior::check() const { - if (mode_ != "nothing" and mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor") - throw std::runtime_error("Prior::check() wrong mode value: " + mode_ + ", it should be: 'nothing', 'initial_guess', 'fix' or 'factor'. " + print()); - - if (mode_ == "nothing" and state_.size() == 0) - return; + 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; @@ -91,16 +84,16 @@ void Prior::check() const throw std::runtime_error("Prior::check() Prior " + type_ + " state is not valid (local param violation). " + print()); // check state size - if (mode_ != "nothing" and state_.size() != sb->getSize()) + if (state_.size() != sb->getSize()) throw std::runtime_error("Prior::check() Prior " + type_ + " state size different of state size. " + print()); // check sigma size - if (mode_ == "factor" and sigma_.size() != sb->getLocalSize()) + 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()); // check sigma size - if (dynamic_ and sigma_drift_.size() != sb->getLocalSize()) - throw std::runtime_error("Prior::check() Prior " + type_ + " sigma_drift size different of state local size. " + print()); + 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()); } std::string Prior::print() const @@ -108,9 +101,9 @@ std::string Prior::print() const return "Prior " + type_ + "\n" + "mode: " + mode_ + "\n" + "state: " + to_string(state_) + "\n" - + (mode_ == "factor" ? "sigma: " + to_string(sigma_) + "\n" : "") + + (mode_ == "factor" ? "sigma: " + to_string(sigma_std_) + "\n" : "") + "dynamic: " + to_string(dynamic_) + "\n" - + (dynamic_ ? "sigma_drift: "+ to_string(sigma_drift_) + "\n" : ""); + + (dynamic_ ? "sigma_drift_std: "+ to_string(sigma_drift_std_) + "\n" : ""); } } // namespace wolf \ No newline at end of file diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp index 6332a69928a558cfdede9125a73fd7ab7f835338..8c15d647982fe4cefaa98ecf07e68d860edb4984 100644 --- a/test/gtest_factory_state_block.cpp +++ b/test/gtest_factory_state_block.cpp @@ -162,7 +162,7 @@ TEST(FactoryStateBlock, creator_O_is_angle) TEST(FactoryStateBlock, creator_O_is_wrong_size) { - ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::length_error); + ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::runtime_error); } int main(int argc, char **argv) diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp index 43a507e274269f9ce19cbc07fa2c42c6a1670592..5121b7acfaf85988282654259a33904f459a6dae 100644 --- a/test/gtest_prior.cpp +++ b/test/gtest_prior.cpp @@ -57,22 +57,22 @@ void testPrior(const Prior& P, if (pstruct.mode == "factor") { - ASSERT_MATRIX_APPROX(pstruct.sigma, P.getSigma(), 1e-8); + ASSERT_MATRIX_APPROX(pstruct.sigma, P.getSigmaStd(), 1e-8); } else { - ASSERT_EQ(0,P.getSigma().size()); + ASSERT_EQ(0,P.getSigmaStd().size()); } ASSERT_EQ(pstruct.dynamic, P.isDynamic()); if (pstruct.dynamic) { - ASSERT_MATRIX_APPROX(pstruct.sigma_drift, P.getSigmaDrift(), 1e-8); + ASSERT_MATRIX_APPROX(pstruct.sigma_drift, P.getSigmaDriftStd(), 1e-8); } else { - ASSERT_EQ(0,P.getSigmaDrift().size()); + ASSERT_EQ(0,P.getSigmaDriftStd().size()); } }; @@ -89,10 +89,10 @@ void testPriors(const std::vector<PriorAsStruct>& setups, bool should_work) "\n\tdynamic: ", setup.dynamic, "\n\tsigma_drift: ", setup.sigma_drift.transpose()); if (should_work) - testPrior(Prior(setup.type, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup); + testPrior(Prior(setup.type, setup.state, setup.mode, setup.sigma, setup.dynamic, setup.sigma_drift), setup); else { - ASSERT_THROW(testPrior(Prior(setup.type, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup),std::runtime_error); + ASSERT_THROW(testPrior(Prior(setup.type, setup.state, setup.mode, setup.sigma, setup.dynamic, setup.sigma_drift), setup),std::runtime_error); } } } @@ -101,17 +101,6 @@ TEST(Prior, P) { std::vector<PriorAsStruct> setups_ok, setups_death; - // Nothing - not dynamic (not allowed for P and O) - setups_ok .push_back(PriorAsStruct({"P","nothing",vector2,vector0,false})); - setups_ok .push_back(PriorAsStruct({"P","nothing",vector3,vector0,false})); - setups_death.push_back(PriorAsStruct({"P","nothing",vector4,vector0,false})); // wrong state size - - // Nothing - dynamic - setups_ok .push_back(PriorAsStruct({"P","nothing",vector2,vector0,true,vector2})); - setups_ok .push_back(PriorAsStruct({"P","nothing",vector3,vector0,true,vector3})); - setups_death.push_back(PriorAsStruct({"P","nothing",vector4,vector0,true,vector4})); // wrong state size - setups_death.push_back(PriorAsStruct({"P","nothing",vector3,vector0,true,vector4})); // inconsistent size - // Initial guess - not dynamic setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,false})); setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,false})); @@ -156,19 +145,6 @@ TEST(Prior, O) { std::vector<PriorAsStruct> setups_ok, setups_death; - // nothing - not dynamic - setups_ok .push_back(PriorAsStruct({"O","nothing",vector1,vector0,false})); - setups_ok .push_back(PriorAsStruct({"O","nothing",vectorq,vector0,false})); - setups_death.push_back(PriorAsStruct({"O","nothing",vector4,vector0,false})); // not normalized - setups_death.push_back(PriorAsStruct({"O","nothing",vector3,vector0,false})); // wrong size - - // Nothing - dynamic - setups_ok .push_back(PriorAsStruct({"O","nothing",vector1,vector0,true,vector1})); - setups_ok .push_back(PriorAsStruct({"O","nothing",vectorq,vector0,true,vector3})); - setups_death.push_back(PriorAsStruct({"O","nothing",vector4,vector0,true,vector3})); // not normalized - setups_death.push_back(PriorAsStruct({"O","nothing",vector3,vector0,true,vector3})); // wrong size - setups_death.push_back(PriorAsStruct({"O","nothing",vectorq,vector0,true,vector4})); // inconsistent size - // Initial guess - not dynamic setups_ok .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,false})); setups_ok .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,false})); @@ -220,19 +196,8 @@ TEST(Prior, StateBlock) std::vector<PriorAsStruct> setups_ok, setups_death; // Unknown type -> StateBlock - setups_ok .push_back(PriorAsStruct({"K","nothing",vector1,vector0,false})); - - // Nothing - not dynamic (not allowed for P and O) - setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector2,vector0,false})); - setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector3,vector0,false})); - setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector4,vector0,false})); - - // Nothing - dynamic - setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector2,vector0,true,vector2})); - setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector3,vector0,true,vector3})); - setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector4,vector0,true,vector4})); - setups_death.push_back(PriorAsStruct({"StateBlock","nothing",vector3,vector0,true,vector4})); // inconsistent size - + setups_ok .push_back(PriorAsStruct({"K","initial_guess",vector1,vector0,false})); + // Initial guess - not dynamic setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,false})); setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,false})); @@ -276,15 +241,6 @@ TEST(Prior, StateAngle) { std::vector<PriorAsStruct> setups_ok, setups_death; - // nothing - not dynamic - setups_ok .push_back(PriorAsStruct({"StateAngle","nothing",vector1,vector0,false})); - setups_death.push_back(PriorAsStruct({"StateAngle","nothing",vector3,vector0,false})); // wrong size - - // Nothing - dynamic - setups_ok .push_back(PriorAsStruct({"StateAngle","nothing",vector1,vector0,true,vector1})); - setups_death.push_back(PriorAsStruct({"StateAngle","nothing",vector3,vector0,true,vector3})); // wrong size - setups_death.push_back(PriorAsStruct({"StateAngle","nothing",vector1,vector0,true,vector4})); // inconsistent size - // Initial guess - not dynamic setups_ok .push_back(PriorAsStruct({"StateAngle","initial_guess",vector1,vector0,false})); setups_death.push_back(PriorAsStruct({"StateAngle","initial_guess",vector3,vector0,false})); // wrong size @@ -323,17 +279,6 @@ TEST(Prior, StateQuaternion) { std::vector<PriorAsStruct> setups_ok, setups_death; - // nothing - not dynamic - setups_ok .push_back(PriorAsStruct({"StateQuaternion","nothing",vectorq,vector0,false})); - setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector4,vector0,false})); // not normalized - setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector3,vector0,false})); // wrong size - - // Nothing - dynamic - setups_ok .push_back(PriorAsStruct({"StateQuaternion","nothing",vectorq,vector0,true,vector3})); - setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector4,vector0,true,vector3})); // not normalized - setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector3,vector0,true,vector3})); // wrong size - setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vectorq,vector0,true,vector4})); // inconsistent size - // Initial guess - not dynamic setups_ok .push_back(PriorAsStruct({"StateQuaternion","initial_guess",vectorq,vector0,false})); setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector4,vector0,false})); // not normalized @@ -381,7 +326,7 @@ TEST(Prior, ParamsServer) std::vector<std::string> keys({"P","O"}); std::vector<int> dims({2,3}); - std::vector<std::string> modes({"nothing","initial_guess","fix","factor"}); + 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); @@ -412,17 +357,15 @@ TEST(Prior, ParamsServer) sigma = po_sigma.head(dim == 2 ? 1 : 3); } - if (mode != "nothing") - { - ASSERT_MATRIX_APPROX(P.getState(),state,wolf::Constants::EPS); - } + ASSERT_MATRIX_APPROX(P.getState(),state,wolf::Constants::EPS); + if (dynamic) { - ASSERT_MATRIX_APPROX(P.getSigmaDrift(),sigma,wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(P.getSigmaDriftStd(),sigma,wolf::Constants::EPS); } if (mode == "factor") { - ASSERT_MATRIX_APPROX(P.getSigma(),sigma,wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(P.getSigmaStd(),sigma,wolf::Constants::EPS); } } @@ -435,17 +378,14 @@ TEST(Prior, ParamsServer) auto P = Prior(prefix, "StateBlock", server); ASSERT_EQ(P.getMode(), mode); ASSERT_EQ(P.isDynamic(), dynamic); - if (mode != "nothing") - { - ASSERT_MATRIX_APPROX(P.getState(),p_state,wolf::Constants::EPS); - } + ASSERT_MATRIX_APPROX(P.getState(),p_state,wolf::Constants::EPS); if (dynamic) { - ASSERT_MATRIX_APPROX(P.getSigmaDrift(),po_sigma,wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(P.getSigmaDriftStd(),po_sigma,wolf::Constants::EPS); } if (mode == "factor") { - ASSERT_MATRIX_APPROX(P.getSigma(),po_sigma,wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(P.getSigmaStd(),po_sigma,wolf::Constants::EPS); } } } @@ -457,7 +397,7 @@ TEST(Prior, ParamsServerWrong) std::vector<std::string> keys({"P","O"}); std::vector<int> dims({2,3}); - std::vector<std::string> modes({"nothing","initial_guess","fix","factor"}); + 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); diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp index 0a367cd3047d706bbf46ce4697a4ea9c67fbd388..f2ef98beed7eeae2c9928bc0abcdd982df69e435 100644 --- a/test/gtest_sensor_base.cpp +++ b/test/gtest_sensor_base.cpp @@ -31,24 +31,188 @@ #include "core/utils/utils_gtest.h" using namespace wolf; +using namespace Eigen; -TEST(SensorBase, POfix) +VectorXd p_state_2D = VectorXd::Random(2); +VectorXd p_state_3D = VectorXd::Random(3); +VectorXd o_state_2D = VectorXd::Random(1); +VectorXd o_state_3D = VectorXd::Random(4).normalized(); +VectorXd p_sigma_2D = VectorXd::Random(2).cwiseAbs(); +VectorXd p_sigma_3D = VectorXd::Random(3).cwiseAbs(); +VectorXd o_sigma_2D = VectorXd::Random(1).cwiseAbs(); +VectorXd o_sigma_3D = VectorXd::Random(3).cwiseAbs(); + +VectorXd noise_std = Vector2d::Constant(0.1); +MatrixXd noise_cov = noise_std.cwiseAbs2().asDiagonal(); + + +// CONSTRUCTOR WITH PRIORS AND PARAMS +// 2D Fix +TEST(SensorBase, POfix2D) +{ + auto params = std::make_shared<ParamsSensorBase>(); + params->noise_std = noise_std; + + auto S = std::make_shared<SensorBase>("SensorBase", + "sensor1", + 2, + Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic + {'O',Prior("O", o_state_2D)}}), + params); + + ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8); + ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8); + ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8); + ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8); + ASSERT_TRUE(S->getP()->isFixed()); + ASSERT_TRUE(S->getO()->isFixed()); +} + +// 3D Fix +TEST(SensorBase, POfix3D) +{ + auto params = std::make_shared<ParamsSensorBase>(); + params->noise_std = noise_std; + + auto S = std::make_shared<SensorBase>("SensorBase", + "sensor1", + 3, + Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic + {'O',Prior("O", o_state_3D)}}), + params); + + ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8); + ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8); + ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8); + ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8); + ASSERT_TRUE(S->getP()->isFixed()); + ASSERT_TRUE(S->getO()->isFixed()); +} + +// 2D Initial guess +TEST(SensorBase, POinitial_guess2D) +{ + auto params = std::make_shared<ParamsSensorBase>(); + params->noise_std = noise_std; + + auto S = std::make_shared<SensorBase>("SensorBase", + "sensor1", + 2, + Priors({{'P',Prior("P", p_state_2D, "initial_guess")}, + {'O',Prior("O", o_state_2D, "initial_guess")}}), + params); + + ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8); + ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8); + ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8); + ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8); + ASSERT_FALSE(S->getP()->isFixed()); + ASSERT_FALSE(S->getO()->isFixed()); +} + +// 3D Initial guess +TEST(SensorBase, POinitial_guess3D) +{ + auto params = std::make_shared<ParamsSensorBase>(); + params->noise_std = noise_std; + + auto S = std::make_shared<SensorBase>("SensorBase", + "sensor1", + 3, + Priors({{'P',Prior("P", p_state_3D, "initial_guess")}, + {'O',Prior("O", o_state_3D, "initial_guess")}}), + params); + + ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8); + ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8); + ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8); + ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8); + ASSERT_FALSE(S->getP()->isFixed()); + ASSERT_FALSE(S->getO()->isFixed()); +} + +// 2D Factor +TEST(SensorBase, POfactor2D) { - auto noise_std = Eigen::Vector2d::Constant(0.1); - Eigen::MatrixXd noise_cov = noise_std.cwiseAbs2().asDiagonal(); + auto params = std::make_shared<ParamsSensorBase>(); + params->noise_std = noise_std; + + auto S = std::make_shared<SensorBase>("SensorBase", + "sensor1", + 2, + Priors({{'P',Prior("P", p_state_2D, "factor", p_sigma_2D)}, + {'O',Prior("O", o_state_2D, "factor", o_sigma_2D)}}), + params); + + ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8); + ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8); + ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8); + ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8); + ASSERT_FALSE(S->getP()->isFixed()); + ASSERT_FALSE(S->getO()->isFixed()); +} +// 3D Factor +TEST(SensorBase, POfactor3D) +{ auto params = std::make_shared<ParamsSensorBase>(); - params->noise_std = Eigen::Vector2d::Constant(0.1); + params->noise_std = noise_std; + + auto S = std::make_shared<SensorBase>("SensorBase", + "sensor1", + 3, + Priors({{'P',Prior("P", p_state_3D, "factor", p_sigma_3D)}, + {'O',Prior("O", o_state_3D, "factor", o_sigma_3D)}}), + params); + + ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8); + ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8); + ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8); + ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8); + ASSERT_FALSE(S->getP()->isFixed()); + ASSERT_FALSE(S->getO()->isFixed()); +} + +// 2D Initial guess dynamic +TEST(SensorBase, POinitial_guess_dynamic2D) +{ + auto params = std::make_shared<ParamsSensorBase>(); + params->noise_std = noise_std; auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, - Priors({{'P',Prior("P","fix", Eigen::Vector2d::Zero())}, - {'O',Prior("O","fix", Eigen::Vector1d::Zero())}}), + Priors({{'P',Prior("P", p_state_2D, "initial_guess", p_sigma_2D, true, p_sigma_2D)}, + {'O',Prior("O", o_state_2D, "initial_guess", o_sigma_2D, true, o_sigma_2D)}}), + params); + + ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8); + ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8); + ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8); + ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8); + ASSERT_FALSE(S->getP()->isFixed()); + ASSERT_FALSE(S->getO()->isFixed()); +} + +// 3D Initial guess dynamic +TEST(SensorBase, POinitial_guess_dynamic3D) +{ + auto params = std::make_shared<ParamsSensorBase>(); + params->noise_std = noise_std; + + auto S = std::make_shared<SensorBase>("SensorBase", + "sensor1", + 3, + Priors({{'P',Prior("P", p_state_3D, "initial_guess", p_sigma_3D, true, p_sigma_3D)}, + {'O',Prior("O", o_state_3D, "initial_guess", o_sigma_3D, true, o_sigma_3D)}}), params); ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8); ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8); + ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8); + ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8); + ASSERT_FALSE(S->getP()->isFixed()); + ASSERT_FALSE(S->getO()->isFixed()); } int main(int argc, char **argv) diff --git a/test/yaml/params_prior.yaml b/test/yaml/params_prior.yaml index 1b1955cdd541043243419ea56ed0e4be17201404..17b77f93c4b8bc8aacc91db7f5ff2ed148bbbece 100644 --- a/test/yaml/params_prior.yaml +++ b/test/yaml/params_prior.yaml @@ -3,10 +3,6 @@ config: dim: 2 # P in 2D - P2_nothing: - mode: nothing - dynamic: false - P2_initial_guess: mode: initial_guess state: [1, 2] @@ -22,11 +18,6 @@ config: state: [1, 2] sigma: [0.1, 0.2] dynamic: false - - P2_nothing_dynamic: - mode: nothing - dynamic: true - sigma_drift: [0.1, 0.2] P2_initial_guess_dynamic: mode: initial_guess @@ -48,10 +39,6 @@ config: sigma_drift: [0.1, 0.2] # P in 3D - P3_nothing: - mode: nothing - dynamic: false - P3_initial_guess: mode: initial_guess state: [1, 2, 3] @@ -67,11 +54,6 @@ config: state: [1, 2, 3] sigma: [0.1, 0.2, 0.3] dynamic: false - - P3_nothing_dynamic: - mode: nothing - dynamic: true - sigma_drift: [0.1, 0.2, 0.3] P3_initial_guess_dynamic: mode: initial_guess @@ -93,10 +75,6 @@ config: sigma_drift: [0.1, 0.2, 0.3] # O in 2D - O2_nothing: - mode: nothing - dynamic: false - O2_initial_guess: mode: initial_guess state: [1] @@ -112,11 +90,6 @@ config: state: [1] sigma: [0.1] dynamic: false - - O2_nothing_dynamic: - mode: nothing - dynamic: true - sigma_drift: [0.1] O2_initial_guess_dynamic: mode: initial_guess @@ -138,10 +111,6 @@ config: sigma_drift: [0.1] # O in 3D - O3_nothing: - mode: nothing - dynamic: false - O3_initial_guess: mode: initial_guess state: [1, 0, 0, 0] @@ -157,11 +126,6 @@ config: state: [1, 0, 0, 0] sigma: [0.1, 0.2, 0.3] dynamic: false - - O3_nothing_dynamic: - mode: nothing - dynamic: true - sigma_drift: [0.1, 0.2, 0.3] O3_initial_guess_dynamic: mode: initial_guess @@ -183,10 +147,6 @@ config: sigma_drift: [0.1, 0.2, 0.3] # I - I_nothing: - mode: nothing - dynamic: false - I_initial_guess: mode: initial_guess state: [1, 2, 3, 4] @@ -202,11 +162,6 @@ config: state: [1, 2, 3, 4] sigma: [0.1, 0.2, 0.3, 0.4] dynamic: false - - I_nothing_dynamic: - mode: nothing - dynamic: true - sigma_drift: [0.1, 0.2, 0.3, 0.4] I_initial_guess_dynamic: mode: initial_guess diff --git a/test/yaml/params_prior_wrong.yaml b/test/yaml/params_prior_wrong.yaml index 5a40d4743123e8e777016d86cbcb7aa178a0ed66..1d83bad82680c0deb719c1c4948bbbbb73449a44 100644 --- a/test/yaml/params_prior_wrong.yaml +++ b/test/yaml/params_prior_wrong.yaml @@ -3,10 +3,6 @@ config: dim: 2 # P in 2D - P2_nothing: - mode: no # wrong mode - dynamic: false - P2_initial_guess: mode: initial_guess #state: [1, 2] # missing @@ -22,11 +18,6 @@ config: state: [1, 2] sigma: [0.1, 0.2, 0.3] # wrong size dynamic: false - - P2_nothing_dynamic: - mode: nothing - dynamic: true - #sigma_drift: [0.1, 0.2] # missing P2_initial_guess_dynamic: mode: initial_guess @@ -48,10 +39,6 @@ config: sigma_drift: [0.1, 0.2] # P in 3D - P3_nothing: - mode: nothing - #dynamic: false # missing - P3_initial_guess: #mode: initial_guess # missing state: [1, 2, 3] @@ -67,11 +54,6 @@ config: state: [1, 2, 3] sigma: [0.1, 0.2] # wrong size dynamic: false - - P3_nothing_dynamic: - mode: no # wrong mode - dynamic: true - sigma_drift: [0.1, 0.2, 0.3] P3_initial_guess_dynamic: mode: initial_guess @@ -93,10 +75,6 @@ config: sigma_drift: [0.1, 0.2, 0.3] # O in 2D - O2_nothing: - mode: no # wrong mode - dynamic: false - O2_initial_guess: mode: initial_guess state: [1, 2] # wrong size @@ -112,11 +90,6 @@ config: state: [1] sigma: [0.1, 0.2] # wrong size dynamic: false - - O2_nothing_dynamic: - mode: nothing - dynamic: true - sigma_drift: [0.1 0.2] # wrong size O2_initial_guess_dynamic: mode: initial_guess @@ -138,10 +111,6 @@ config: sigma_drift: [0.1] # O in 3D - O3_nothing: - mode: no # wrong mode - dynamic: false - O3_initial_guess: mode: initial_guess #state: [1, 0, 0, 0] # missing @@ -157,11 +126,6 @@ config: state: [1, 0, 0, 0] sigma: [0.1, 0.2] # wrong size dynamic: false - - O3_nothing_dynamic: - mode: nothing - #dynamic: true # missing - sigma_drift: [0.1, 0.2, 0.3] O3_initial_guess_dynamic: mode: initial_guess @@ -183,18 +147,14 @@ config: sigma_drift: [0.1, 0.2] # wrong size # I - I_nothing: - mode: nothing - #dynamic: false # missing - I_initial_guess: #mode: initial_guess # missing state: [1, 2, 3, 4] dynamic: false I_fix: - #mode: fix # missing - state: [1, 2, 3, 4] + mode: fix + #state: [1, 2, 3, 4] # missing dynamic: false I_factor: @@ -202,11 +162,6 @@ config: state: [1, 2, 3, 4] sigma: [0.1, 0.2, 0.3] # wrong size dynamic: false - - I_nothing_dynamic: - mode: nothing - dynamic: true - #sigma_drift: [0.1, 0.2, 0.3, 0.4] # missing I_initial_guess_dynamic: mode: initial_guess