diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 996d48263c113555ba055422153bf10459bfb564..21d9622c3366f33eea12ce94ef9a332d14cebc64 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -56,14 +56,20 @@ SensorBase::SensorBase(const std::string& _type, noise_cov_.setZero(); if (_p_ptr) + { + _p_ptr->setNonTransformable(); addStateBlock('P', _p_ptr, _p_dyn); + } if (_o_ptr) + { + _o_ptr->setNonTransformable(); addStateBlock('O', _o_ptr, _o_dyn); - + } if (_intr_ptr) + { addStateBlock('I', _intr_ptr, _intr_dyn); - + } } SensorBase::SensorBase(const std::string& _type, @@ -87,11 +93,15 @@ SensorBase::SensorBase(const std::string& _type, setNoiseStd(_noise_std); if (_p_ptr) + { + _p_ptr->setNonTransformable(); addStateBlock('P', _p_ptr, _p_dyn); - + } if (_o_ptr) + { + _o_ptr->setNonTransformable(); addStateBlock('O', _o_ptr, _o_dyn); - + } if (_intr_ptr) addStateBlock('I', _intr_ptr, _intr_dyn); }