Example #1
0
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;
    }
Example #3
0
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);
};
Example #4
0
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 );
	}

    }
  }