bool MeasurementModel_XY::measure(const Pose2d &pose, 
				      const Landmark2d &landmark, 
				      Measurement2d &measurement,
				      Eigen::Matrix2d *jacobian_wrt_lmk,
				      Eigen::Matrix<double, 2, 3> *jacobian_wrt_pose){
  
  Eigen::Vector3d robotPose;
  Eigen::Vector2d mean, landmarkState;
  Eigen::Matrix2d H_lmk, landmarkUncertainty, cov;
  Eigen::Matrix<double, 2, 3> H_robot;
  Eigen::Matrix3d robotUncertainty;
  double dx_I, dy_I, dx_R, dy_R, c_RI, s_RI, range, range2, bearing;

  pose.get(robotPose, robotUncertainty);
  landmark.get(landmarkState,landmarkUncertainty);

  dx_I = landmarkState(0) - robotPose(0);
  dy_I = landmarkState(1) - robotPose(1);
  c_RI = cos(robotPose(2));
  s_RI = sin(robotPose(2));

  range2 = pow(dx_I, 2) + pow(dy_I, 2) ;
  range = sqrt(range2);

  dx_R =  c_RI * dx_I + s_RI * dy_I;
  dy_R = -s_RI * dx_I + c_RI * dy_I;
  mean << dx_R, dy_R ;

  H_lmk <<  c_RI, s_RI,
           -s_RI, c_RI;

  H_robot << -c_RI, -s_RI, -dx_I*s_RI + dy_I*c_RI,
              s_RI, -c_RI, -dx_I*c_RI - dy_I*s_RI;

  cov = H_lmk * landmarkUncertainty * H_lmk.transpose() + H_robot * robotUncertainty * H_robot.transpose() + R_;
  measurement.set(mean, cov);

  if(jacobian_wrt_lmk != NULL)
    *jacobian_wrt_lmk = H_lmk;

  if(jacobian_wrt_pose != NULL)
    *jacobian_wrt_pose = H_robot;

  if(range > config.rangeLimMax_ || range < config.rangeLimMin_)
    return false;
  else
    return true;
}
void MeasurementModel_RngBrg::inverseMeasure(const Pose2d &pose, 
				       const Measurement2d &measurement, 
				       Landmark2d &landmark){
  Eigen::Vector3d poseState;
  Eigen::Vector2d measurementState, mean;
  Eigen::Matrix2d measurementUncertainty, covariance, Hinv;
 
  pose.get(poseState);
  measurement.get(measurementState);
  this->getNoise(measurementUncertainty); 
  mean << poseState(0) + measurementState(0) *cos( poseState(2) + measurementState(1) ),
          poseState(1) + measurementState(0) *sin( poseState(2) + measurementState(1) );

  Hinv << cos(poseState(2)+measurementState(1)) , -measurementState(0)*sin(poseState(2)+measurementState(1)) ,
          sin(poseState(2)+measurementState(1)) , measurementState(0)*cos(poseState(2)+measurementState(1));

  covariance = Hinv * measurementUncertainty *Hinv.transpose();
  landmark.set( mean, covariance );

}
bool MeasurementModel_RngBrg::measure(const Pose2d &pose, 
				const Landmark2d &landmark, 
				Measurement2d &measurement, 
				Eigen::Matrix2d *jacobian){

  Eigen::Vector3d robotPose;
  Eigen::Vector2d mean, landmarkState;
  Eigen::Matrix2d H, landmarkUncertainty, cov;
  double range, bearing;

  pose.get(robotPose);
  landmark.get(landmarkState,landmarkUncertainty);

  range = sqrt(  pow(landmarkState(0) - robotPose(0), 2)
		+pow(landmarkState(1) - robotPose(1), 2) );

  bearing = atan2( landmarkState(1) - robotPose(1) , landmarkState(0) - robotPose(0) ) - robotPose(2);

  while(bearing>PI) bearing-=2*PI;
  while(bearing<-PI) bearing+=2*PI;

  mean << range, bearing ;

  H <<  (landmarkState(0)-robotPose(0))/mean(0)          , (landmarkState(1)-robotPose(1))/mean(0) ,
        -(landmarkState(1)-robotPose(1))/(pow(mean(0),2)), (landmarkState(0)-robotPose(0))/pow(mean(0),2) ;
  
  cov = H * landmarkUncertainty * H.transpose() + R_;
  measurement.set(mean, cov);

  
  if(jacobian != NULL)
    *jacobian = H;

  if(range > config.rangeLimMax_ || range < config.rangeLimMin_)
    return false;
  else
    return true;
}
void MeasurementModel_XY::inverseMeasure(const Pose2d &pose, 
					 const Measurement2d &measurement, 
					 Landmark2d &landmark){
  Eigen::Vector3d poseState;
  Eigen::Vector2d measurementState, mean;
  Eigen::Matrix2d measurementUncertainty, covariance, Hinv;
 
  pose.get(poseState);
  measurement.get(measurementState);
  this->getNoise(measurementUncertainty); 

  double c_RI = cos(poseState(2));
  double s_RI = sin(poseState(2));

  mean << poseState(0) + c_RI * measurementState(0) - s_RI * measurementState(1),
          poseState(1) + s_RI * measurementState(0) + c_RI * measurementState(1);

  Hinv << c_RI, -s_RI,
          s_RI, c_RI;
  
  covariance = Hinv * measurementUncertainty * Hinv.transpose();
  landmark.set( mean, covariance );

}