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 */