bool approxEqual(const MatrixWrapper::ColumnVector& a, const MatrixWrapper::ColumnVector& b, double epsilon)
{
if (a.rows() != b.rows()) return false;
for (unsigned int r=0; r<a.rows(); r++)
if (!approxEqual(a(r+1), b(r+1),epsilon)) return false;
return true;
}
bool Quaternion_Wrapper::getEulerAngles(std::string axes, MatrixWrapper::ColumnVector& output) {
bool ret = false;
// Normalize quaternion
MyQuaternion& quat = (*(MyQuaternion*)this);
quat.normalize();
if (!(output.rows() == 3))
ret = false;
else {
if (!axes.compare("xyz")) {
double r = quat(1);
double x = quat(2);
double y = quat(3);
double z = quat(4);
output(1) = atan2(2*y*z + 2*x*r, 1 - 2*pow(x,2) - 2*pow(y,2));
output(2) = asin(-2*x*z + 2*y*r);
output(3) = atan2(2*x*y + 2*z*r, 1 - 2*pow(y,2) - 2*pow(z,2));
// NOTE Reference http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
// output(1) = atan2(-2*x*y - 2*z*r, 1 - 2*pow(y,2) - 2*pow(z,2));
// output(2) = asin(-2*x*z + 2*y*r);
// output(3) = atan2(-2*z*y + 2*x*r, 1 - 2*pow(x,2) - 2*pow(y,2));
ret = true;
} else
std::cout << "[quaternion_wrapper.cpp] Computation for axis order " << axes << " has not been implemented yet" << std::endl;
}
return ret;
}
void DetectorFilter::composeTransform(const MatrixWrapper::ColumnVector& vector,
geometry_msgs::PoseWithCovarianceStamped& pose)
{
assert(vector.rows() == 6);
// construct kdl rotation from vector (x, y, z, Rx, Ry, Rz), and extract quaternion
KDL::Rotation rot = KDL::Rotation::RPY(vector(4), vector(5), vector(6));
rot.GetQuaternion(pose.pose.pose.orientation.x, pose.pose.pose.orientation.y,
pose.pose.pose.orientation.z, pose.pose.pose.orientation.w);
pose.pose.pose.position.x = vector(1);
pose.pose.pose.position.y = vector(2);
pose.pose.pose.position.z = vector(3);
};
void DetectorFilter::decomposeTransform(const geometry_msgs::PoseWithCovarianceStamped& pose,
MatrixWrapper::ColumnVector& vector)
{
assert(vector.rows() == 6);
// construct kdl rotation from quaternion, and extract RPY
KDL::Rotation rot = KDL::Rotation::Quaternion(pose.pose.pose.orientation.x,
pose.pose.pose.orientation.y,
pose.pose.pose.orientation.z,
pose.pose.pose.orientation.w);
rot.GetRPY(vector(4), vector(5), vector(6));
vector(1) = pose.pose.pose.position.x;
vector(2) = pose.pose.pose.position.y;
vector(3) = pose.pose.pose.position.z;
};
void BeaconKFNode::beaconCallback( geometry_msgs::PoseWithCovarianceStampedConstPtr msg )
{
tf::StampedTransform T_beacon_to_camera;
tf::poseMsgToTF( msg->pose.pose, T_beacon_to_camera);
ros::Time beacon_stamp = msg->header.stamp;
T_beacon_to_camera.stamp_ = beacon_stamp;
_camera_frame_id = msg->header.frame_id;
//measured map->odom transform
tf::Transform T_measured_mto;
try {
_tf.waitForTransform("base_link", _camera_frame_id,
beacon_stamp, ros::Duration(1.0));
tf::StampedTransform T_camera_to_base;
_tf.lookupTransform("base_link", _camera_frame_id, beacon_stamp, T_camera_to_base);
_tf.waitForTransform(_odometry_frame, "base_link",
beacon_stamp, ros::Duration(1.0));
tf::StampedTransform T_base_to_odom;
_tf.lookupTransform(_odometry_frame, "base_link", beacon_stamp, T_base_to_odom);
//this is in the reverse of the normal xform direction, for the error xform calc
//this is a static xform from beacon_finder, timestamp isn't important
tf::StampedTransform T_map_to_beacon;
_tf.lookupTransform("beacon", _world_fixed_frame, ros::Time(0), T_map_to_beacon);
//This calculates the correction thusly:
//odom->base->camera->measured-beacon * beacon->map
T_measured_mto = T_base_to_odom*T_camera_to_base*T_beacon_to_camera*T_map_to_beacon;
T_measured_mto = T_measured_mto.inverse();
//broadcast this T_map_to odom from real map
tf::StampedTransform test_map_to_odom(T_measured_mto,
msg->header.stamp,
_world_fixed_frame,
"beacon_localizer_T");
_tf_broadcast.sendTransform( test_map_to_odom );
}
catch (tf::TransformException &ex) {
ROS_ERROR("BEACON_LOCALIZER transform lookup failure: %s", ex.what());
return;
}
MatrixWrapper::ColumnVector measurement(3);
measurement(1) = T_measured_mto.getOrigin()[0];
measurement(2) = T_measured_mto.getOrigin()[1];
measurement(3) = tf::getYaw( T_measured_mto.getRotation() );
ROS_DEBUG_STREAM("BEACON LOCALIZER measurement: " << measurement.transpose() );
MatrixWrapper::ColumnVector state =_filter->PostGet()->ExpectedValueGet();
MatrixWrapper::ColumnVector err = (state-measurement);
double distance = sqrt(err.transpose()*err);
MatrixWrapper::SymmetricMatrix cov(6);
cov(1,1) = msg->pose.covariance[0]; cov(1,2) = msg->pose.covariance[1]; cov(1,3) = msg->pose.covariance[2]; cov(1,4) = msg->pose.covariance[3]; cov(1,5) = msg->pose.covariance[4]; cov(1,6) = msg->pose.covariance[5];
cov(2,1) = msg->pose.covariance[6]; cov(2,2) = msg->pose.covariance[7]; cov(2,3) = msg->pose.covariance[8]; cov(2,4) = msg->pose.covariance[9]; cov(2,5) = msg->pose.covariance[10];cov(2,6) = msg->pose.covariance[11];
cov(3,1) = msg->pose.covariance[12];cov(3,2) = msg->pose.covariance[13];cov(3,3) = msg->pose.covariance[14];cov(3,4) = msg->pose.covariance[15];cov(3,5) = msg->pose.covariance[16];cov(3,6) = msg->pose.covariance[17];
cov(4,1) = msg->pose.covariance[18];cov(4,2) = msg->pose.covariance[19];cov(4,3) = msg->pose.covariance[20];cov(4,4) = msg->pose.covariance[21];cov(4,5) = msg->pose.covariance[22];cov(4,6) = msg->pose.covariance[23];
cov(5,1) = msg->pose.covariance[24];cov(5,2) = msg->pose.covariance[25];cov(5,3) = msg->pose.covariance[26];cov(5,4) = msg->pose.covariance[27];cov(5,5) = msg->pose.covariance[28];cov(5,6) = msg->pose.covariance[29];
cov(6,1) = msg->pose.covariance[30];cov(6,2) = msg->pose.covariance[31];cov(6,3) = msg->pose.covariance[32];cov(6,4) = msg->pose.covariance[33];cov(6,5) = msg->pose.covariance[34];cov(6,6) = msg->pose.covariance[35];
/*
tf::Matrix3x3 pointDevMat(
sqrt(cov(1,1)), 0, 0,
0.0, sqrt(cov(2,2)), 0,
0.0, 0.0, sqrt(cov(3,3)));
ROS_INFO("Point deviation");
ROS_INFO("[ %f, %f, %f ]", pointDevMat[0][0], pointDevMat[0][1], pointDevMat[0][2]);
ROS_INFO("[ %f, %f, %f ]", pointDevMat[1][0], pointDevMat[1][1], pointDevMat[1][2]);
ROS_INFO("[ %f, %f, %f ]", pointDevMat[2][0], pointDevMat[2][1], pointDevMat[2][2]);
tf::Matrix3x3 rpointDev = _T_odom.getBasis()*T_camera_to_base.getBasis()*pointDevMat*T_map_to_beacon.getBasis();
tf::Matrix3x3 rpointCov = rpointDev*rpointDev;
ROS_INFO("rPoint covariance");
ROS_INFO("[ %f, %f, %f ]", rpointCov[0][0], rpointCov[0][1], rpointCov[0][2]);
ROS_INFO("[ %f, %f, %f ]", rpointCov[1][0], rpointCov[1][1], rpointCov[1][2]);
ROS_INFO("[ %f, %f, %f ]", rpointCov[2][0], rpointCov[2][1], rpointCov[2][2]);
*/
MatrixWrapper::SymmetricMatrix measNoiseCovariance(3);
//measNoiseCovariance(1,1) = rpointCov[0][0]; measNoiseCovariance(1,2) = rpointCov[0][1]; measNoiseCovariance(1,3) = 0;
//measNoiseCovariance(2,1) = rpointCov[1][0]; measNoiseCovariance(2,2) = rpointCov[1][1]; measNoiseCovariance(2,3) = 0;
//measNoiseCovariance(3,1) = 0; measNoiseCovariance(3,2) = 0; measNoiseCovariance(3,3) = cov(5,5);
measNoiseCovariance(1,1) = cov(3,3); measNoiseCovariance(1,2) = 0.0; measNoiseCovariance(1,3) = 0.0;
measNoiseCovariance(2,1) = 0.0; measNoiseCovariance(2,2) = cov(3,3); measNoiseCovariance(2,3) = 0.0;
measNoiseCovariance(3,1) = 0.0; measNoiseCovariance(3,2) = 0.0; measNoiseCovariance(3,3) = cov(5,5);
ROS_DEBUG_STREAM("BEACON LOCALIZER measurement noise covariance: " << measNoiseCovariance);
MatrixWrapper::ColumnVector measNoiseMu(3); // zeros
measNoiseMu(1) = 0.0;
measNoiseMu(2) = 0.0;
measNoiseMu(3) = 0.0;
BFL::Gaussian measUncertainty(measNoiseMu, measNoiseCovariance);
MatrixWrapper::Matrix measH(3,3);
measH(1,1) = 1.0; measH(1,2) = 0.0; measH(1,3) = 0.0;
measH(2,1) = 0.0; measH(2,2) = 1.0; measH(2,3) = 0.0;
measH(3,1) = 0.0; measH(3,2) = 0.0; measH(3,3) = 1.0;
BFL::LinearAnalyticConditionalGaussian beaconMeasurementPdf(measH, measUncertainty);
BFL::LinearAnalyticMeasurementModelGaussianUncertainty beaconMeasurementModel(&beaconMeasurementPdf);
_filter->Update(_system_model, &beaconMeasurementModel, measurement);
ROS_DEBUG_STREAM("BEACON LOCALIZER State: " << _filter->PostGet()->ExpectedValueGet().transpose() );
ROS_DEBUG_STREAM("BEACON LOCALIZER Covariance: " << _filter->PostGet()->CovarianceGet() );
}
void
SRIteratedExtendedKalmanFilter::MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,const MatrixWrapper::ColumnVector& z,const MatrixWrapper::ColumnVector& s)
{
MatrixWrapper::Matrix invS(z.rows(),z.rows());
MatrixWrapper::Matrix Sr(z.rows(),z.rows());
MatrixWrapper::Matrix K_i(_post->CovarianceGet().rows(),z.rows());
MatrixWrapper::ColumnVector x_k = _post->ExpectedValueGet();
MatrixWrapper::SymmetricMatrix P_k = _post->CovarianceGet();
MatrixWrapper::ColumnVector x_i = _post->ExpectedValueGet();
MatrixWrapper::Matrix H_i; MatrixWrapper::SymmetricMatrix R_i;
MatrixWrapper::Matrix R_vf; MatrixWrapper::Matrix SR_vf;
MatrixWrapper::ColumnVector Z_i;
MatrixWrapper::Matrix U; MatrixWrapper::ColumnVector V; MatrixWrapper::Matrix W;
MatrixWrapper::Matrix JP1; int change;
Matrix diag(JP.rows(),JP.columns());
Matrix invdiag(JP.rows(),JP.columns());
diag=0;invdiag=0;change=0;
V=0;U=0;W=0;
// matrix determining the numerical limitations of covariance matrix:
for(unsigned int j=1;j<JP.rows()+1;j++){diag(j,j)=100; invdiag(j,j)=0.01;}
for (unsigned int i=1; i<nr_iterations+1; i++)
{
x_i = _post->ExpectedValueGet();
H_i = ((LinearAnalyticMeas_Implicit*)measmodel)->df_dxGet(s,x_i);
Z_i = ((LinearAnalyticMeas_Implicit*)measmodel)->ExpectedValueGet() + ( H_i * (x_k - x_i) );
R_i = ((LinearAnalyticMeas_Implicit*)measmodel)->CovarianceGet();
SR_vf = ((LinearAnalyticMeas_Implicit*)measmodel)->SRCovariance();
// check two different types of Kalman filters:
if(((LinearAnalyticMeas_Implicit*)measmodel)->Is_Identity()==1)
{
R_vf = SR_vf.transpose();
}
else
{
R_i.cholesky_semidefinite(R_vf);
R_vf = R_vf.transpose();
}
// numerical limitations
// The singular values of the Square root covariance matrix are limited the the value of 10e-4
// because of numerical stabilisation of the Kalman filter algorithm.
JP.SVD(V,U,W);
MatrixWrapper::Matrix V_matrix(U.columns(),W.columns());
for(unsigned int k=1;k<JP.rows()+1;k++)
{
V_matrix(k,k) = V(k);
V(k)=max(V(k),1.0/(Numerical_Limitation));
if(V(k)==1/(Numerical_Limitation)){change=1;}
}
if(change==1)
{
JP = U*V_matrix*(W.transpose());
}
// end limitations
CalculateMatrix(H_i, R_i , invS , K_i , Sr );
CalculateMean(x_k, z, Z_i , K_i);
if (i==nr_iterations)
{
CalculateCovariance( R_vf, H_i, invS, Sr );
}
}
}