Commit 8ffb4e8c by Ryan Pavlik

### Update flexkalman

parent 637f1bb5
 ... ... @@ -94,7 +94,7 @@ namespace util { Eigen::Quaternion ret; ret.vec() = vecscale * vec; ret.w() = std::cos(theta); return ret.normalized(); return ret; } /*! ... ... @@ -107,14 +107,21 @@ namespace util { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); using Scalar = typename Derived::Scalar; //! @todo get better way of determining "zero vec" for this purpose. if (vec.isApproxToConstant(0, 1.e-4)) { if (vec.derived().isApproxToConstant(0, 1.e-4)) { return Eigen::Quaternion::Identity(); } // For non-zero vectors, the vector scale sinc(theta) approximately // equals 1, while the scale for w, cos(theta), is approximately 0. // So, we treat this as a "pure" quaternion and normalize it. return Eigen::Quaternion{0, vec.x(), vec.y(), vec.z()} .normalized(); // For non-zero vectors, the vector scale // sinc(theta)=sin(theta)/theta approximately equals 1, and w, // cos(theta), is approximately 1 - theta^2/2. // To ensure we're exactly normalized, we could treat the vec as the // vector portion of a quaternion and compute the other part to make // it exactly normalized: // Scalar w = std::sqrt(1 - vec.derived().squaredNorm()); // Instead we'll do the small-angle approx to really skip the sqrt, // and we'll be approximately normalized. Scalar w = 1 - vec.derived().squaredNorm() / 2.; return Eigen::Quaternion{ w, vec.derived().x(), vec.derived().y(), vec.derived().z()}; } /*! ... ... @@ -150,7 +157,8 @@ namespace util { // result (i.e., ln(qv, qw) = (phi/sin(phi)) * qv ) Scalar vecnorm = quat.vec().norm(); // "best for numerical stability" vs asin or acos // "best for numerical stability" vs asin or acos. // Approximately vecnorm near 0. Scalar phi = std::atan2(vecnorm, quat.w()); // Here is where we compute the coefficient to scale the vector part ... ...
 ... ... @@ -100,9 +100,9 @@ inline types::SquareMatrix()> predictErrorCovariance(StateType const &state, ProcessModelType &processModel, double dt) { using StateSquareMatrix = types::SquareMatrix()>; StateSquareMatrix A = processModel.getStateTransitionMatrix(state, dt); const auto A = processModel.getStateTransitionMatrix(state, dt); // FLEXKALMAN_DEBUG_OUTPUT("State transition matrix", A); StateSquareMatrix P = state.errorCovariance(); auto &&P = state.errorCovariance(); /*! * @todo Determine if the fact that Q is (at least in one case) * symmetrical implies anything else useful performance-wise here or ... ...
 ... ... @@ -158,7 +158,7 @@ namespace pose_exp_map { } void postCorrect() { Eigen::Vector3d ori(orientation(m_state)); types::Vector<3> ori = orientation(m_state); matrix_exponential_map::avoidSingularities(ori); orientation(m_state) = ori; m_cacheData.reset(ori); ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!