double MeasurementModel_RngBrg::probabilityOfDetection( const Pose2d &pose,
const Landmark2d &landmark,
bool &isCloseToSensingLimit ){
Pose2d::Vec robotPose;
Landmark2d::Vec landmarkState;
double range, Pd;
isCloseToSensingLimit = false;
pose.get(robotPose);
landmark.get(landmarkState);
range = sqrt( pow(landmarkState(0) - robotPose(0), 2)
+pow(landmarkState(1) - robotPose(1), 2) );
if( range <= config.rangeLimMax_ && range >= config.rangeLimMin_){
Pd = config.probabilityOfDetection_;
if( range >= (config.rangeLimMax_ - config.rangeLimBuffer_ ) ||
range <= (config.rangeLimMin_ + config.rangeLimBuffer_ ) )
isCloseToSensingLimit = true;
}else{
Pd = 0;
if( range <= (config.rangeLimMax_ + config.rangeLimBuffer_ ) &&
range >= (config.rangeLimMin_ - config.rangeLimBuffer_ ) )
isCloseToSensingLimit = true;
}
return Pd;
}
void OdometryMotionModel2d::step( Pose2d &s_k,
Pose2d &s_km,
Odometry2d &input_k,
const double dT ){
Pose2d::Vec x_k_i_; /* \f[ \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}_{k} \f]*/
Pose2d::Vec x_km_i_; /* \f[ \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}_{k-1} \f]*/
Eigen::Vector2d p_k_i_; /* \f[ \begin{bmatrix} x \\ y \end{bmatrix}_{k} \f]*/
Eigen::Vector2d p_km_i_; /* \f[ \begin{bmatrix} x \\ y \end{bmatrix}_{k-1} \f]*/
double theta_k_; /* \f[ \theta_k \f\] */
double theta_km_; /* \f[ \theta_k \f\] */
Eigen::Matrix2d C_k_i_; /* rotation matrix k */
Eigen::Matrix2d C_km_i_; /* rotation matrix k-1 */
Eigen::Vector3d u_k_km_; /* odometry input */
Eigen::Matrix3d Sd_k_km_; /* uncertainty of translation input */
Eigen::Vector2d dp_k_km_; /* translation input */
double dtheta_k_km_; /* rotation input */
Eigen::Matrix2d C_k_km_; /* rotation matrix from odometry input */
/* State at k-1 */
s_km.get(x_km_i_);
p_km_i_ = x_km_i_.head(2);
theta_km_ = x_km_i_(2);
double ct = cos(theta_km_);
double st = sin(theta_km_);
C_km_i_ << ct, st, -st, ct;
/* Odometry */
double t;
input_k.get(u_k_km_, t);
dp_k_km_ = u_k_km_.head(2);
dtheta_k_km_ = u_k_km_(2);
ct = cos(dtheta_k_km_);
st = sin(dtheta_k_km_);
C_k_km_ << ct, st, -st, ct;
/* Step forward */
p_k_i_ = p_km_i_ + C_km_i_.transpose() * dp_k_km_;
C_k_i_ = C_k_km_ * C_km_i_;
/* Write state at k */
theta_k_ = atan2( C_k_i_(0, 1), C_k_i_(0, 0) );
x_k_i_.head(2) = p_k_i_;
x_k_i_(2) = theta_k_;
s_k.set(x_k_i_);
}
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 );
}
