mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
repredictor.h
1#ifndef REPREDICTOR_H
2#define REPREDICTOR_H
3
4#include <Eigen/Dense>
5#include <boost/circular_buffer.hpp>
6#include <std_msgs/msg/header.hpp>
7#include <rclcpp/rclcpp.hpp>
8#include <mrs_lib/utils.h>
9
10namespace mrs_lib
11{
37 template <class Model, bool disable_reprediction = false>
39 {
40 public:
41 /* states, inputs etc. definitions (typedefs, constants etc) //{ */
42
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;
49 using statecov_t = typename Model::statecov_t;
50 using ModelPtr = typename std::shared_ptr<Model>;
52 //}
53
54 /* predictTo() method //{ */
65 template <bool check = disable_reprediction>
66 std::enable_if_t<!check, statecov_t> predictTo(const rclcpp::Time& to_stamp)
67 {
68
69 assert(!m_history.empty());
70 auto hist_it = std::begin(m_history);
71
72 if (hist_it->is_measurement)
73 {
74 // if the first history point is measurement, don't use it for correction (to prevent it from being used twice)
75 hist_it->is_measurement = false;
76 }
77 // cur_stamp corresponds to the time point of cur_sc estimation
78 auto cur_stamp = hist_it->stamp;
79 // cur_sc is the current state and covariance estimate
80 auto cur_sc = m_sc;
81 do
82 {
83 cur_sc.stamp = hist_it->stamp;
84 // do the correction if current history point is a measurement
85 if ((hist_it->is_measurement))
86 cur_sc = correctFrom(cur_sc, *hist_it);
87
88 // decide whether to predict to the next history point or straight to the desired stamp already
89 // (whichever comes sooner or directly to the desired stamp if no further history point is available)
90 rclcpp::Time next_stamp = to_stamp;
91
92 if ((hist_it + 1) != std::end(m_history) && (hist_it + 1)->stamp <= to_stamp)
93 next_stamp = (hist_it + 1)->stamp;
94
95 // do the prediction
96 cur_sc = predictFrom(cur_sc, *hist_it, cur_stamp, next_stamp);
97 cur_stamp = next_stamp;
98
99 // increment the history pointer
100 hist_it++;
101 } while (hist_it != std::end(m_history) && hist_it->stamp <= to_stamp);
102
103 cur_sc.stamp = to_stamp;
104
105 return cur_sc;
106 }
107
120 template <bool check = disable_reprediction>
121 std::enable_if_t<check, statecov_t> predictTo(const rclcpp::Time& to_stamp)
122 {
123
124 assert(!m_history.empty());
125 const auto& info = m_history.front();
126 auto sc = predictFrom(m_sc, info, info.stamp, to_stamp);
127 sc.stamp = to_stamp;
128
129 return sc;
130 }
131 //}
132
133 /* addInputChangeWithNoise() method //{ */
146 template <bool check = disable_reprediction>
147 std::enable_if_t<!check> addInputChangeWithNoise(const u_t& u, const Q_t& Q, const rclcpp::Time& stamp, const ModelPtr& model = nullptr)
148 {
149 const info_t info(stamp, u, Q, model);
150 // find the next point in the history buffer
151 const auto next_it = std::lower_bound(std::begin(m_history), std::end(m_history), info, &Repredictor<Model>::earlier);
152 // add the point to the history buffer
153 const auto added = addInfo(info, next_it);
154 // update all measurements following the newly added system input up to the next system input
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);
158 }
159
172 template <bool check = disable_reprediction>
173 std::enable_if_t<check> addInputChangeWithNoise(const u_t& u, const Q_t& Q, [[maybe_unused]] const rclcpp::Time& stamp, const ModelPtr& model = nullptr)
174 {
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;
181 }
182 //}
183
184 /* addInputChange() method //{ */
196 template <bool check = disable_reprediction>
197 std::enable_if_t<!check> addInputChange(const u_t& u, const rclcpp::Time& stamp, const ModelPtr& model = nullptr)
198 {
199
200 assert(!m_history.empty());
201
202 // find the next point in the history buffer
203 const auto next_it = std::lower_bound(std::begin(m_history), std::end(m_history), stamp, &Repredictor<Model>::earlier);
204
205 // get the previous history point (or the first one to avoid out of bounds)
206 const auto prev_it = next_it == std::begin(m_history) ? next_it : next_it - 1;
207
208 // initialize a new history info point
209 const info_t info(stamp, u, prev_it->Q, model);
210
211 // add the point to the history buffer
212 const auto added = addInfo(info, next_it);
213
214 // update all measurements following the newly added system input up to the next system input
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);
218 }
219
231 template <bool check = disable_reprediction>
232 std::enable_if_t<check> addInputChange(const u_t& u, [[maybe_unused]] const rclcpp::Time& stamp, const ModelPtr& model = nullptr)
233 {
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;
239 }
240 //}
241
242 /* addProcessNoiseChange() method //{ */
254 template <bool check = disable_reprediction>
255 std::enable_if_t<!check> addProcessNoiseChange(const Q_t& Q, const rclcpp::Time& stamp, const ModelPtr& model = nullptr)
256 {
257
258 assert(!m_history.empty());
259
260 // find the next point in the history buffer
261 const auto next_it = std::lower_bound(std::begin(m_history), std::end(m_history), stamp, &Repredictor<Model>::earlier);
262
263 // get the previous history point (or the first one to avoid out of bounds)
264 const auto prev_it = next_it == std::begin(m_history) ? next_it : next_it - 1;
265
266 // initialize a new history info point
267 const info_t info(stamp, prev_it->u, Q, model);
268
269 // add the point to the history buffer
270 const auto added = addInfo(info, next_it);
271
272 // update all measurements following the newly added system input up to the next system input
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);
276 }
277
289 template <bool check = disable_reprediction>
290 std::enable_if_t<check> addProcessNoiseChange(const Q_t& Q, [[maybe_unused]] const rclcpp::Time& stamp, const ModelPtr& model = nullptr)
291 {
292
293 if (m_history.empty())
294 m_history.push_back({stamp});
295
296 m_history.front().Q = Q;
297 m_history.front().stamp = stamp;
298 m_history.front().predict_model = model;
299 }
300 //}
301
302 /* addMeasurement() method //{ */
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)
317 {
318
319 assert(!m_history.empty());
320
321 // helper variable for searching of the next point in the history buffer
322 const auto next_it = std::lower_bound(std::begin(m_history), std::end(m_history), stamp, &Repredictor<Model>::earlier);
323
324 // get the previous history point (or the first one to avoid out of bounds)
325 const auto prev_it = next_it == std::begin(m_history) ? next_it : next_it - 1;
326
327 // initialize a new history info point
328 const info_t info(stamp, z, R, model, *prev_it, meas_id);
329
330 // add the point to the history buffer
331 addInfo(info, next_it);
332 }
333
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)
348 {
349
350 if (m_history.empty())
351 m_history.push_back({stamp});
352
353 auto& info = m_history.front();
354 const rclcpp::Time to_stamp = stamp > info.stamp ? stamp : info.stamp;
355 const auto sc = predictTo(to_stamp);
356
357 info.z = z;
358 info.R = R;
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);
364 }
365 //}
366
367 public:
368 /* constructor //{ */
369
383 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)
384 : m_sc{x0, P0}, m_default_model(model), m_history(history_t(hist_len))
385 {
386
387 assert(hist_len > 0);
388
389 addInputChangeWithNoise(u0, Q0, t0, model);
390 };
391
397
411 Repredictor(const x_t& x0, const P_t& P0, const Q_t& Q0, const rclcpp::Time& t0, const ModelPtr& model, const unsigned hist_len)
412 : m_sc{x0, P0}, m_default_model(model), m_history(history_t(hist_len))
413 {
414
415 assert(hist_len > 0);
416
417 const u_t u0{0};
418
419 addInputChangeWithNoise(u0, Q0, t0, model);
420 };
421 //}
422
423 protected:
424 // state and covariance corresponding to the oldest element in the history buffer
425 statecov_t m_sc;
426 // default model to use for updates
427 ModelPtr m_default_model;
428
429 private:
430 /* helper structs and usings //{ */
431
432 struct info_t
433 {
434 rclcpp::Time stamp;
435
436 // system input-related information
437 u_t u;
438 Q_t Q;
439 ModelPtr predict_model;
440
441 // measurement-related information (unused in case is_measurement=false)
442 z_t z;
443 R_t R;
444 ModelPtr correct_model;
445 bool is_measurement;
446 int meas_id;
447
448 // constructor for a dummy info (for searching in the history)
449 info_t(const rclcpp::Time& stamp) : stamp(stamp), is_measurement(false){};
450
451 // constructor for a system input
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){};
454
455 // constructor for a measurement
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)
458 {
459 updateUsing(prev_info);
460 };
461
462 // copy system input-related information from a previous info
463 void updateUsing(const info_t& info)
464 {
465 u = info.u;
466 Q = info.Q;
467 predict_model = info.predict_model;
468 };
469 };
470
471
472 //}
473
474 protected:
475 using history_t = boost::circular_buffer<info_t>;
476 // the history buffer
477 history_t m_history;
478
479 // | ---------------- helper debugging methods ---------------- |
480 /* checkMonotonicity() method //{ */
481 template <typename T>
482 bool checkMonotonicity(const T& buf)
483 {
484 if (buf.empty())
485 return true;
486 for (auto it = std::begin(buf) + 1; it != std::end(buf); it++)
487 if (earlier(*it, *(it - 1)))
488 return false;
489 return true;
490 }
491 //}
492
493 /* printBuffer() method //{ */
494 template <typename T>
495 void printBuffer(const T& buf)
496 {
497 if (buf.empty())
498 return;
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++)
502 {
503 std::cerr << it << ":\t" << buf.at(it).stamp - first_stamp << std::endl;
504 }
505 }
506 //}
507
508 private:
509 // | -------------- helper implementation methods ------------- |
510
511 /* addInfo() method //{ */
512 typename history_t::iterator addInfo(const info_t& info, const typename history_t::iterator& next_it)
513 {
514 // check if the new element would be added before the first element of the history buffer and ignore it if so
515 if (next_it == std::begin(m_history) && !m_history.empty())
516 {
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);
520 }
521
522 // check if adding a new element would throw out the oldest one
523 if (m_history.size() == m_history.capacity())
524 { // if so, first update m_sc
525 // figure out, what will be the oldest element in the buffer after inserting the newly received one
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))
528 {
529 new_oldest = info;
530 } else if (m_history.size() > 1)
531 {
532 new_oldest = m_history.at(1);
533 }
534 // update m_sc to the time of the new oldest element (after inserting the new element)
535 m_sc = predictTo(new_oldest.stamp);
536 // insert the new element into the history buffer, causing the original oldest element to be removed
537 }
538
539 // add the new point finally
540 const auto ret = m_history.insert(next_it, info);
541 /* debug check //{ */
542
543#ifdef REPREDICTOR_DEBUG
544 if (!checkMonotonicity(m_history))
545 {
546 std::cerr << "History buffer is not monotonous after modification!" << std::endl;
547 }
548 std::cerr << "Added info (" << m_history.size() << " total)" << std::endl;
549#endif
550
551 //}
552 return ret;
553 }
554 //}
555
556 /* earlier() method //{ */
557 static bool earlier(const info_t& ob1, const info_t& ob2)
558 {
559 return ob1.stamp < ob2.stamp;
560 }
561 //}
562
563 /* predictFrom() method //{ */
564 statecov_t predictFrom(const statecov_t& sc, const info_t& inpt, const rclcpp::Time& from_stamp, const rclcpp::Time& to_stamp)
565 {
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);
569 }
570 //}
571
572 /* correctFrom() method //{ */
573 statecov_t correctFrom(const statecov_t& sc, const info_t& meas)
574 {
575 assert(meas.is_measurement);
576 const auto model = meas.correct_model == nullptr ? m_default_model : meas.correct_model;
577 auto sc_tmp = sc;
578 return model->correct(sc_tmp, meas.z, meas.R);
579 }
580 //}
581 };
582} // namespace mrs_lib
583
584
585#endif // REPREDICTOR_H
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.