kalman_filter_wrapper.hpp

Go to the documentation of this file.

// Copyright 2021 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.


#ifndef STATE_ESTIMATION_NODES__KALMAN_FILTER_WRAPPER_HPP_
#define STATE_ESTIMATION_NODES__KALMAN_FILTER_WRAPPER_HPP_

#include <common/types.hpp>
#include <kalman_filter/esrcf.hpp>
#include <motion_model/constant_acceleration.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <state_estimation_nodes/history.hpp>
#include <state_estimation_nodes/measurement.hpp>
#include <state_estimation_nodes/measurement_typedefs.hpp>
#include <state_estimation_nodes/steady_time_grid.hpp>
#include <state_estimation_nodes/visibility_control.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <limits>
#include <cstdint>
#include <memory>
#include <string>
#include <chrono>

namespace autoware
{
namespace prediction
{

template<typename MotionModelT, std::int32_t kNumOfStates, std::int32_t kProcessNoiseDim>
class STATE_ESTIMATION_NODES_PUBLIC KalmanFilterWrapper
{
  using FilterT = prediction::kalman_filter::Esrcf<kNumOfStates, kProcessNoiseDim>;

  template<std::int32_t kRows, std::int32_t kCols>
  using RectangularMatrixT = Eigen::Matrix<common::types::float32_t, kRows, kCols>;

  template<std::int32_t kNum>
  using SquareMatrixT = Eigen::Matrix<common::types::float32_t, kNum, kNum>;

  template<std::int32_t kLength>
  using VectorT = Eigen::Matrix<common::types::float32_t, kLength, 1>;

  using HistoryT = History<
    FilterT,
    kNumOfStates,
    PredictionEvent,
    ResetEvent<FilterT>,
    MeasurementPose,
    MeasurementSpeed,
    MeasurementPoseAndSpeed>;

public:
  KalmanFilterWrapper(
    const SquareMatrixT<kNumOfStates> & initial_covariance_factor,
    const RectangularMatrixT<kNumOfStates, kProcessNoiseDim> & process_noise,
    const std::chrono::nanoseconds & expected_dt,
    const std::string & frame_id,
    const std::chrono::nanoseconds & history_duration = std::chrono::milliseconds{5000},
    common::types::float32_t mahalanobis_threshold =
    std::numeric_limits<common::types::float32_t>::max(),
    const MotionModelT & motion_model = MotionModelT{})
  : m_motion_model{motion_model},
    m_initial_covariance_factor{initial_covariance_factor},
    m_frame_id{frame_id},
    m_mahalanobis_threshold{mahalanobis_threshold},
    m_expected_prediction_period{expected_dt}
  {
    static_assert(
      motion_model.get_num_states() == kNumOfStates,
      "Wrong number of states in the motion model.");

    SquareMatrixT<kNumOfStates> F;
    m_motion_model.compute_jacobian(F, expected_dt);
    m_GQ_left_factor = F * process_noise;
    m_filter = std::make_unique<FilterT>(m_motion_model, m_GQ_left_factor);
    m_history = std::make_unique<HistoryT>(
      *m_filter,
      static_cast<std::size_t>(history_duration / expected_dt),
      m_mahalanobis_threshold);
  }

  template<typename MeasurementT>
  void add_reset_event_to_history(const MeasurementT & measurement);

  void add_reset_event_to_history(
    const VectorT<kNumOfStates> & state,
    const SquareMatrixT<kNumOfStates> & initial_covariance_chol,
    const std::chrono::system_clock::time_point & event_timestamp);

  common::types::bool8_t add_next_temporal_update_to_history();

  template<typename MeasurementT>
  common::types::bool8_t add_observation_to_history(const MeasurementT & measurement);

  inline common::types::bool8_t is_initialized() const noexcept
  {
    return (m_history) && (!m_history->empty()) && m_time_grid.is_initialized();
  }

  nav_msgs::msg::Odometry get_state() const;

private:
  std::unique_ptr<FilterT> m_filter{};
  SteadyTimeGrid m_time_grid{};
  MotionModelT m_motion_model;
  RectangularMatrixT<kNumOfStates, kProcessNoiseDim> m_GQ_left_factor;
  SquareMatrixT<kNumOfStates> m_initial_covariance_factor;
  std::string m_frame_id{};
  common::types::float32_t m_mahalanobis_threshold{};
  std::unique_ptr<HistoryT> m_history{};
  std::chrono::nanoseconds m_expected_prediction_period{};
};

using ConstantAccelerationFilter =
  KalmanFilterWrapper<motion::motion_model::ConstantAcceleration, 6, 2>;

}  // namespace prediction
}  // namespace autoware

#endif  // STATE_ESTIMATION_NODES__KALMAN_FILTER_WRAPPER_HPP_