core_state.h

Go to the documentation of this file.

// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>
 
#ifndef CORESTATE_H
#define CORESTATE_H
 
#include <mars/sensors/imu/imu_measurement_type.h>
#include <mars/sensors/sensor_abs_class.h>
#include <mars/time.h>
#include <mars/type_definitions/core_state_type.h>
#include <mars/type_definitions/core_type.h>
#include <Eigen/Dense>
 
namespace mars
{
class CoreState
{
private:
  bool fixed_acc_bias_{ false };   
  bool fixed_gyro_bias_{ false };  
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  CoreStateType state;  
 
  // Noise (STD)
  Eigen::Vector3d n_a_{ Eigen::Vector3d::Zero() };   
  Eigen::Vector3d n_ba_{ Eigen::Vector3d::Zero() };  
  Eigen::Vector3d n_w_{ Eigen::Vector3d::Zero() };   
  Eigen::Vector3d n_bw_{ Eigen::Vector3d::Zero() };  
 
  const Eigen::Vector3d g_{ 0, 0, 9.81 };  
 
  CoreStateMatrix initial_covariance_;
 
  std::shared_ptr<SensorAbsClass> propagation_sensor_{ nullptr };  
 
  bool test_state_transition_{ false };  
  bool verbose_{ false };                
 
  CoreState();
 
  void set_fixed_acc_bias(const bool& value);
 
  void set_fixed_gyro_bias(const bool& value);
 
  void set_propagation_sensor(std::shared_ptr<SensorAbsClass> propagation_sensor);
 
  void set_noise_std(const Eigen::Vector3d& n_w, const Eigen::Vector3d& n_bw, const Eigen::Vector3d& n_a,
                     const Eigen::Vector3d& n_ba);
 
  CoreStateType InitializeState(const Eigen::Vector3d& ang_vel, const Eigen::Vector3d& lin_acc,
                                const Eigen::Vector3d& p_wi, const Eigen::Vector3d& v_wi,
                                const Eigen::Quaterniond& q_wi, const Eigen::Vector3d& b_w, const Eigen::Vector3d& b_a);
 
  void set_initial_covariance(const Eigen::Vector3d& p, const Eigen::Vector3d& v, const Eigen::Vector3d& q,
                              const Eigen::Vector3d& bw, const Eigen::Vector3d& ba);
 
  CoreStateMatrix InitializeCovariance();
 
  CoreStateType PropagateState(const CoreStateType& prior_state, const IMUMeasurementType& measurement,
                               const double& dt);
  CoreType PredictProcessCovariance(const CoreType& prior_core_state, const IMUMeasurementType& system_input,
                                    const double& dt);
 
  // Static
  static CoreStateMatrix GenerateFdTaylor();
 
  static CoreStateMatrix GenerateFdClosedForm();
 
  static CoreStateMatrix GenerateFdSmallAngleApprox(const Eigen::Quaterniond& q_wi, const Eigen::Vector3d& a_est,
                                                    const Eigen::Vector3d& w_est, const double& dt);
  static CoreStateMatrix CalcQSmallAngleApprox(const double& dt, const Eigen::Quaterniond& q_wi,
                                               const Eigen::Vector3d& a_m, const Eigen::Vector3d& n_a,
                                               const Eigen::Vector3d& b_a, const Eigen::Vector3d& n_ba,
                                               const Eigen::Vector3d& w_m, const Eigen::Vector3d& n_w,
                                               const Eigen::Vector3d& b_w, const Eigen::Vector3d& n_bw);
};
}  // namespace mars
 
#endif  // CORESTATE_H