Print sensor priors

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)