propagator.hpp

// Copyright (C) 2023 Alessandro Fornasier.
// 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 authors at <alessandro.fornasier@ieee.org>
 
#ifndef PROPAGATOR_HPP
#define PROPAGATOR_HPP
 
#include "msceqf/options/msceqf_options.hpp"
#include "msceqf/symmetry/symmetry.hpp"
#include "sensors/sensor_data.hpp"
#include "msceqf/state/state.hpp"
#include "types/fptypes.hpp"
#include "utils/tools.hpp"
 
namespace msceqf
{
class Propagator
{
 public:
  using ImuBuffer = std::deque<Imu>;  
 
  Propagator(const PropagatorOptions& opts);
 
  void insertImu(MSCEqFState& X, const SystemState& xi0, const Imu& imu, fp& timestamp);
 
  bool propagate(MSCEqFState& X, const SystemState& xi0, fp& timestamp, const fp& new_timestamp);
 
 private:
  ImuBuffer getImuReadings(const fp& t0, const fp& t1);
 
  Imu lerp(const Imu& pre, const Imu& post, const fp& alpha);
 
  void propagateMean(MSCEqFState& X, const SystemState& xi0, const Imu& u, const fp& dt);
 
  void propagateCovariance(MSCEqFState& X, const SystemState& xi0, const Imu& u, const fp& dt);
 
  const MatrixX stateMatrix(MSCEqFState& X, const SystemState& xi0, const Imu& u) const;
 
  const MatrixX inputMatrix(MSCEqFState& X, const SystemState& xi0) const;
 
  const MatrixX discreteTimeMatrix(const MatrixX& A, const MatrixX& B, const fp& dt) const;
 
  ImuBuffer imu_buffer_;  
  Matrix12 Q_;            
 
  int state_transition_order_;  
  uint imu_buffer_max_size_;    
 
  std::mutex mutex_;  
 
  static constexpr fp eps_ = 1e-6;  
};
 
}  // namespace msceqf
 
#endif  // PROPAGATOR_HPP