84 lines
2.4 KiB
C++
84 lines
2.4 KiB
C++
/*
|
|
* AHRS.h
|
|
*
|
|
* Created on: Jan 26, 2012
|
|
* Author: cbeall3
|
|
*/
|
|
|
|
#ifndef AHRS_H_
|
|
#define AHRS_H_
|
|
|
|
#include "Mechanization_bRn2.h"
|
|
#include <gtsam_unstable/base/dllexport.h>
|
|
#include <gtsam/linear/KalmanFilter.h>
|
|
|
|
namespace gtsam {
|
|
|
|
class GTSAM_UNSTABLE_EXPORT AHRS {
|
|
|
|
private:
|
|
|
|
// Initial state
|
|
Mechanization_bRn2 mech0_; ///< Initial mechanization state
|
|
KalmanFilter KF_; ///< initial Kalman filter
|
|
|
|
// Quantities needed for integration
|
|
Matrix3 F_g_; ///< gyro bias dynamics
|
|
Matrix3 F_a_; ///< acc bias dynamics
|
|
|
|
typedef Eigen::Matrix<double,12,1> Variances;
|
|
Variances var_w_; ///< dynamic noise variances
|
|
|
|
// Quantities needed for aiding
|
|
Vector3 sigmas_v_a_; ///< measurement sigma
|
|
Vector3 n_g_; ///< gravity in nav frame
|
|
Matrix3 n_g_cross_; ///< and its skew-symmetric matrix
|
|
|
|
Matrix3 Pg_, Pa_;
|
|
|
|
public:
|
|
|
|
typedef Eigen::Matrix<double,3,Eigen::Dynamic> Vector3s;
|
|
static Matrix3 Cov(const Vector3s& m);
|
|
|
|
/**
|
|
* AHRS constructor
|
|
* @param stationaryU initial interval of gyro measurements, 3xn matrix
|
|
* @param stationaryF initial interval of accelerator measurements, 3xn matrix
|
|
* @param g_e if given, initializes gravity with correct value g_e
|
|
*/
|
|
AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, bool flat=false);
|
|
|
|
std::pair<Mechanization_bRn2, KalmanFilter::State> initialize(double g_e);
|
|
|
|
std::pair<Mechanization_bRn2, KalmanFilter::State> integrate(
|
|
const Mechanization_bRn2& mech, KalmanFilter::State state,
|
|
const Vector& u, double dt);
|
|
|
|
bool isAidingAvailable(const Mechanization_bRn2& mech,
|
|
const Vector& f, const Vector& u, double ge) const;
|
|
|
|
/**
|
|
* Aid the AHRS with an accelerometer reading, typically called when isAidingAvailable == true
|
|
* @param mech current mechanization state
|
|
* @param state current Kalman filter state
|
|
* @param f accelerometer reading
|
|
* @param Farrell
|
|
*/
|
|
std::pair<Mechanization_bRn2, KalmanFilter::State> aid(
|
|
const Mechanization_bRn2& mech, KalmanFilter::State state,
|
|
const Vector& f, bool Farrell=0) const;
|
|
|
|
std::pair<Mechanization_bRn2, KalmanFilter::State> aidGeneral(
|
|
const Mechanization_bRn2& mech, KalmanFilter::State state,
|
|
const Vector& f, const Vector& f_expected, const Rot3& R_previous) const;
|
|
|
|
/// print
|
|
void print(const std::string& s = "") const;
|
|
|
|
virtual ~AHRS();
|
|
};
|
|
|
|
} /* namespace gtsam */
|
|
#endif /* AHRS_H_ */
|