quater_ikf
Ikf.hpp
Go to the documentation of this file.
1 
20 #ifndef INDIRECT_KALMAN_FILTER_HPP
21 #define INDIRECT_KALMAN_FILTER_HPP
22 
23 #include <Eigen/Geometry>
24 #include <Eigen/StdVector>
25 #include <Eigen/LU>
26 #include <Eigen/SVD>
27 #include <Eigen/Eigenvalues>
30 #include <vector>
33 #include <boost/shared_ptr.hpp>
36 #include "AdaptiveAttitudeCov.hpp"
37 
38 //#define INDIRECT_KALMAN_FILTER_DEBUG_PRINTS 1
39 
40 namespace filter
41 {
42  using namespace Eigen;
43 
44  template <typename _Scalar, bool _Accelerometers, bool _Inclinometers>
45  class Ikf
46  {
47 
48  public:
50  static const int flagAcc = static_cast<int>(_Accelerometers);
51  static const int flagIncl = static_cast<int>(_Inclinometers);
52 
54  static const unsigned int QUATERNION_SIZE = 4;
55  static const unsigned int IKFSTATEVECTORSIZE = 6 + (flagAcc*3) + (flagIncl*3);
56 
57  private:
59  const int Re = 6378137;
60  const int Rp = 6378137;
61  const double ECC = 0.0818191908426;
62  const double GRAVITY = 9.79766542;
63  const double GWGS0 = 9.7803267714;
64  const double GWGS1 = 0.00193185138639;
65  const double EARTHW = 7.292115e-05;
68  enum DECLINATION_CONSTS {
69  EAST = 1,
70  WEST = 2
71  };
72 
74  typedef AdaptiveAttitudeCov<_Scalar, IKFSTATEVECTORSIZE, 3> AdaptiveAttitudeCovAcc;
75  typedef AdaptiveAttitudeCov<_Scalar, IKFSTATEVECTORSIZE, 3> AdaptiveAttitudeCovIncl;
76 
78  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,1> x;
79  Eigen::Matrix <_Scalar,3,1> gtilde;
80  Eigen::Matrix <_Scalar,3,1> mtilde;
81  Eigen::Quaternion <_Scalar> q4;
82  Eigen::Matrix <_Scalar,QUATERNION_SIZE,QUATERNION_SIZE> oldomega4;
83  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> P;
84  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> A;
85  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> Q;
86  Eigen::Matrix <_Scalar,3,3> Ra;
87  Eigen::Matrix <_Scalar,3,3> Rg;
88  Eigen::Matrix <_Scalar,3,3> Rm;
89  Eigen::Matrix <_Scalar,3,3> Ri;
90  Eigen::Matrix <_Scalar,3,IKFSTATEVECTORSIZE> H1;
91  Eigen::Matrix <_Scalar,3,IKFSTATEVECTORSIZE> H2;
92  Eigen::Matrix <_Scalar,3,IKFSTATEVECTORSIZE> H3;
93  Eigen::Matrix <_Scalar,3,1> bahat;
94  Eigen::Matrix <_Scalar,3,1> bghat;
95  Eigen::Matrix <_Scalar,3,1> bihat;
98  boost::shared_ptr<AdaptiveAttitudeCovAcc> adapAttAcc;
99 
101  boost::shared_ptr<AdaptiveAttitudeCovIncl> adapAttIncl;
102 
103  protected:
104  void initAdaptiveAttitude(const unsigned int accM1, const unsigned int accM2, const double accGamma,
105  const unsigned int incM1, const unsigned int incM2, const double incGamma)
106  {
107  this->adapAttAcc.reset(new AdaptiveAttitudeCovAcc (accM1, accM2, accGamma));
108  this->adapAttIncl.reset(new AdaptiveAttitudeCovIncl (incM1, incM2, incGamma));
109  }
110 
111 
112  public:
113 
116  ~Ikf()
117  {
118  adapAttAcc.reset();
119  adapAttIncl.reset();
120  }
121 
142  void Init(const Eigen::Matrix <_Scalar,Ikf::IKFSTATEVECTORSIZE,Ikf::IKFSTATEVECTORSIZE> &P_0,
143  const Eigen::Matrix <_Scalar,3,3> &Ra,
144  const Eigen::Matrix <_Scalar, 3, 3> &Rg,
145  const Eigen::Matrix <_Scalar,3,3> &Rm,
146  const Eigen::Matrix <_Scalar,3,3> &Ri,
147  const Eigen::Matrix <_Scalar, 3, 3> &Qbg,
148  const Eigen::Matrix <_Scalar,3,3> &Qba,
149  const Eigen::Matrix <_Scalar,3,3> &Qbi,
150  double g, double alpha,
151  unsigned int am1, unsigned int am2, double agamma,
152  unsigned int im1, unsigned int im2, double igamma)
153  {
155  gtilde << 0, 0, g;
156 
158  mtilde(0) = cos(alpha);
159  mtilde(1) = 0;
160  mtilde(2) = -sin(alpha);
161 
162 
164  x = Eigen::Matrix <_Scalar,Ikf::IKFSTATEVECTORSIZE,1>::Zero();
165 
166  Q = Eigen::Matrix <_Scalar,Ikf::IKFSTATEVECTORSIZE,Ikf::IKFSTATEVECTORSIZE>::Zero();
167  Q.template block<3, 3> (0,0) = 0.25 * (Rg);
168  Q.template block<3, 3> (3,3) = (Qbg);
169  if (_Accelerometers)
170  Q.template block<3, 3> (6,6) = (Qba);
171  if (_Inclinometers)
172  Q.template block<3, 3> (6+(flagAcc*3),6+(flagAcc*3)) = (Qbi);
173 
175  P = (P_0);
176 
178  H1 = Eigen::Matrix<_Scalar, 3,Ikf::IKFSTATEVECTORSIZE>::Zero();
179  H2 = Eigen::Matrix<_Scalar, 3,Ikf::IKFSTATEVECTORSIZE>::Zero();
180  H3 = Eigen::Matrix<_Scalar, 3,Ikf::IKFSTATEVECTORSIZE>::Zero();
181 
182  if (_Accelerometers)
183  {
184  H1(0,6) = 1; H1(1,7) = 1; H1(2,8) = 1;
185  }
186  if (_Inclinometers)
187  {
188  H3(0,6) = 1; H3(1,7) = 1; H3(2,8) = 1;
189  }
190  if (_Accelerometers && _Inclinometers)
191  {
192  H3 = Eigen::Matrix<_Scalar, 3,Ikf::IKFSTATEVECTORSIZE>::Zero();
193  H3(0,9) = 1; H3(1,10) = 1; H3(2,11) = 1;
194  }
195 
197  A = Eigen::Matrix <_Scalar,Ikf::IKFSTATEVECTORSIZE,Ikf::IKFSTATEVECTORSIZE>::Zero();
198  A(0,3) = -0.5;A(1,4) = -0.5;A(2,5) = -0.5;
199 
201  bghat = Eigen::Matrix <_Scalar,3,1>::Zero();
202  bahat = Eigen::Matrix <_Scalar,3,1>::Zero();
203  bihat = Eigen::Matrix <_Scalar,3,1>::Zero();
204 
206  oldomega4 << 0 , 0 , 0 , 0,
207  0 , 0 , 0 , 0,
208  0 , 0 , 0 , 0,
209  0 , 0 , 0 , 0;
210 
211 
213  q4.w() = 1.00;
214  q4.x() = 0.00;
215  q4.y() = 0.00;
216  q4.z() = 0.00;
217 
219  this->Ikf::Ra = Ra;
220  this->Ikf::Rg = Rg;
221  this->Ikf::Rm = Rm;
222  this->Ikf::Ri = Ri;
223 
225  initAdaptiveAttitude(am1, am2, agamma, im1, im2, igamma);
226 
228  #ifdef INDIRECT_KALMAN_FILTER_DEBUG_PRINTS
229  std::cout<< "P:\n"<<P<<"\n";
230  std::cout<< "Q:\n"<<Q<<"\n";
231  std::cout<< "H1:\n"<<H1<<"\n";
232  std::cout<< "H2:\n"<<H2<<"\n";
233  std::cout<< "H3:\n"<<H3<<"\n";
234  std::cout<< "A:\n"<<A<<"\n";
235  std::cout<< "mtilde:\n"<<mtilde<<"\n";
236  std::cout<< "gtilde:\n"<<gtilde<<"\n";
237  std::cout<< "Ra:\n"<<Ra<<"\n";
238  std::cout<< "Rg:\n"<<Rg<<"\n";
239  std::cout<< "Rm:\n"<<Rm<<"\n";
240  std::cout<< "Ri:\n"<<Ri<<"\n";
241  #endif
242 
243  return;
244 
245  }
246 
262  void predict(Eigen::Matrix <_Scalar,3,1> &u, double dt)
263  {
264  Eigen::Matrix <_Scalar,3,3> vec2product;
265  Eigen::Matrix <_Scalar,3,1> angvelo;
266  Eigen::Matrix <_Scalar,QUATERNION_SIZE,QUATERNION_SIZE> omega4;
267  Eigen::Matrix <_Scalar,QUATERNION_SIZE,1> quat;
268  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> dA;
269  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> Qd;
272  angvelo = (u) - bghat;
274  vec2product << 0, -angvelo(2), angvelo(1),
275  angvelo(2), 0, -angvelo(0),
276  -angvelo(1), angvelo(0), 0;
277 
279  A.template block<3, 3> (0,0) = -vec2product;
280  dA = Eigen::Matrix<_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE>::Identity() + A * dt + 0.5 *A * A * pow(dt,2);
281 
283  x = dA * x;
284  Qd = Q*dt + 0.5*dt*A*Q + 0.5*dt*Q*A.transpose();
285  Qd = 0.5*(Qd + Qd.transpose());//Guarantee symmetry
286  P = dA*P*dA.transpose() + Qd;
287 
288  omega4 << 0,-angvelo(0), -angvelo(1), -angvelo(2),
289  angvelo(0), 0, angvelo(2), -angvelo(1),
290  angvelo(1), -angvelo(2), 0, angvelo(0),
291  angvelo(2), angvelo(1), -angvelo(0), 0;
292 
293  quat(0) = q4.w();
294  quat(1) = q4.x();
295  quat(2) = q4.y();
296  quat(3) = q4.z();
297 
299  quat = (Eigen::Matrix<_Scalar,QUATERNION_SIZE,QUATERNION_SIZE>::Identity() +(0.75 * omega4 *dt)-(0.25 * oldomega4 * dt) -
300  ((1.0/6.0) * angvelo.squaredNorm() * pow(dt,2) * Eigen::Matrix<_Scalar,QUATERNION_SIZE,QUATERNION_SIZE>::Identity()) -
301  ((1.0/24.0) * omega4 * oldomega4 * pow(dt,2)) - ((1.0/48.0) * angvelo.squaredNorm() * omega4 * pow(dt,3))) * quat;
302 
303  q4.w() = quat(0);
304  q4.x() = quat(1);
305  q4.y() = quat(2);
306  q4.z() = quat(3);
307  q4.normalize();
308 
309  oldomega4 = omega4;
310 
311  return;
312 
313  }
314 
315  void update(const Eigen::Matrix <_Scalar,3,1> &acc, bool acc_on)
316  {
317  if (_Accelerometers)
318  {
319  Eigen::Matrix <_Scalar,3,1> aux; aux.setZero();
320  update(acc, acc_on, aux, false, aux, false);
321  }
322  else
323  throw std::runtime_error("IKF without accelerometers and you are updating with accelerometers");
324  }
325 
326  void update(const Eigen::Matrix <_Scalar,3,1> &acc, bool acc_on,
327  const Eigen::Matrix< _Scalar,3, 1> &measurement, bool measurement_on)
328  {
329  Eigen::Matrix <_Scalar,3,1> aux; aux.setZero();
330  if (_Inclinometers)
331  {
332  update(acc, acc_on, measurement, measurement_on, aux, false);
333  }
334  else
335  {
336  update(acc, acc_on, aux, false, measurement, measurement_on);
337  }
338  }
339 
340 
341  void update(const Eigen::Matrix <_Scalar,3,1> &measurement1,
342  const Eigen::Matrix< _Scalar,3, 1> &measurement2)
343  {
344  Eigen::Matrix <_Scalar,3,1> aux; aux.setZero();
345  if (_Accelerometers && _Inclinometers)
346  {
347  update(measurement1, true, measurement2, true, aux, false);
348  }
349  else if (_Accelerometers)
350  {
351  update(measurement1, true, aux, false, measurement2, true);
352  }
353  else if (_Inclinometers)
354  {
355  update(aux, false, measurement1, true, measurement2, true);
356  }
357  else
358  throw std::runtime_error("IKF without accelerometers and inclinometers. Only update with magnetometers allowed");
359  }
360 
361 
386  void update(const Eigen::Matrix <_Scalar,3,1> &acc, bool acc_on,
387  const Eigen::Matrix< _Scalar,3, 1> &incl, bool incl_on,
388  const Eigen::Matrix <_Scalar,3, 1> &mag, bool magn_on)
389  {
390  Eigen::Matrix <_Scalar,3,3> vec2product;
391  Eigen::Matrix <_Scalar,3,3> fooR2;
392  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> P1;
393  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> P2;
394  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> P3;
395  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> auxM;
396  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE, 3> K1;
397  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE, 3> K2;
398  Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE, 3> K3;
399  Eigen::Matrix <_Scalar,3,3> R1;
400  Eigen::Matrix <_Scalar,3,3> R3;
401  Eigen::Quaternion <_Scalar> qe;
402  Eigen::Matrix <_Scalar,3,1> gtilde_body;
403  Eigen::Matrix <_Scalar,3,1> mtilde_body;
404  Eigen::Matrix <_Scalar,3,1> z1;
405  Eigen::Matrix <_Scalar,3,1> z2;
406  Eigen::Matrix <_Scalar,3,1> z3;
407  Eigen::Matrix <_Scalar,3,1> auxvector;
414  if (acc_on && _Accelerometers)
415  {
417  gtilde_body = q4.inverse() * gtilde;
418  vec2product << 0, -gtilde_body(2), gtilde_body(1),
419  gtilde_body(2), 0, -gtilde_body(0),
420  -gtilde_body(1), gtilde_body(0), 0;
421 
422  H1.template block<3, 3> (0,0) = 2*vec2product;
423 
425  z1 = (acc) - bahat - gtilde_body;
426 
427  #ifdef INDIRECT_KALMAN_FILTER_DEBUG_PRINTS
428  std::cout<<"acc:\n"<<acc<<"\n";
429  std::cout<<"z1:\n"<<z1<<"\n";
430  std::cout<<"g_body:\n"<<gtilde_body<<"\n";
431  #endif
432 
434  R1 = adapAttAcc->matrix (x, P, z1, H1, Ra);
435 
437  P1 = P;
438  Eigen::Matrix<_Scalar, 3, 3> S1, S1_inverse;
439  S1 = H1 * P1 * H1.transpose() + R1;
440  S1_inverse = S1.inverse();
441  K1 = P1 * H1.transpose() * S1_inverse;
442  Eigen::Matrix<_Scalar, 3, 1> innovationAcc = (z1 - H1 * x);
443 
445  x = x + K1 * innovationAcc;
446  P = (Eigen::Matrix<_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE>::Identity()
447  -K1*H1)*P*(Eigen::Matrix<_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE>::Identity()
448  -K1*H1).transpose() + K1*R1*K1.transpose();
449 
450  #ifdef INDIRECT_KALMAN_FILTER_DEBUG_PRINTS
451  std::cout<<"x(k+1|k+1):\n"<<x<<"\n";
452  std::cout<<"P(k+1|k+1):\n"<<P<<"\n";
453  std::cout<<"innovation:\n"<<innovationAcc<<"\n";
454  std::cout<<"K1:\n"<<K1<<"\n";
455  std::cout<<"R1:\n"<<R1<<"\n";
456  #endif
457 
460  qe.w() = 1;
461  qe.x() = x(0);
462  qe.y() = x(1);
463  qe.z() = x(2);
464  q4 = q4 * qe;
465 
467  q4.normalize();
468 
470  x.template block<3,1>(0,0) = Eigen::Matrix<_Scalar, 3, 1>::Zero();
471  }
472 
478  if (magn_on)
479  {
480 
482  mtilde_body = q4.inverse() * mtilde;
483  vec2product << 0, -mtilde_body(2), mtilde_body(1),
484  mtilde_body(2), 0, -mtilde_body(0),
485  -mtilde_body(1), mtilde_body(0), 0;
486 
488  H2.template block<3, 3> (0,0) = 2*vec2product;
489 
491  z2 = (mag) - mtilde_body;
492 
493  P2 = Eigen::Matrix<_Scalar, IKFSTATEVECTORSIZE, IKFSTATEVECTORSIZE>::Zero();
494  P2.template block<3, 3>(0,0) = P.template block<3, 3>(0,0);
495 
496  auxvector << 0, 0, 1;
497  auxvector = q4.inverse() * auxvector;
498 
500  auxM = Eigen::Matrix<_Scalar, IKFSTATEVECTORSIZE, IKFSTATEVECTORSIZE>::Zero();
501  auxM.template block<3, 3>(0,0) = auxvector * auxvector.transpose();
502  K2 = auxM * P2 * H2.transpose() * (H2*P2*H2.transpose() + Rm).inverse();
503 
505  x = x + K2*(z2 - (H2*x));
506  P = P - K2 * H2 * P - P * H2.transpose() * K2.transpose() + K2*(H2*P*H2.transpose() + Rm)*K2.transpose();
507 
509  qe.w() = 1;
510  qe.x() = x(0);
511  qe.y() = x(1);
512  qe.z() = x(2);
513  q4 = q4 * qe;
514 
516  q4.normalize();
517 
519  x.template block<3,1>(0,0) = Eigen::Matrix<_Scalar, 3, 1>::Zero();
520  }
521 
522  if (incl_on && _Inclinometers)
523  {
529  gtilde_body = q4.inverse() * gtilde;
530  vec2product << 0, -gtilde_body(2), gtilde_body(1),
531  gtilde_body(2), 0, -gtilde_body(0),
532  -gtilde_body(1), gtilde_body(0), 0;
533 
534  H3.template block<3, 3> (0,0) = 2*vec2product;
535 
537  z3 = (incl) - bihat - gtilde_body;
538 
539  #ifdef INDIRECT_KALMAN_FILTER_DEBUG_PRINTS
540  std::cout<<"incl:\n"<<incl<<"\n";
541  std::cout<<"z3:\n"<<z3<<"\n";
542  std::cout<<"g_body:\n"<<gtilde_body<<"\n";
543  #endif
544 
546  R3 = adapAttIncl->matrix (x, P, z3, H3, Ri);
547 
549  P3 = P;
550  Eigen::Matrix<_Scalar, 3, 3> S3, S3_inverse;
551  S3 = H3 * P3 * H3.transpose() + R3;
552  S3_inverse = S3.inverse();
553  K3 = P3 * H3.transpose() * S3_inverse;
554  Eigen::Matrix<_Scalar, 3, 1> innovationIncl = (z3 - H3 * x);
555 
557  x = x + K3 * innovationIncl;
558  P = (Eigen::Matrix<_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE>::Identity()
559  -K3*H3)*P*(Eigen::Matrix<_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE>::Identity()
560  -K3*H3).transpose() + K3*R3*K3.transpose();
561 
562  #ifdef INDIRECT_KALMAN_FILTER_DEBUG_PRINTS
563  std::cout<<"x(k+1|k+1):\n"<<x<<"\n";
564  std::cout<<"P(k+1|k+1):\n"<<P<<"\n";
565  std::cout<<"innovation:\n"<<innovationIncl<<"\n";
566  std::cout<<"K3:\n"<<K3<<"\n";
567  std::cout<<"R3:\n"<<R3<<"\n";
568  #endif
569 
572  qe.w() = 1;
573  qe.x() = x(0);
574  qe.y() = x(1);
575  qe.z() = x(2);
576  q4 = q4 * qe;
577 
579  q4.normalize();
580 
582  x.template block<3,1>(0,0) = Eigen::Matrix<_Scalar, 3, 1>::Zero();
583  }
584 
588  bghat = bghat + x.template block<3, 1> (3,0);
589  x.template block<3, 1> (3,0) = Eigen::Matrix <_Scalar, 3, 1>::Zero();
590 
591  if (_Accelerometers)
592  {
593  bahat = bahat + x.template block<3, 1> (6,0);
594  x.template block<3, 1> (6,0) = Eigen::Matrix <_Scalar, 3, 1>::Zero();
595  }
596 
597  if (_Inclinometers)
598  {
599  bihat = bihat + x.template block<3, 1> (6+(flagAcc*3),0);
600  x.template block<3, 1> (6+(flagAcc*3),0) = Eigen::Matrix <_Scalar, 3, 1>::Zero();
601  }
602 
604  P = this->guaranteeSPD< Eigen::Matrix <_Scalar,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> >(P);
605  P = 0.5 * (P + P.transpose());//Guarantee symmetry
606 
607  #ifdef INDIRECT_KALMAN_FILTER_DEBUG_PRINTS
608  std::cout<<"bahat:\n"<<bahat<<"\n";
609  std::cout<<"bghat:\n"<<bghat<<"\n";
610  std::cout<<"bihat:\n"<<bihat<<"\n";
611  #endif
612 
613  return;
614 
615  }
616 
629  bool setAttitude (const Eigen::Quaternion <double> &initq)
630  {
631  if (&initq != NULL)
632  {
634  q4 = initq;
635 
636  return true;
637  }
638  return false;
639  }
640 
654  void setState (const Eigen::Matrix <double,IKFSTATEVECTORSIZE,1> &x_0)
655  {
656  x = x_0;
657 
658  return;
659  }
660 
674  bool setOmega (const Eigen::Matrix <_Scalar, 3,1> &u)
675  {
676  if (&u != NULL)
677  {
679  oldomega4 << 0,-(u)(0), -(u)(1), -(u)(2),
680  (u)(0), 0, (u)(2), -(u)(1),
681  (u)(1), -(u)(2), 0, (u)(0),
682  (u)(2), (u)(1), -(u)(0), 0;
683 
684  return true;
685  }
686  return false;
687  }
688 
701  void setInitBias (const Eigen::Matrix<_Scalar, 3, 1> &gbias,
702  const Eigen::Matrix<_Scalar, 3, 1> &abias,
703  const Eigen::Matrix<_Scalar, 3, 1> &ibias)
704  {
705  this->bghat = gbias;
706  this->bahat = abias;
707  this->bihat = ibias;
708 
709  return;
710  }
711 
724  void setGravity(const double gravity)
725  {
726  gtilde << 0.00, 0.00, gravity;
727  return;
728  }
729 
738  void setCovariance(const Eigen::Matrix< double, Ikf::IKFSTATEVECTORSIZE , Ikf::IKFSTATEVECTORSIZE> &Pk)
739  {
740  P = Pk;
741  return;
742  }
743 
747  Eigen::Matrix<double, 3, 1> getGyroBias()
748  {
749  return this->bghat;
750  }
751 
755  Eigen::Matrix<double, 3, 1> getAccBias()
756  {
757  return this->bahat;
758  }
759 
763  Eigen::Matrix<double, 3, 1> getInclBias()
764  {
765  return this->bihat;
766  }
767 
776  Eigen::Matrix <double,IKFSTATEVECTORSIZE,1> getState()
777  {
778  return x;
779  }
780 
784  Eigen::Matrix<_Scalar, 3, 1> getGravityinBody()
785  {
786  return q4.inverse() * gtilde;
787  }
788 
792  inline Eigen::Matrix<_Scalar, 3, 1> getGravity() const
793  {
794  return gtilde;
795  }
796 
805  Eigen::Quaternion <double> getAttitude()
806  {
807  return q4;
808  }
809 
818  Eigen::Matrix <double,IKFSTATEVECTORSIZE,IKFSTATEVECTORSIZE> getCovariance()
819  {
820  return P;
821  }
822 
838  void Quaternion2DCM(Eigen::Quaternion< _Scalar >* q, Eigen::Matrix< _Scalar, 3, 3 >*C)
839  {
840  double q0, q1, q2, q3;
841 
842  if (C != NULL)
843  {
845  q0 = q->w();
846  q1 = q->x();
847  q2 = q->y();
848  q3 = q->z();
849 
851  (*C)(0,0) = 2 * q0 * q0 + 2 * q1 * q1 - 1;
852  (*C)(0,1) = 2 * q1 * q2 + 2 * q0 * q3;
853  (*C)(0,2) = 2 * q1 * q3 - 2 * q0 * q2;
854  (*C)(1,0) = 2 * q1 * q2 - 2 * q0 * q3;
855  (*C)(1,1) = 2 * q0 * q0 + 2 * q2 * q2 - 1;
856  (*C)(1,2) = 2 * q2 * q3 + 2 * q0 * q1;
857  (*C)(2,0) = 2 * q1 * q3 + 2 * q0 * q2;
858  (*C)(2,1) = 2 * q2 * q3 - 2 * q0 * q1;
859  (*C)(2,2) = 2 * q0 * q0 + 2 * q3 * q3 - 1;
860  }
861  return;
862  }
863 
867  template <typename _MatrixType>
868  static _MatrixType guaranteeSPD (const _MatrixType &A)
869  {
870  _MatrixType spdA;
871  Eigen::VectorXd s;
872  s.resize(A.rows(), 1);
873 
877  Eigen::JacobiSVD <Eigen::MatrixXd > svdOfA (A, Eigen::ComputeThinU | Eigen::ComputeThinV);
878 
879  s = svdOfA.singularValues();
880 
881  #ifdef INDIRECT_KALMAN_FILTER_DEBUG_PRINTS
882  std::cout<<"[SPD-SVD] s: \n"<<s<<"\n";
883  std::cout<<"[SPD-SVD] svdOfA.matrixU():\n"<<svdOfA.matrixU()<<"\n";
884  std::cout<<"[SPD-SVD] svdOfA.matrixV():\n"<<svdOfA.matrixV()<<"\n";
885 
886  Eigen::EigenSolver<_MatrixType> eig(A);
887  std::cout << "[SPD-SVD] BEFORE: eigen values: " << eig.eigenvalues().transpose() << std::endl;
888  #endif
889 
890  for (register int i=0; i<s.size(); ++i)
891  {
892  #ifdef INDIRECT_KALMAN_FILTER_DEBUG_PRINTS
893  std::cout<<"[SPD-SVD] i["<<i<<"]\n";
894  #endif
895 
896  if (s(i) < 0.00)
897  s(i) = 0.00;
898  }
899 
900  spdA = svdOfA.matrixU() * s.matrix().asDiagonal() * svdOfA.matrixV().transpose();
901 
902  #ifdef INDIRECT_KALMAN_FILTER_DEBUG_PRINTS
903  Eigen::EigenSolver<_MatrixType> eigSPD(spdA);
904  if (eig.eigenvalues() == eigSPD.eigenvalues())
905  std::cout<<"[SPD-SVD] EQUAL!!\n";
906 
907  std::cout << "[SPD-SVD] AFTER: eigen values: " << eigSPD.eigenvalues().transpose() << std::endl;
908  #endif
909 
910  return spdA;
911  };
912  };
913 } // end namespace filter
914 
915 #endif
filter::Ikf::getInclBias
Eigen::Matrix< double, 3, 1 > getInclBias()
Gets the current inclinometers bias.
Definition: Ikf.hpp:763
filter::Ikf::initAdaptiveAttitude
void initAdaptiveAttitude(const unsigned int accM1, const unsigned int accM2, const double accGamma, const unsigned int incM1, const unsigned int incM2, const double incGamma)
Definition: Ikf.hpp:104
filter::Ikf::guaranteeSPD
static _MatrixType guaranteeSPD(const _MatrixType &A)
Method to guarantee Semi-Positive Definite (SPD) matrix.
Definition: Ikf.hpp:868
filter::Ikf::getState
Eigen::Matrix< double, IKFSTATEVECTORSIZE, 1 > getState()
Gets the current state vector of the filter.
Definition: Ikf.hpp:776
filter::Ikf::update
void update(const Eigen::Matrix< _Scalar, 3, 1 > &measurement1, const Eigen::Matrix< _Scalar, 3, 1 > &measurement2)
Definition: Ikf.hpp:341
filter::Ikf
Definition: Ikf.hpp:45
filter::Ikf::Init
void Init(const Eigen::Matrix< _Scalar, Ikf::IKFSTATEVECTORSIZE, Ikf::IKFSTATEVECTORSIZE > &P_0, const Eigen::Matrix< _Scalar, 3, 3 > &Ra, const Eigen::Matrix< _Scalar, 3, 3 > &Rg, const Eigen::Matrix< _Scalar, 3, 3 > &Rm, const Eigen::Matrix< _Scalar, 3, 3 > &Ri, const Eigen::Matrix< _Scalar, 3, 3 > &Qbg, const Eigen::Matrix< _Scalar, 3, 3 > &Qba, const Eigen::Matrix< _Scalar, 3, 3 > &Qbi, double g, double alpha, unsigned int am1, unsigned int am2, double agamma, unsigned int im1, unsigned int im2, double igamma)
This function Initialize the vectors and matrix of the IKF.
Definition: Ikf.hpp:142
filter::Ikf::getAccBias
Eigen::Matrix< double, 3, 1 > getAccBias()
Gets the current accelerometers bias.
Definition: Ikf.hpp:755
filter::AdaptiveAttitudeCov
Class for Adaptive measurement matrix for the attitude correction in 3D.
Definition: AdaptiveAttitudeCov.hpp:18
filter
Definition: AdaptiveAttitudeCov.hpp:13
filter::Ikf::getGyroBias
Eigen::Matrix< double, 3, 1 > getGyroBias()
Gets the current gyroscopes bias.
Definition: Ikf.hpp:747
filter::Ikf::update
void update(const Eigen::Matrix< _Scalar, 3, 1 > &acc, bool acc_on, const Eigen::Matrix< _Scalar, 3, 1 > &measurement, bool measurement_on)
Definition: Ikf.hpp:326
filter::Ikf::getGravityinBody
Eigen::Matrix< _Scalar, 3, 1 > getGravityinBody()
Gets gravity in the IMU body frame.
Definition: Ikf.hpp:784
filter::Ikf::~Ikf
~Ikf()
Definition: Ikf.hpp:116
filter::Ikf::setGravity
void setGravity(const double gravity)
Initial gravity.
Definition: Ikf.hpp:724
filter::Ikf::getCovariance
Eigen::Matrix< double, IKFSTATEVECTORSIZE, IKFSTATEVECTORSIZE > getCovariance()
Gets Noise covariance matrix.
Definition: Ikf.hpp:818
filter::Ikf::setOmega
bool setOmega(const Eigen::Matrix< _Scalar, 3, 1 > &u)
This function set the initial Omega matrix.
Definition: Ikf.hpp:674
filter::Ikf::Quaternion2DCM
void Quaternion2DCM(Eigen::Quaternion< _Scalar > *q, Eigen::Matrix< _Scalar, 3, 3 > *C)
Conversion Quaternion to DCM (Direct Cosine Matrix) (Alternative to Eigen)
Definition: Ikf.hpp:838
filter::Ikf::getAttitude
Eigen::Quaternion< double > getAttitude()
Gets the current orientation in Quaternion.
Definition: Ikf.hpp:805
filter::Ikf::setCovariance
void setCovariance(const Eigen::Matrix< double, Ikf::IKFSTATEVECTORSIZE, Ikf::IKFSTATEVECTORSIZE > &Pk)
Set the filter covariance.
Definition: Ikf.hpp:738
filter::Ikf::setInitBias
void setInitBias(const Eigen::Matrix< _Scalar, 3, 1 > &gbias, const Eigen::Matrix< _Scalar, 3, 1 > &abias, const Eigen::Matrix< _Scalar, 3, 1 > &ibias)
On/Off initial bias.
Definition: Ikf.hpp:701
filter::Ikf::setState
void setState(const Eigen::Matrix< double, IKFSTATEVECTORSIZE, 1 > &x_0)
This function Initialize the State vector.
Definition: Ikf.hpp:654
filter::Ikf::getGravity
Eigen::Matrix< _Scalar, 3, 1 > getGravity() const
Gets gravity in the local Geographic frame.
Definition: Ikf.hpp:792
filter::Ikf::update
void update(const Eigen::Matrix< _Scalar, 3, 1 > &acc, bool acc_on)
Definition: Ikf.hpp:315
filter::Ikf::update
void update(const Eigen::Matrix< _Scalar, 3, 1 > &acc, bool acc_on, const Eigen::Matrix< _Scalar, 3, 1 > &incl, bool incl_on, const Eigen::Matrix< _Scalar, 3, 1 > &mag, bool magn_on)
Performs the measurement and correction steps of the filter.
Definition: Ikf.hpp:386
filter::Ikf::setAttitude
bool setAttitude(const Eigen::Quaternion< double > &initq)
This function Initialize Attitude.
Definition: Ikf.hpp:629
filter::Ikf::predict
void predict(Eigen::Matrix< _Scalar, 3, 1 > &u, double dt)
Performs the prediction step of the filter.
Definition: Ikf.hpp:262
Generated by   doxygen 1.8.13