Address all TODO-s in the code.

[ser94mor]

No description provided.

[YayoYY]

Here is a case to test CTRV process model + Lidar measurement model:

int main()
{
  using EKF = ExtendedKalmanFilter<CTRV::ProcessModel, Lidar::MeasurementModel<CTRV::ProcessModel>>;
  using BEL = Belief<CTRV::StateVector, CTRV::StateCovarianceMatrix>;

  float v = 2;
  float omega = 30 * M_PI / 180;
  float v_yaw_rate = v / omega;

  float x = 0;
  float y = 0;
  float theta = 0;
  CTRV::StateVector mu;
  mu << x, y, 0, 0, 0;
  CTRV::StateCovarianceMatrix Sigma = Eigen::MatrixXd::Zero(5, 5);
  Sigma.diagonal() << 1., 1., 1., 1., 1.;
  BEL belief{0, mu, Sigma};

  CTRV::ProcessModel pm;
  CTRV::ProcessNoiseCovarianceMatrix mtx;
  mtx << 1.0, 0,
          0, 1.0;
  pm.SetProcessNoiseCovarianceMatrix(mtx);

  CTRV::ControlVector control_vector{CTRV::ControlVector::Zero()};

  Lidar::MeasurementModel<CTRV::ProcessModel> lidar_mm;
  Lidar::MeasurementCovarianceMatrix lidar_mtx;
  lidar_mtx << 1.0, 0.,
               0., 1.;
  lidar_mm.SetMeasurementCovarianceMatrix(lidar_mtx);

  int dt = 1;
  for (int step = 1; step < 100; step += dt)
  {

      BEL belief_prior{belief}; //初始化

      if (step != 1)
      {
          belief_prior = EKF::Predict(belief, control_vector, dt, pm);
      }

      theta += omega * dt;
      x += v_yaw_rate * (std::sin(theta+omega*dt) - std::sin(theta));
      y += v_yaw_rate * (-std::cos(theta+omega*dt) + std::cos(theta));
      Lidar::MeasurementVector m;
      m << x, y;
      std::cout << x << " " << y << std::endl;
      Lidar::Measurement measurement{step, m};

      BEL belief_posterior{EKF::Update(belief_prior, measurement, lidar_mm)};
      belief = belief_posterior;

      std::cout << belief.mu()(0) << " " << belief.mu()(1) << " " << belief.mu()(2)
                << " " << belief.mu()(3) << " " << belief.mu()(4) << std::endl;

  }
}