#ifndef ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_
#define ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_

#include "origincar_base/wall_fit_core.hpp"

namespace origincar_wall
{

struct WallKalmanNoise
{
  double x;
  double y;
  double theta;
};

class WallKalmanFilter
{
public:
  explicit WallKalmanFilter(const Pose2D &initial_pose);

  void reset(const Pose2D &pose);
  void predict(double vx, double vy, double yaw_rate, double dt);
  void correct(const Pose2D &measurement, const WallKalmanNoise &measurement_noise);

  Pose2D pose() const;
  WallKalmanNoise covariance() const;

private:
  Pose2D pose_;
  WallKalmanNoise covariance_;
  WallKalmanNoise process_noise_per_second_;
};

}  // namespace origincar_wall

#endif  // ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_
