ekf.hpp

// Copyright 2022 Jacob Hartzer
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
#ifndef EKF__EKF_HPP_
#define EKF__EKF_HPP_
 
#include <eigen3/Eigen/Eigen>
#include <stddef.h>
 
#include <memory>
#include <string>
#include <vector>
 
#include "ekf/constants.hpp"
#include "ekf/types.hpp"
#include "infrastructure/data_logger.hpp"
#include "infrastructure/debug_logger.hpp"
 
class EKF
{
public:
  typedef struct Parameters
  {
    std::shared_ptr<DebugLogger> debug_logger;                  
    double data_log_rate{0.0};                                  
    std::string log_directory{""};                              
    AugmentationType augmenting_type{AugmentationType::ALL};    
    double augmenting_delta_time{1.0};                          
    double augmenting_pos_error{0.1};                           
    double augmenting_ang_error{0.1};                           
    Eigen::VectorXd process_noise{Eigen::VectorXd::Ones(g_body_state_size)};
    Eigen::Vector3d pos_b_in_l{Eigen::Vector3d::Zero()};        
    Eigen::Quaterniond ang_b_to_l {1, 0, 0, 0};                 
    Eigen::Vector3d pos_e_in_g {Eigen::Vector3d::Zero()};       
    double ang_l_to_e{0.0};                                     
    GpsInitType gps_init_type {GpsInitType::CONSTANT};          
    double gps_init_baseline_dist {100.0};  
    double gps_init_pos_thresh {0.1};       
    double gps_init_ang_thresh {0.1};       
    double motion_detection_chi_squared {1.0};   
    double imu_noise_scale_factor {100.0};       
    bool use_root_covariance{false};  
    bool use_first_estimate_jacobian{false};  
  } Parameters;
 
  explicit EKF(Parameters params);
 
  unsigned int GetStateSize() const;
 
  ImuState GetImuState(unsigned int imu_id);
 
  GpsState GetGpsState(unsigned int gps_id);
  CamState GetCamState(unsigned int cam_id);
 
  unsigned int GetImuCount() const;
 
  unsigned int GetImuStateSize() const;
 
  unsigned int GetGpsCount() const;
 
  unsigned int GetGpsStateSize() const;
 
  unsigned int GetCamStateSize() const;
 
  unsigned int GetCamCount() const;
 
  void LogBodyStateIfNeeded(int execution_count);
 
  void PredictModel(double local_time);
 
  static Eigen::MatrixXd GetStateTransition(double delta_time);
 
  void Initialize(double initial_time, const BodyState & body_state_init);
 
  void RegisterIMU(
    unsigned int imu_id,
    const ImuState & imu_state,
    const Eigen::MatrixXd & covariance);
 
  void RegisterGPS(
    unsigned int gps_id,
    const GpsState & gps_state,
    const Eigen::Matrix3d & covariance);
 
  void RegisterCamera(
    unsigned int cam_id,
    const CamState & cam_state,
    const Eigen::MatrixXd & covariance);
 
  void RegisterFiducial(const FidState & fid_state, const Eigen::MatrixXd & covariance);
 
  Eigen::MatrixXd AugmentCovariance(const Eigen::MatrixXd & in_cov, unsigned int index) const;
 
  void AugmentStateIfNeeded();
 
  void AugmentStateIfNeeded(unsigned int camera_id, unsigned int frame_id);
 
  void SetMaxTrackLength(unsigned int max_track_length);
 
  void SetBodyProcessNoise(const Eigen::VectorXd & process_noise);
 
  void SetGpsReference(const Eigen::VectorXd & pos_e_in_g, double ang_l_to_e);
 
  void SetZeroAcceleration(bool is_zero_acceleration);
 
  Eigen::VectorXd GetReferenceLLA() const;
 
  double GetReferenceAngle() const;
 
  bool IsLlaInitialized() const;
 
  bool IsGravityInitialized() const;
 
  void InitializeGravity();
 
  AugState GetAugState(unsigned int camera_id, unsigned int frame_id, double time);
 
  unsigned int GetAugStateSize() const;
 
  void RefreshIndices();
 
  void AttemptGpsInitialization(
    double time,
    const Eigen::Vector3d & gps_lla);
 
  std::vector<double> GetGpsTimeVector() const;
 
  std::vector<Eigen::Vector3d> GetGpsEcefVector() const;
 
  std::vector<Eigen::Vector3d> GetGpsXyzVector() const;
 
  double GetCurrentTime() const;
 
  unsigned int GetImuStateStart() const;
 
  unsigned int GetGpsStateStart() const;
 
  unsigned int GetCamStateStart() const;
 
  unsigned int GetAugStateStart() const;
 
  unsigned int GetFidStateStart() const;
 
  double GetMotionDetectionChiSquared() const;
 
  double GetImuNoiseScaleFactor() const;
 
  bool GetUseRootCovariance() const;
 
  bool GetUseFirstEstimateJacobian() const;
 
  double CalculateLocalTime(double time);
 
  State m_state;
 
  Eigen::MatrixXd m_cov = Eigen::MatrixXd::Identity(g_body_state_size, g_body_state_size) * 1e-2;
 
private:
  unsigned int m_state_size{g_body_state_size};
  unsigned int m_imu_state_size{0};
  unsigned int m_gps_state_size{0};
  unsigned int m_cam_state_size{0};
  unsigned int m_aug_state_size{0};
  unsigned int m_fid_state_size{0};
  double m_current_time {0};
  double m_reference_time {0};
  bool m_time_initialized {false};
  unsigned int m_max_track_length{20};
  Eigen::MatrixXd m_process_noise {Eigen::MatrixXd::Zero(g_body_state_size, g_body_state_size)};
  Eigen::VectorXd m_body_process_noise {Eigen::VectorXd::Zero(g_body_state_size)};
  std::shared_ptr<DebugLogger> m_debug_logger;
  DataLogger m_data_logger;
  DataLogger m_augmentation_logger;
  double m_data_log_rate{0.0};
 
  GpsInitType m_gps_init_type;
  double m_gps_init_pos_thresh;
  double m_gps_init_ang_thresh;
  double m_gps_init_baseline_dist;
  bool m_is_lla_initialized;
  Eigen::Vector3d m_pos_e_in_g;
  double m_ang_l_to_e;
  std::vector<double> m_gps_time_vec;
  std::vector<Eigen::Vector3d> m_gps_ecef_vec;
  std::vector<Eigen::Vector3d> m_gps_xyz_vec;
 
  AugmentationType m_augmenting_type;
  double m_augmenting_prev_time;
  double m_augmenting_delta_time;
  double m_augmenting_pos_error;
  double m_augmenting_ang_error;
  unsigned int m_primary_camera_id{0};
  double m_max_frame_period {0.0};
  double m_max_track_duration {0.0};
  double m_min_aug_period {1.0};
 
  unsigned int m_imu_state_start{0};
  unsigned int m_gps_state_start{0};
  unsigned int m_cam_state_start{0};
  unsigned int m_aug_state_start{0};
  unsigned int m_fid_state_start{0};
 
  bool m_is_gravity_initialized{false};
  double m_motion_detection_chi_squared{1.0};
  double m_imu_noise_scale_factor{100.0};
 
  bool m_use_root_covariance{true};
  bool m_use_first_estimate_jacobian{false};
  bool m_is_zero_acceleration{true};
  bool m_frame_received_since_last_aug {true};
};
 
#endif  // EKF__EKF_HPP_