AHRS.h

/*
 * AHRS.h
 *
 *  Created on: Jan 26, 2012
 *      Author: cbeall3
 */
 
#pragma once
 
#include "Mechanization_bRn2.h"
#include <gtsam_unstable/dllexport.h>
#include <gtsam/linear/KalmanFilter.h>
 
namespace gtsam {
 
class GTSAM_UNSTABLE_EXPORT AHRS {
 
private:
 
  // Initial state
  Mechanization_bRn2 mech0_; 
  KalmanFilter KF_;          
 
  // Quantities needed for integration
  Matrix3 F_g_;              
  Matrix3 F_a_;              
 
  typedef Eigen::Matrix<double,12,1> Variances;
  Variances var_w_;            
 
  // Quantities needed for aiding
  Vector3 sigmas_v_a_;       
  Vector3 n_g_;              
  Matrix3 n_g_cross_;        
 
  Matrix3 Pg_, Pa_;
 
public:
 
  typedef Eigen::Matrix<double,3,Eigen::Dynamic> Vector3s;
  static Matrix3 Cov(const Vector3s& m);

  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;

  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;

  void print(const std::string& s = "") const;
 
  virtual ~AHRS();
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
} /* namespace gtsam */