5#include <boost/circular_buffer.hpp>
6#include <std_msgs/msg/header.hpp>
7#include <rclcpp/rclcpp.hpp>
37 template <
class Model,
bool disable_reprediction = false>
43 using x_t =
typename Model::x_t;
44 using u_t =
typename Model::u_t;
45 using z_t =
typename Model::z_t;
46 using P_t =
typename Model::P_t;
47 using R_t =
typename Model::R_t;
48 using Q_t =
typename Model::Q_t;
50 using ModelPtr =
typename std::shared_ptr<Model>;
65 template <
bool check = disable_reprediction>
66 std::enable_if_t<!check, statecov_t>
predictTo(
const rclcpp::Time& to_stamp)
69 assert(!m_history.empty());
70 auto hist_it = std::begin(m_history);
72 if (hist_it->is_measurement)
75 hist_it->is_measurement =
false;
78 auto cur_stamp = hist_it->stamp;
83 cur_sc.stamp = hist_it->stamp;
85 if ((hist_it->is_measurement))
86 cur_sc = correctFrom(cur_sc, *hist_it);
90 rclcpp::Time next_stamp = to_stamp;
92 if ((hist_it + 1) != std::end(m_history) && (hist_it + 1)->stamp <= to_stamp)
93 next_stamp = (hist_it + 1)->stamp;
96 cur_sc = predictFrom(cur_sc, *hist_it, cur_stamp, next_stamp);
97 cur_stamp = next_stamp;
101 }
while (hist_it != std::end(m_history) && hist_it->stamp <= to_stamp);
103 cur_sc.stamp = to_stamp;
120 template <
bool check = disable_reprediction>
121 std::enable_if_t<check, statecov_t>
predictTo(
const rclcpp::Time& to_stamp)
124 assert(!m_history.empty());
125 const auto& info = m_history.front();
126 auto sc = predictFrom(m_sc, info, info.stamp, to_stamp);
146 template <
bool check = disable_reprediction>
149 const info_t info(stamp, u, Q, model);
153 const auto added = addInfo(info, next_it);
155 if (added != std::end(m_history))
156 for (
auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
157 it->updateUsing(info);
172 template <
bool check = disable_reprediction>
175 if (m_history.empty())
176 m_history.push_back({stamp});
177 m_history.front().u = u;
178 m_history.front().Q = Q;
179 m_history.front().stamp = stamp;
180 m_history.front().predict_model = model;
196 template <
bool check = disable_reprediction>
200 assert(!m_history.empty());
206 const auto prev_it = next_it == std::begin(m_history) ? next_it : next_it - 1;
209 const info_t info(stamp, u, prev_it->Q, model);
212 const auto added = addInfo(info, next_it);
215 if (added != std::end(m_history))
216 for (
auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
217 it->updateUsing(info);
231 template <
bool check = disable_reprediction>
234 if (m_history.empty())
235 m_history.push_back({stamp});
236 m_history.front().u = u;
237 m_history.front().stamp = stamp;
238 m_history.front().predict_model = model;
254 template <
bool check = disable_reprediction>
258 assert(!m_history.empty());
264 const auto prev_it = next_it == std::begin(m_history) ? next_it : next_it - 1;
267 const info_t info(stamp, prev_it->u, Q, model);
270 const auto added = addInfo(info, next_it);
273 if (added != std::end(m_history))
274 for (
auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
275 it->updateUsing(info);
289 template <
bool check = disable_reprediction>
293 if (m_history.empty())
294 m_history.push_back({stamp});
296 m_history.front().Q = Q;
297 m_history.front().stamp = stamp;
298 m_history.front().predict_model = model;
315 template <
bool check = disable_reprediction>
316 std::enable_if_t<!check>
addMeasurement(
const z_t& z,
const R_t& R,
const rclcpp::Time& stamp,
const ModelPtr& model =
nullptr,
const double& meas_id = -1)
319 assert(!m_history.empty());
325 const auto prev_it = next_it == std::begin(m_history) ? next_it : next_it - 1;
328 const info_t info(stamp, z, R, model, *prev_it, meas_id);
331 addInfo(info, next_it);
346 template <
bool check = disable_reprediction>
347 std::enable_if_t<check>
addMeasurement(
const z_t& z,
const R_t& R,
const rclcpp::Time& stamp,
const ModelPtr& model =
nullptr,
const double& meas_id = -1)
350 if (m_history.empty())
351 m_history.push_back({stamp});
353 auto& info = m_history.front();
354 const rclcpp::Time to_stamp = stamp > info.stamp ? stamp : info.stamp;
359 info.stamp = to_stamp;
360 info.is_measurement =
true;
361 info.meas_id = meas_id;
362 info.correct_model = model;
363 m_sc = correctFrom(sc, info);
384 : m_sc{x0, P0}, m_default_model(model), m_history(history_t(hist_len))
387 assert(hist_len > 0);
412 : m_sc{x0, P0}, m_default_model(model), m_history(history_t(hist_len))
415 assert(hist_len > 0);
449 info_t(
const rclcpp::Time& stamp) : stamp(stamp), is_measurement(false){};
452 info_t(
const rclcpp::Time& stamp,
const u_t& u,
const Q_t& Q,
const ModelPtr& model)
453 : stamp(stamp), u(u), Q(Q), predict_model(model), is_measurement(false){};
456 info_t(
const rclcpp::Time& stamp,
const z_t& z,
const R_t& R,
const ModelPtr& model,
const info_t& prev_info,
const int& meas_id)
457 : stamp(stamp), z(z), R(R), correct_model(model), is_measurement(true), meas_id(meas_id)
459 updateUsing(prev_info);
463 void updateUsing(
const info_t& info)
467 predict_model = info.predict_model;
475 using history_t = boost::circular_buffer<info_t>;
481 template <
typename T>
482 bool checkMonotonicity(
const T& buf)
486 for (
auto it = std::begin(buf) + 1; it != std::end(buf); it++)
487 if (earlier(*it, *(it - 1)))
494 template <
typename T>
495 void printBuffer(
const T& buf)
499 const auto first_stamp = buf.front().stamp;
500 std::cerr <<
"first stamp: " << first_stamp << std::endl;
501 for (
size_t it = 0; it < buf.size(); it++)
503 std::cerr << it <<
":\t" << buf.at(it).stamp - first_stamp << std::endl;
512 typename history_t::iterator addInfo(
const info_t& info,
const typename history_t::iterator& next_it)
515 if (next_it == std::begin(m_history) && !m_history.empty())
517 std::cout <<
"[Repredictor]: Added history point is older than the oldest by " << (next_it->stamp - info.stamp).seconds()
518 <<
"s. Ignoring it! Consider increasing the history buffer size (currently: " << m_history.size() <<
")" << std::endl;
519 return std::end(m_history);
523 if (m_history.size() == m_history.capacity())
526 auto new_oldest = m_history.front();
527 if (m_history.size() == 1 || (m_history.size() > 1 && info.stamp > m_history.at(0).stamp && info.stamp < m_history.at(1).stamp))
530 }
else if (m_history.size() > 1)
532 new_oldest = m_history.at(1);
540 const auto ret = m_history.insert(next_it, info);
543#ifdef REPREDICTOR_DEBUG
544 if (!checkMonotonicity(m_history))
546 std::cerr <<
"History buffer is not monotonous after modification!" << std::endl;
548 std::cerr <<
"Added info (" << m_history.size() <<
" total)" << std::endl;
557 static bool earlier(
const info_t& ob1,
const info_t& ob2)
559 return ob1.stamp < ob2.stamp;
564 statecov_t predictFrom(
const statecov_t& sc,
const info_t& inpt,
const rclcpp::Time& from_stamp,
const rclcpp::Time& to_stamp)
566 const auto model = inpt.predict_model ==
nullptr ? m_default_model : inpt.predict_model;
567 const auto dt = (to_stamp - from_stamp).seconds();
568 return model->predict(sc, inpt.u, inpt.Q, dt);
573 statecov_t correctFrom(
const statecov_t& sc,
const info_t& meas)
575 assert(meas.is_measurement);
576 const auto model = meas.correct_model ==
nullptr ? m_default_model : meas.correct_model;
578 return model->correct(sc_tmp, meas.z, meas.R);
Implementation of the Repredictor for fusing measurements with variable delays.
Definition repredictor.h:39
std::enable_if_t< check > addInputChange(const u_t &u, const rclcpp::Time &stamp, const ModelPtr &model=nullptr)
Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
Definition repredictor.h:232
Repredictor()
Empty constructor.
Definition repredictor.h:396
std::enable_if_t<!check > addProcessNoiseChange(const Q_t &Q, const rclcpp::Time &stamp, const ModelPtr &model=nullptr)
Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
Definition repredictor.h:255
typename Model::u_t u_t
Input vector type .
Definition repredictor.h:44
Repredictor(const x_t &x0, const P_t &P0, const Q_t &Q0, const rclcpp::Time &t0, const ModelPtr &model, const unsigned hist_len)
Variation of the constructor for cases without a system input.
Definition repredictor.h:411
typename Model::z_t z_t
Measurement vector type .
Definition repredictor.h:45
std::enable_if_t<!check > addMeasurement(const z_t &z, const R_t &R, const rclcpp::Time &stamp, const ModelPtr &model=nullptr, const double &meas_id=-1)
Adds one measurement to the history buffer, removing the oldest element in the buffer if it is full.
Definition repredictor.h:316
std::enable_if_t< check, statecov_t > predictTo(const rclcpp::Time &to_stamp)
Estimates the system state and covariance matrix at the specified time.
Definition repredictor.h:121
std::enable_if_t<!check, statecov_t > predictTo(const rclcpp::Time &to_stamp)
Estimates the system state and covariance matrix at the specified time.
Definition repredictor.h:66
std::enable_if_t<!check > addInputChange(const u_t &u, const rclcpp::Time &stamp, const ModelPtr &model=nullptr)
Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
Definition repredictor.h:197
typename Model::statecov_t statecov_t
Helper struct for passing around the state and its covariance in one variable.
Definition repredictor.h:49
std::enable_if_t< check > addProcessNoiseChange(const Q_t &Q, const rclcpp::Time &stamp, const ModelPtr &model=nullptr)
Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
Definition repredictor.h:290
std::enable_if_t<!check > addInputChangeWithNoise(const u_t &u, const Q_t &Q, const rclcpp::Time &stamp, const ModelPtr &model=nullptr)
Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
Definition repredictor.h:147
typename Model::R_t R_t
Measurement noise covariance matrix type .
Definition repredictor.h:47
Repredictor(const x_t &x0, const P_t &P0, const u_t &u0, const Q_t &Q0, const rclcpp::Time &t0, const ModelPtr &model, const unsigned hist_len)
The main constructor.
Definition repredictor.h:383
typename Model::x_t x_t
State vector type .
Definition repredictor.h:43
typename Model::P_t P_t
State uncertainty covariance matrix type .
Definition repredictor.h:46
std::enable_if_t< check > addInputChangeWithNoise(const u_t &u, const Q_t &Q, const rclcpp::Time &stamp, const ModelPtr &model=nullptr)
Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.
Definition repredictor.h:173
typename std::shared_ptr< Model > ModelPtr
Shorthand type for a shared pointer-to-Model.
Definition repredictor.h:50
std::enable_if_t< check > addMeasurement(const z_t &z, const R_t &R, const rclcpp::Time &stamp, const ModelPtr &model=nullptr, const double &meas_id=-1)
Adds one measurement to the history buffer, removing the oldest element in the buffer if it is full.
Definition repredictor.h:347
typename Model::Q_t Q_t
Process noise covariance matrix type .
Definition repredictor.h:48
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24
Defines various general utility functions.