diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 9717307bf..3d140fc77 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -60,6 +60,13 @@ namespace gtsam { ExtendedKalmanFilter(T x_initial, noiseModel::Gaussian::shared_ptr P_initial); + /// print + void print(const std::string& s="") const { + std::cout << s << "\n"; + x_.print(s+"x"); + priorFactor_->print(s+"density"); + } + T predict(const MotionFactor& motionFactor); T update(const MeasurementFactor& measurementFactor); };