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>
12#include <gtsam/linear/KalmanFilter.h>
13
14namespace gtsam {
15
16class GTSAM_UNSTABLE_EXPORT AHRS {
17
18private:
19
20 // Initial state
21 Mechanization_bRn2 mech0_; ///< Initial mechanization state
22 KalmanFilter KF_; ///< initial Kalman filter
23
24 // Quantities needed for integration
25 Matrix3 F_g_; ///< gyro bias dynamics
26 Matrix3 F_a_; ///< acc bias dynamics
27
28 typedef Eigen::Matrix<double,12,1> Variances;
29 Variances var_w_; ///< dynamic noise variances
30
31 // Quantities needed for aiding
32 Vector3 sigmas_v_a_; ///< measurement sigma
33 Vector3 n_g_; ///< gravity in nav frame
34 Matrix3 n_g_cross_; ///< and its skew-symmetric matrix
35
36 Matrix3 Pg_, Pa_;
37
38public:
39
40 typedef Eigen::Matrix<double,3,Eigen::Dynamic> Vector3s;
41 static Matrix3 Cov(const Vector3s& m);
42
43 /**
44 * AHRS constructor
45 * @param stationaryU initial interval of gyro measurements, 3xn matrix
46 * @param stationaryF initial interval of accelerator measurements, 3xn matrix
47 * @param g_e if given, initializes gravity with correct value g_e
48 */
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(
54 const Mechanization_bRn2& mech, KalmanFilter::State state,
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
60 /**
61 * Aid the AHRS with an accelerometer reading, typically called when isAidingAvailable == true
62 * @param mech current mechanization state
63 * @param state current Kalman filter state
64 * @param f accelerometer reading
65 * @param Farrell
66 */
67 std::pair<Mechanization_bRn2, KalmanFilter::State> aid(
68 const Mechanization_bRn2& mech, KalmanFilter::State state,
69 const Vector& f, bool Farrell=0) const;
70
71 std::pair<Mechanization_bRn2, KalmanFilter::State> aidGeneral(
72 const Mechanization_bRn2& mech, KalmanFilter::State state,
73 const Vector& f, const Vector& f_expected, const Rot3& R_previous) const;
74
75 /// print
76 void print(const std::string& s = "") const;
77
78 virtual ~AHRS();
79
80 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81};
82
83} /* namespace gtsam */
84