gtsam  4.1.0
gtsam
AHRS.h
1 /*
2  * AHRS.h
3  *
4  * Created on: Jan 26, 2012
5  * Author: cbeall3
6  */
7 
8 #ifndef AHRS_H_
9 #define AHRS_H_
10 
11 #include "Mechanization_bRn2.h"
12 #include <gtsam_unstable/dllexport.h>
14 
15 namespace gtsam {
16 
17 class GTSAM_UNSTABLE_EXPORT AHRS {
18 
19 private:
20 
21  // Initial state
22  Mechanization_bRn2 mech0_;
23  KalmanFilter KF_;
24 
25  // Quantities needed for integration
26  Matrix3 F_g_;
27  Matrix3 F_a_;
28 
29  typedef Eigen::Matrix<double,12,1> Variances;
30  Variances var_w_;
31 
32  // Quantities needed for aiding
33  Vector3 sigmas_v_a_;
34  Vector3 n_g_;
35  Matrix3 n_g_cross_;
36 
37  Matrix3 Pg_, Pa_;
38 
39 public:
40 
41  typedef Eigen::Matrix<double,3,Eigen::Dynamic> Vector3s;
42  static Matrix3 Cov(const Vector3s& m);
43 
50  AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, bool flat=false);
51 
52  std::pair<Mechanization_bRn2, KalmanFilter::State> initialize(double g_e);
53 
54  std::pair<Mechanization_bRn2, KalmanFilter::State> integrate(
55  const Mechanization_bRn2& mech, KalmanFilter::State state,
56  const Vector& u, double dt);
57 
58  bool isAidingAvailable(const Mechanization_bRn2& mech,
59  const Vector& f, const Vector& u, double ge) const;
60 
68  std::pair<Mechanization_bRn2, KalmanFilter::State> aid(
69  const Mechanization_bRn2& mech, KalmanFilter::State state,
70  const Vector& f, bool Farrell=0) const;
71 
72  std::pair<Mechanization_bRn2, KalmanFilter::State> aidGeneral(
73  const Mechanization_bRn2& mech, KalmanFilter::State state,
74  const Vector& f, const Vector& f_expected, const Rot3& R_previous) const;
75 
77  void print(const std::string& s = "") const;
78 
79  virtual ~AHRS();
80 
81  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
82 };
83 
84 } /* namespace gtsam */
85 #endif /* AHRS_H_ */
gtsam::AHRS
Definition: AHRS.h:17
gtsam::KalmanFilter::State
GaussianDensity::shared_ptr State
The Kalman filter state is simply a GaussianDensity.
Definition: KalmanFilter.h:56
gtsam::Mechanization_bRn2
Definition: Mechanization_bRn2.h:17
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::KalmanFilter
Kalman Filter class.
Definition: KalmanFilter.h:41
gtsam::Rot3
Definition: Rot3.h:59
KalmanFilter.h
Simple linear Kalman filter.