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 );
}