AHRS.h
Go to the documentation of this file.
1 /*
2  * AHRS.h
3  *
4  * Created on: Jan 26, 2012
5  * Author: cbeall3
6  */
7 
8 #pragma once
9 
10 #include "Mechanization_bRn2.h"
11 #include <gtsam_unstable/dllexport.h>
13 
14 namespace gtsam {
15 
16 class GTSAM_UNSTABLE_EXPORT AHRS {
17 
18 private:
19 
20  // Initial state
23 
24  // Quantities needed for integration
25  Matrix3 F_g_;
26  Matrix3 F_a_;
27 
29  Variances var_w_;
30 
31  // Quantities needed for aiding
34  Matrix3 n_g_cross_;
35 
36  Matrix3 Pg_, Pa_;
37 
38 public:
39 
41  static Matrix3 Cov(const Vector3s& m);
42 
49  AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, bool flat=false);
50 
51  std::pair<Mechanization_bRn2, KalmanFilter::State> initialize(double g_e);
52 
53  std::pair<Mechanization_bRn2, KalmanFilter::State> integrate(
55  const Vector& u, double dt);
56 
57  bool isAidingAvailable(const Mechanization_bRn2& mech,
58  const Vector& f, const Vector& u, double ge) const;
59 
67  std::pair<Mechanization_bRn2, KalmanFilter::State> aid(
69  const Vector& f, bool Farrell=0) const;
70 
71  std::pair<Mechanization_bRn2, KalmanFilter::State> aidGeneral(
73  const Vector& f, const Vector& f_expected, const Rot3& R_previous) const;
74 
76  void print(const std::string& s = "") const;
77 
78  virtual ~AHRS();
79 
81 };
82 
83 } /* namespace gtsam */
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Matrix3f m
Mechanization_bRn2 mech0_
Initial mechanization state.
Definition: AHRS.h:21
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector3 sigmas_v_a_
measurement sigma
Definition: AHRS.h:32
GaussianDensity::shared_ptr State
Definition: KalmanFilter.h:56
Matrix3 F_g_
gyro bias dynamics
Definition: AHRS.h:25
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Memory.h:841
Eigen::Matrix< double, 3, Eigen::Dynamic > Vector3s
Definition: AHRS.h:40
Matrix stationaryF
Definition: testAHRS.cpp:20
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
const double dt
double g_e
Definition: testAHRS.cpp:21
Eigen::VectorXd Vector
Definition: Vector.h:38
Matrix stationaryU
Definition: testAHRS.cpp:19
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
RealScalar s
Vector3 n_g_
gravity in nav frame
Definition: AHRS.h:33
KalmanFilter KF_
initial Kalman filter
Definition: AHRS.h:22
Variances var_w_
dynamic noise variances
Definition: AHRS.h:29
traits
Definition: chartTesting.h:28
Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF...
Matrix3 Pg_
Definition: AHRS.h:36
Matrix3 F_a_
acc bias dynamics
Definition: AHRS.h:26
Matrix3 n_g_cross_
and its skew-symmetric matrix
Definition: AHRS.h:34
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:375
Eigen::Matrix< double, 12, 1 > Variances
Definition: AHRS.h:28


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:33:53