kalman_filter_wrapper.hpp

Go to the documentation of this file.

// Copyright 2020 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/measurement.hpp>
#include <state_estimation_nodes/measurement_time_keeper.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>;
 
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,
    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}
  {
    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_ekf = std::make_unique<FilterT>(m_motion_model, m_GQ_left_factor);
  }
 
  template<typename MeasurementT>
  void reset(const MeasurementT & measurement, const GlobalTime & time_of_event_occurance);
 
  void reset(
    const VectorT<kNumOfStates> & state,
    const SquareMatrixT<kNumOfStates> & initial_covariance_chol,
    const MeasurementBasedTime & event_timestamp,
    const GlobalTime & time_of_event_occurance);
 
  template<TimeReferenceFrame kTimeReferenceFrame>
  common::types::bool8_t temporal_update(
    const Time<kTimeReferenceFrame> & time_of_update);
 
  template<typename MeasurementT>
  common::types::bool8_t observation_update(
    const GlobalTime & message_received_time,
    const MeasurementT & measurement);
 
  common::types::bool8_t is_initialized() const noexcept
  {
    return m_ekf_initialized && m_time_keeper.is_initialized();
  }
 
  nav_msgs::msg::Odometry get_state() const;
 
private:
  common::types::bool8_t passes_mahalanobis_gate(
    const VectorT<kNumOfStates> & sample,
    const VectorT<kNumOfStates> & mean,
    const SquareMatrixT<kNumOfStates> & covariance_factor) const;
 
  common::types::bool8_t m_ekf_initialized{};
  MeasurementBasedTimeKeeper m_time_keeper{};
  MotionModelT m_motion_model;
  RectangularMatrixT<kNumOfStates, kProcessNoiseDim> m_GQ_left_factor;
  SquareMatrixT<kNumOfStates> m_initial_covariance_factor;
  std::unique_ptr<FilterT> m_ekf{};
  std::string m_frame_id{};
  common::types::float32_t m_mahalanobis_threshold{};
};
 
using ConstantAccelerationFilter =
  KalmanFilterWrapper<motion::motion_model::ConstantAcceleration, 6, 2>;
 
}  // namespace prediction
}  // namespace autoware
 
#endif  // STATE_ESTIMATION_NODES__KALMAN_FILTER_WRAPPER_HPP_