core_logic.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 CORELOGIC_H
#define CORELOGIC_H
 
#include <mars/buffer.h>
#include <mars/core_state.h>
#include <mars/sensor_manager.h>
#include <mars/type_definitions/core_state_type.h>
#include <Eigen/Dense>
#include <iostream>
#include <memory>
 
namespace mars
{
class CoreLogic
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  std::shared_ptr<CoreState> core_states_{ nullptr };  
  Buffer buffer_;                                      
  Buffer buffer_prior_core_init_;                      
  SensorManager sensor_manager_;
  bool core_is_initialized_{ false };  
  bool core_init_warn_once_{ false };
  bool verbose_{ false };                    
  bool verbose_out_of_order_{ false };       
  bool discard_ooo_prop_meas_{ false };      
  bool add_interm_buffer_entries_{ false };  
 
  CoreLogic(std::shared_ptr<CoreState> core_states);
  CoreLogic() = default;
 
  int Initialize(const Eigen::Vector3d& p_wi_init, const Eigen::Quaterniond& q_wi_init);
 
  CoreStateMatrix GenerateStateTransitionBlock(const int& first_transition_idx, const int& last_transition_idx);
 
  Eigen::MatrixXd PropagateSensorCrossCov(const Eigen::MatrixXd& sensor_cov, const CoreStateMatrix& core_cov,
                                          const CoreStateMatrix& state_transition);
 
  bool PerformSensorUpdate(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp, BufferEntryType* sensor_data);
  bool PerformSensorUpdate(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp, BufferEntryType* sensor_data,
                           bool* added_interm_state);
 
  void PerformCoreStatePropagation(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp,
                                   const BufferEntryType& prior_state_entry, BufferEntryType* sensor_entry);
 
  void ReworkBufferStartingAtIndex(const int& index);
 
  bool ProcessMeasurement(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp, const BufferDataType& data);
};
}  // namespace mars
 
#endif  // CORELOGIC_H