mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
jlkf.h
1 // clang: MatousFormat
6 #ifndef JLKF_H
7 #define JLKF_H
8 
9 #include <mrs_lib/kalman_filter_aloamgarm.h>
10 /* #include <mrs_lib/lkf.h> */
11 #include <iostream>
12 
13 #include <vector>
14 
15 // for debug
16 #include <ros/ros.h>
17 #include <mrs_msgs/Float64ArrayStamped.h>
18 #include <mrs_msgs/Float64Stamped.h>
19 #include <mrs_msgs/BoolStamped.h>
20 
21 namespace mrs_lib
22 {
23 
24  /* class JLKF */ /*//{*/
25  template <int n_states, int n_inputs, int n_measurements, int n_biases>
26  class JLKF : public KalmanFilterAloamGarm<n_states, n_inputs, n_measurements>
27  {
28  public:
29  /* LKF definitions (typedefs, constants etc) //{ */
30  static const int n = n_states;
31  static const int m = n_inputs;
32  static const int p = n_measurements;
33  using Base_class = KalmanFilterAloamGarm<n, m, p>;
34 
35  using x_t = typename Base_class::x_t;
36  using u_t = typename Base_class::u_t;
37  using z_t = typename Base_class::z_t;
38  using P_t = typename Base_class::P_t;
39  using R_t = typename Base_class::R_t;
40  using statecov_t = typename Base_class::statecov_t;
41  /* using A_t = typename Base_class::A_t; // measurement mapping p*n */
42  /* using B_t = typename Base_class::B_t; // process covariance n*n */
43  /* using H_t = typename Base_class::H_t; // measurement mapping p*n */
44  using Q_t = typename Base_class::Q_t; // process covariance n*n
45  /* using K_t = typename Base_class::K_t; // Kalman gain */
46 
47  typedef Eigen::Matrix<double, n, n> A_t;
48  typedef Eigen::Matrix<double, n, m> B_t;
49  typedef Eigen::Matrix<double, p, n> H_t;
50  typedef Eigen::Matrix<double, n, p> K_t;
52  typedef Eigen::Matrix<double, p, p> C_t;
53  typedef Eigen::Matrix<double, n, n> D_t;
55  using generateA_t = std::function<A_t(double)>;
56  using generateB_t = std::function<B_t(double)>;
57  //}
58 
65  struct inverse_exception : public std::exception
66  {
72  const char* what() const throw()
73  {
74  return "LKF: could not compute matrix inversion!!! Fix your covariances (the measurement's is probably too low...)";
75  }
76  };
77  //}
78 
79  public:
87  JLKF(const generateA_t& generateA, const generateB_t& generateB, const H_t& H, const ros::NodeHandle& nh, const double& nis_thr,
88  const double& nis_avg_thr)
89  : m_generateA(generateA),
90  m_generateB(generateB),
91  H(H),
92  m_nh(nh),
93  m_nis_thr(nis_thr),
94  m_nis_avg_thr(nis_avg_thr)
95  {
96  debug_nis_pub = m_nh.advertise<mrs_msgs::Float64ArrayStamped>("debug_nis", 1);
97  };
98 
99  /* predict() method //{ */
117  virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override
118  {
119  /* std::cout << "prediction, sc.x: " << sc.x << std::endl; */
120  statecov_t ret;
121  A_t A = m_generateA(dt);
122  B_t B = m_generateB(dt);
123  ret.x = state_predict(A, sc.x, B, u);
124  ret.P = covariance_predict(A, sc.P, Q, dt);
125  ret.nis_buffer = sc.nis_buffer;
126  return ret;
127  };
128  //}
129 
130  /* correct() method //{ */
143  virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R) const override
144  {
145  /* return correct_optimized(sc, z, R, H); */
146  return correction_impl(sc, z, R, H);
147  };
148  //}
149 
150  protected:
151  /* invert_W() method //{ */
152  static R_t invert_W(R_t W)
153  {
154  Eigen::ColPivHouseholderQR<R_t> qr(W);
155  if (!qr.isInvertible())
156  {
157  // add some stuff to the tmp matrix diagonal to make it invertible
158  R_t ident = R_t::Identity(W.rows(), W.cols());
159  W += 1e-9 * ident;
160  qr.compute(W);
161  if (!qr.isInvertible())
162  {
163  // never managed to make this happen except for explicitly putting NaNs in the input
164  throw inverse_exception();
165  }
166  }
167  const R_t W_inv = qr.inverse();
168  return W_inv;
169  }
170  //}
171 
172 
173  /* computeKalmanGain() method //{ */
174  virtual K_t computeKalmanGain(const statecov_t& sc, [[maybe_unused]] const z_t& z, const R_t& R, const H_t& H, double& nis, H_t& H_out,
175  const double& nis_thr, const double& nis_avg_thr) const
176  {
177  H_out = H;
178 
179  R_t W = H * sc.P * H.transpose() + R;
180  R_t W_inv = invert_W(W);
181  K_t K = sc.P * H.transpose() * W_inv;
182  z_t y = z - (H * sc.x);
183 
184  /* const double nis = (y.transpose() * W_inv * y)(0, 0); */
185  nis = (y.transpose() * W_inv * y)(0, 0);
186  double nis_avg = 0;
187  int count = 0;
188  bool nis_over_thr = false;
189 
190  double nis_thr_tmp = nis_thr;
191 
192  if (H(0, 0) > 0 && H(0, 3) != 0)
193  /* if (H(0, 0) > 0) */
194  {
195  mrs_msgs::Float64ArrayStamped msg;
196  msg.header.stamp = sc.stamp;
197 
198  msg.values.push_back(nis);
199 
200  if (sc.nis_buffer != nullptr)
201  {
202 
203  sc.nis_buffer->push_back(nis);
204  for (auto it = sc.nis_buffer->begin(); it != sc.nis_buffer->end(); it++)
205  {
206  nis_avg += *it;
207  count++;
208  if (*it > nis_thr_tmp)
209  {
210  nis_thr_tmp /= 10;
211  }
212  }
213  if (count > 0)
214  {
215  nis_avg /= count;
216  }
217  msg.values.push_back(nis_avg);
218  msg.values.push_back(nis_thr_tmp);
219  }
220  if (nis > nis_thr_tmp)
221  {
222  // old jump correction
223  K = K.Zero();
224  A_t mask = mask.Zero();
225  for (int i = n_states - n_biases; i < n_states; i++)
226  {
227  mask(i, i) = 1;
228  }
229  const H_t H_bias_only = H * mask;
230  K = H_bias_only.transpose() * invert_W(H_bias_only * H_bias_only.transpose());
231  H_out = H_bias_only;
232  }
233 
234  debug_nis_pub.publish(msg);
235  }
236 
237  K_t test = H.transpose() * invert_W(H * H.transpose());
238 
239  return K;
240  }
241  //}
242 
243  /* correction_impl() method //{ */
244  template <int check = n>
245  typename std::enable_if<check >= 0, statecov_t>::type correction_impl(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) const
246  {
247  // the correction phase
248  statecov_t ret;
249  double nis = -1;
250  H_t H_out;
251  const K_t K = computeKalmanGain(sc, z, R, H, nis, H_out, m_nis_thr, m_nis_avg_thr);
252  ret.x = sc.x + K * (z - (H * sc.x));
253  ret.P = (P_t::Identity() - (K * H_out)) * sc.P;
254  ret.nis_buffer = sc.nis_buffer;
255  return ret;
256  }
257 
258  template <int check = n>
259  typename std::enable_if < check<0, statecov_t>::type correction_impl(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) const
260  {
261  // the correction phase
262  statecov_t ret;
263  double nis = -1;
264  H_t H_out;
265  const K_t K = computeKalmanGain(sc, z, R, H, nis, H_out, m_nis_thr, m_nis_avg_thr);
266  ret.x = sc.x + K * (z - (H * sc.x));
267  ret.P = (P_t::Identity(sc.P.rows(), sc.P.cols()) - (K * H_out)) * sc.P;
268  ret.nis_buffer = sc.nis_buffer;
269  return ret;
270  }
271  //}
272 
273  private:
274  ros::NodeHandle m_nh;
275  ros::Publisher debug_nis_pub;
276  std::vector<double> m_nis_window;
277  double m_nis_thr;
278  double m_nis_avg_thr;
279 
280 
281  private:
282  generateA_t m_generateA;
283  generateB_t m_generateB;
284 
285  public:
286  A_t A;
287  B_t B;
288  H_t H;
290  protected:
291  /* covariance_predict() method //{ */
292  static P_t covariance_predict(const A_t& A, const P_t& P, const Q_t& Q, const double dt)
293  {
294  return A * P * A.transpose() + dt*Q;
295  }
296  //}
297 
298  /* state_predict() method //{ */
299  template <int check = n_inputs>
300  static inline typename std::enable_if<check == 0, x_t>::type state_predict(const A_t& A, const x_t& x, [[maybe_unused]] const B_t& B,
301  [[maybe_unused]] const u_t& u)
302  {
303  return A * x;
304  }
305 
306  template <int check = n_inputs>
307  static inline typename std::enable_if<check != 0, x_t>::type state_predict(const A_t& A, const x_t& x, const B_t& B, const u_t& u)
308  {
309  return A * x + B * u;
310  }
311  //}
312 
313  }; // namespace mrs_lib
314  /*//}*/
315 
316 
317 } // namespace mrs_lib
318 
319 #endif // JLKF_H
mrs_lib::JLKF::inverse_exception
This exception is thrown when taking the inverse of a matrix fails.
Definition: jlkf.h:65
mrs_lib::JLKF::K_t
Eigen::Matrix< double, n, p > K_t
Kalman gain matrix type .
Definition: jlkf.h:50
mrs_lib::KalmanFilterAloamGarm::R_t
Eigen::Matrix< double, p, p > R_t
Measurement noise covariance matrix type .
Definition: kalman_filter_aloamgarm.h:40
mrs_lib::JLKF::A_t
Eigen::Matrix< double, n, n > A_t
System transition matrix type .
Definition: jlkf.h:47
mrs_lib::KalmanFilterAloamGarm::Q_t
Eigen::Matrix< double, n, n > Q_t
Process noise covariance matrix type .
Definition: kalman_filter_aloamgarm.h:41
mrs_lib::JLKF::predict
virtual statecov_t predict(const statecov_t &sc, const u_t &u, const Q_t &Q, double dt) const override
Applies the prediction (time) step of the Kalman filter.
Definition: jlkf.h:117
mrs_lib::KalmanFilterAloamGarm::statecov_t
Helper struct for passing around the state and its covariance in one variable.
Definition: kalman_filter_aloamgarm.h:48
mrs_lib::KalmanFilterAloamGarm
This abstract class defines common interfaces and types for a generic Kalman filter.
Definition: kalman_filter_aloamgarm.h:27
mrs_lib::KalmanFilterAloamGarm::u_t
Eigen::Matrix< double, m, 1 > u_t
Input vector type .
Definition: kalman_filter_aloamgarm.h:36
mrs_lib::JLKF::B_t
Eigen::Matrix< double, n, m > B_t
Input to state mapping matrix type .
Definition: jlkf.h:48
mrs_lib::JLKF
Definition: jlkf.h:26
mrs_lib::JLKF::JLKF
JLKF(const generateA_t &generateA, const generateB_t &generateB, const H_t &H, const ros::NodeHandle &nh, const double &nis_thr, const double &nis_avg_thr)
The main constructor.
Definition: jlkf.h:87
mrs_lib::JLKF::H_t
Eigen::Matrix< double, p, n > H_t
State to measurement mapping matrix type .
Definition: jlkf.h:49
mrs_lib::JLKF::inverse_exception::what
const char * what() const
Returns the error message, describing what caused the exception.
Definition: jlkf.h:72
mrs_lib::JLKF::correct
virtual statecov_t correct(const statecov_t &sc, const z_t &z, const R_t &R) const override
Applies the correction (update, measurement, data) step of the Kalman filter.
Definition: jlkf.h:143
mrs_lib
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition: attitude_converter.h:29
mrs_lib::JLKF::D_t
Eigen::Matrix< double, n, n > D_t
D .
Definition: jlkf.h:53
kalman_filter_aloamgarm.h
Defines KalmanFilter - an abstract class, defining common interfaces and types for a generic Kalman f...
mrs_lib::KalmanFilterAloamGarm::z_t
Eigen::Matrix< double, p, 1 > z_t
Measurement vector type .
Definition: kalman_filter_aloamgarm.h:37
mrs_lib::JLKF::C_t
Eigen::Matrix< double, p, p > C_t
correntropy gain
Definition: jlkf.h:52
mrs_lib::KalmanFilterAloamGarm::P_t
Eigen::Matrix< double, n, n > P_t
State uncertainty covariance matrix type .
Definition: kalman_filter_aloamgarm.h:39
mrs_lib::KalmanFilterAloamGarm::x_t
Eigen::Matrix< double, n, 1 > x_t
State vector type .
Definition: kalman_filter_aloamgarm.h:35