diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 0027844eadc4547abf777e9f3c22c90325d075c0..af06fd13624d22a6b0f83ab8b62f0769b1b0284f 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -551,8 +551,18 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st { auto sb = getStateBlockDynamic(key); if (sb) - _stream << _tabs << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", " << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] = ( " - << std::setprecision(3) << sb->getState().transpose() << " )" << " @ " << sb << std::endl; + { + _stream << _tabs << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", " + << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] = ( " << std::setprecision(3) + << sb->getState().transpose() << " )"; + if (params_prior_map_.count(key)) + { + auto ftr = params_prior_map_.at(key); + _stream << " ; mean = ( " << ftr->getMeasurement().transpose() << " )"; + _stream << " , std = ( " << ftr->getMeasurementCovariance().diagonal().array().sqrt().transpose() << " )"; + } + _stream << " @ " << sb << std::endl; + } } } else if (_metric)