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