template<typename MatrixType> void selfadjointeigensolver(const MatrixType& m) { typedef typename MatrixType::Index Index; /* this test covers the following files: EigenSolver.h, SelfAdjointEigenSolver.h (and indirectly: Tridiagonalization.h) */ Index rows = m.rows(); Index cols = m.cols(); typedef typename MatrixType::Scalar Scalar; typedef typename NumTraits<Scalar>::Real RealScalar; RealScalar largerEps = 10*test_precision<RealScalar>(); MatrixType a = MatrixType::Random(rows,cols); MatrixType a1 = MatrixType::Random(rows,cols); MatrixType symmA = a.adjoint() * a + a1.adjoint() * a1; MatrixType symmC = symmA; svd_fill_random(symmA,Symmetric); symmA.template triangularView<StrictlyUpper>().setZero(); symmC.template triangularView<StrictlyUpper>().setZero(); MatrixType b = MatrixType::Random(rows,cols); MatrixType b1 = MatrixType::Random(rows,cols); MatrixType symmB = b.adjoint() * b + b1.adjoint() * b1; symmB.template triangularView<StrictlyUpper>().setZero(); CALL_SUBTEST( selfadjointeigensolver_essential_check(symmA) ); SelfAdjointEigenSolver<MatrixType> eiSymm(symmA); // generalized eigen pb GeneralizedSelfAdjointEigenSolver<MatrixType> eiSymmGen(symmC, symmB); SelfAdjointEigenSolver<MatrixType> eiSymmNoEivecs(symmA, false); VERIFY_IS_EQUAL(eiSymmNoEivecs.info(), Success); VERIFY_IS_APPROX(eiSymm.eigenvalues(), eiSymmNoEivecs.eigenvalues()); // generalized eigen problem Ax = lBx eiSymmGen.compute(symmC, symmB,Ax_lBx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); VERIFY((symmC.template selfadjointView<Lower>() * eiSymmGen.eigenvectors()).isApprox( symmB.template selfadjointView<Lower>() * (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); // generalized eigen problem BAx = lx eiSymmGen.compute(symmC, symmB,BAx_lx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); VERIFY((symmB.template selfadjointView<Lower>() * (symmC.template selfadjointView<Lower>() * eiSymmGen.eigenvectors())).isApprox( (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); // generalized eigen problem ABx = lx eiSymmGen.compute(symmC, symmB,ABx_lx); VERIFY_IS_EQUAL(eiSymmGen.info(), Success); VERIFY((symmC.template selfadjointView<Lower>() * (symmB.template selfadjointView<Lower>() * eiSymmGen.eigenvectors())).isApprox( (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps)); eiSymm.compute(symmC); MatrixType sqrtSymmA = eiSymm.operatorSqrt(); VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView<Lower>()), sqrtSymmA*sqrtSymmA); VERIFY_IS_APPROX(sqrtSymmA, symmC.template selfadjointView<Lower>()*eiSymm.operatorInverseSqrt()); MatrixType id = MatrixType::Identity(rows, cols); VERIFY_IS_APPROX(id.template selfadjointView<Lower>().operatorNorm(), RealScalar(1)); SelfAdjointEigenSolver<MatrixType> eiSymmUninitialized; VERIFY_RAISES_ASSERT(eiSymmUninitialized.info()); VERIFY_RAISES_ASSERT(eiSymmUninitialized.eigenvalues()); VERIFY_RAISES_ASSERT(eiSymmUninitialized.eigenvectors()); VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorSqrt()); VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorInverseSqrt()); eiSymmUninitialized.compute(symmA, false); VERIFY_RAISES_ASSERT(eiSymmUninitialized.eigenvectors()); VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorSqrt()); VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorInverseSqrt()); // test Tridiagonalization's methods Tridiagonalization<MatrixType> tridiag(symmC); VERIFY_IS_APPROX(tridiag.diagonal(), tridiag.matrixT().diagonal()); VERIFY_IS_APPROX(tridiag.subDiagonal(), tridiag.matrixT().template diagonal<-1>()); Matrix<RealScalar,Dynamic,Dynamic> T = tridiag.matrixT(); if(rows>1 && cols>1) { // FIXME check that upper and lower part are 0: //VERIFY(T.topRightCorner(rows-2, cols-2).template triangularView<Upper>().isZero()); } VERIFY_IS_APPROX(tridiag.diagonal(), T.diagonal()); VERIFY_IS_APPROX(tridiag.subDiagonal(), T.template diagonal<1>()); VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView<Lower>()), tridiag.matrixQ() * tridiag.matrixT().eval() * MatrixType(tridiag.matrixQ()).adjoint()); VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView<Lower>()), tridiag.matrixQ() * tridiag.matrixT() * tridiag.matrixQ().adjoint()); // Test computation of eigenvalues from tridiagonal matrix if(rows > 1) { SelfAdjointEigenSolver<MatrixType> eiSymmTridiag; eiSymmTridiag.computeFromTridiagonal(tridiag.matrixT().diagonal(), tridiag.matrixT().diagonal(-1), ComputeEigenvectors); VERIFY_IS_APPROX(eiSymm.eigenvalues(), eiSymmTridiag.eigenvalues()); VERIFY_IS_APPROX(tridiag.matrixT(), eiSymmTridiag.eigenvectors().real() * eiSymmTridiag.eigenvalues().asDiagonal() * eiSymmTridiag.eigenvectors().real().transpose()); } if (rows > 1 && rows < 20) { // Test matrix with NaN symmC(0,0) = std::numeric_limits<typename MatrixType::RealScalar>::quiet_NaN(); SelfAdjointEigenSolver<MatrixType> eiSymmNaN(symmC); VERIFY_IS_EQUAL(eiSymmNaN.info(), NoConvergence); } // regression test for bug 1098 { SelfAdjointEigenSolver<MatrixType> eig(a.adjoint() * a); eig.compute(a.adjoint() * a); } // regression test for bug 478 { a.setZero(); SelfAdjointEigenSolver<MatrixType> ei3(a); VERIFY_IS_EQUAL(ei3.info(), Success); VERIFY_IS_MUCH_SMALLER_THAN(ei3.eigenvalues().norm(),RealScalar(1)); VERIFY((ei3.eigenvectors().transpose()*ei3.eigenvectors().transpose()).eval().isIdentity()); } }
//Simulate the price process: void Sim_Price_Disc::simulate(int retries) { //If we are re-simulating, we need to reset: if (simulated) { //X.simulate(retries); //Re-simulate X X.simulate2(); //Clear Y Y.clear(); Y.push_back(y_0); //Reset the SBM Z: Z.reset(); } else { simulated = true; //Simulate volatility if necessary: if (X.is_simulated() == false) { //X.simulate(retries); X.simulate2(); } } //Get the timestep vector from the volatility: vector<double> timestep = X.get_timestep(); double h_t, drift; matrix vola(n,n), tmp(n,n); //For each discretisation step: for (unsigned int i=1; i<timestep.size(); ++i) { //Compute stepsize of the Brownian motion (in case this has changed): h_t = timestep[i] - timestep[i-1]; //Even though we are not interested in eigenvalues anymore, we need the decomposition // for the matrix square root. matrix V_x(n,n), D_x(n,n); eig(V_x, D_x, X.get_X_i(i-1)); //Compute next simulation point using the discretisation formula (see Section 5) matrix V_r(n,n), D_r(n,n); eig(V_r, D_r, (eye(n) - R * flens::transpose(R))); drift = (r - 0.5*trace(X.get_X_i(i-1))) * h_t; mat4mult(vola, Z.increment(h_t), V_r, matsqrt_diag(D_r), flens::transpose(V_r)); tmp = X.get_W_incr_i(i) * flens::transpose(R) + vola; mat4mult(vola, V_x, matsqrt_diag(D_x), flens::transpose(V_x), tmp); //Add simulation point to storage: Y.push_back(Y[i-1] + drift + trace(vola)); } }
void bug_1204() { SparseMatrix<double> A(2,2); A.setIdentity(); SelfAdjointEigenSolver<Eigen::SparseMatrix<double> > eig(A); }
GURLS_EXPORT void eig(const gMat2D<float>& A, gMat2D<float>& V, gVec<float>& W){ gVec<float> tmp(W.getSize()); tmp = 0; eig(A, V, W, tmp); }
GURLS_EXPORT void eig(const gMat2D<float>& A, gVec<float>& W) { gVec<float> tmp = W; eig(A, W, tmp); }
// ----------------------------------------------------------------------------- // // ----------------------------------------------------------------------------- void CalculateTriangleGroupCurvatures::operator()() const { // Get the Triangles Array // DREAM3D::SurfaceMesh::FaceList_t::Pointer trianglesPtr = m_SurfaceMeshDataContainer->getFaces(); // DREAM3D::SurfaceMesh::Face_t* triangles = trianglesPtr->GetPointer(0); IDataArray::Pointer flPtr = m_SurfaceMeshDataContainer->getFaceData(DREAM3D::FaceData::SurfaceMeshFaceLabels); DataArray<int32_t>* faceLabelsPtr = DataArray<int32_t>::SafePointerDownCast(flPtr.get()); int32_t* faceLabels = faceLabelsPtr->GetPointer(0); // Instantiate a FindNRingNeighbors class to use during the loop FindNRingNeighbors::Pointer nRingNeighborAlg = FindNRingNeighbors::New(); // Make Sure we have triangle centroids calculated IDataArray::Pointer centroidPtr = m_SurfaceMeshDataContainer->getFaceData(DREAM3D::FaceData::SurfaceMeshFaceCentroids); if (NULL == centroidPtr.get()) { std::cout << "Triangle Centroids are required for this algorithm" << std::endl; return; } DataArray<double>* centroids = DataArray<double>::SafePointerDownCast(centroidPtr.get()); // Make sure we have triangle normals calculated IDataArray::Pointer normalPtr = m_SurfaceMeshDataContainer->getFaceData(DREAM3D::FaceData::SurfaceMeshFaceNormals); if (NULL == normalPtr.get()) { std::cout << "Triangle Normals are required for this algorithm" << std::endl; return; } DataArray<double>* normals = DataArray<double>::SafePointerDownCast(normalPtr.get()); int32_t* fl = faceLabels + m_TriangleIds[0] * 2; int grain0 = 0; int grain1 = 0; if (fl[0] < fl[1]) { grain0 = fl[0]; grain1 = fl[1]; } else { grain0 = fl[1]; grain1 = fl[0]; } bool computeGaussian = (m_GaussianCurvature.get() != NULL); bool computeMean = (m_MeanCurvature.get() != NULL); bool computeDirection = (m_PrincipleDirection1.get() != NULL); std::stringstream ss; std::vector<int>::size_type tCount = m_TriangleIds.size(); // For each triangle in the group for(std::vector<int>::size_type i = 0; i < tCount; ++i) { if (m_ParentFilter->getCancel() == true) { return; } int triId = m_TriangleIds[i]; nRingNeighborAlg->setTriangleId(triId); nRingNeighborAlg->setRegionId0(grain0); nRingNeighborAlg->setRegionId1(grain1); nRingNeighborAlg->setRing(m_NRing); nRingNeighborAlg->setSurfaceMeshDataContainer(m_SurfaceMeshDataContainer); nRingNeighborAlg->generate(); DREAM3D::SurfaceMesh::UniqueFaceIds_t triPatch = nRingNeighborAlg->getNRingTriangles(); BOOST_ASSERT(triPatch.size() > 1); DataArray<double>::Pointer patchCentroids = extractPatchData(triId, triPatch, centroids->GetPointer(0), std::string("Patch_Centroids")); DataArray<double>::Pointer patchNormals = extractPatchData(triId, triPatch, normals->GetPointer(0), std::string("Patch_Normals")); // Translate the patch to the 0,0,0 origin double sub[3] = {patchCentroids->GetComponent(0,0),patchCentroids->GetComponent(0,1), patchCentroids->GetComponent(0,2)}; subtractVector3d(patchCentroids, sub); double np[3] = {patchNormals->GetComponent(0,0), patchNormals->GetComponent(0,1), patchNormals->GetComponent(0, 2) }; double seedCentroid[3] = {patchCentroids->GetComponent(0,0), patchCentroids->GetComponent(0,1), patchCentroids->GetComponent(0,2) }; double firstCentroid[3] = {patchCentroids->GetComponent(1,0), patchCentroids->GetComponent(1,1), patchCentroids->GetComponent(1,2) }; double temp[3] = {firstCentroid[0] - seedCentroid[0], firstCentroid[1] - seedCentroid[1], firstCentroid[2] - seedCentroid[2]}; double vp[3] = {0.0, 0.0, 0.0}; // Cross Product of np and temp MatrixMath::NormalizeVector(np); MatrixMath::CrossProduct(np, temp, vp); MatrixMath::NormalizeVector(vp); // get the third orthogonal vector double up[3] = {0.0, 0.0, 0.0}; MatrixMath::CrossProduct(vp, np, up); // this constitutes a rotation matrix to a local coordinate system double rot[3][3] = {{up[0], up[1], up[2]}, {vp[0], vp[1], vp[2]}, {np[0], np[1], np[2]} }; double out[3]; // Transform all centroids and normals to new coordinate system for(size_t m = 0; m < patchCentroids->GetNumberOfTuples(); ++m) { ::memcpy(out, patchCentroids->GetPointer(m*3), 3*sizeof(double)); MatrixMath::Multiply3x3with3x1(rot, patchCentroids->GetPointer(m*3), out); ::memcpy(patchCentroids->GetPointer(m*3), out, 3*sizeof(double)); ::memcpy(out, patchNormals->GetPointer(m*3), 3*sizeof(double)); MatrixMath::Multiply3x3with3x1(rot, patchNormals->GetPointer(m*3), out); ::memcpy(patchNormals->GetPointer(m*3), out, 3*sizeof(double)); // We rotate the normals now but we dont use them yet. If we start using part 3 of Goldfeathers paper then we // will need the normals. } { // Solve the Least Squares fit static const unsigned int NO_NORMALS = 3; static const unsigned int USE_NORMALS = 7; int cols = NO_NORMALS; if (m_UseNormalsForCurveFitting == true) { cols = USE_NORMALS; } int rows = patchCentroids->GetNumberOfTuples(); Eigen::MatrixXd A(rows, cols); Eigen::VectorXd b(rows); double x, y, z; for(int m = 0; m < rows; ++m) { x = patchCentroids->GetComponent(m, 0); y = patchCentroids->GetComponent(m, 1); z = patchCentroids->GetComponent(m, 2); A(m) = 0.5 * x * x; // 1/2 x^2 A(m + rows) = x * y; // x*y A(m + rows*2) = 0.5 * y * y; // 1/2 y^2 if (m_UseNormalsForCurveFitting == true) { A(m + rows*3) = x*x*x; A(m + rows*4) = x*x*y; A(m + rows*5) = x*y*y; A(m + rows*6) = y*y*y; } b[m] = z; // The Z Values } Eigen::Matrix2d M; if (false == m_UseNormalsForCurveFitting) { typedef Eigen::Matrix<double, NO_NORMALS, 1> Vector3d; Vector3d sln1 = A.colPivHouseholderQr().solve(b); // Now that we have the A, B, C constants we can solve the Eigen value/vector problem // to get the principal curvatures and pricipal directions. M << sln1(0), sln1(1), sln1(1), sln1(2); } else { typedef Eigen::Matrix<double, USE_NORMALS, 1> Vector7d; Vector7d sln1 = A.colPivHouseholderQr().solve(b); // Now that we have the A, B, C, D, E, F & G constants we can solve the Eigen value/vector problem // to get the principal curvatures and pricipal directions. M << sln1(0), sln1(1), sln1(1), sln1(2); } Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eig(M); Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d>::RealVectorType eValues = eig.eigenvalues(); Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d>::MatrixType eVectors = eig.eigenvectors(); // Kappa1 >= Kappa2 double kappa1 = eValues(0) * -1;// Kappa 1 double kappa2 = eValues(1) * -1; //kappa 2 BOOST_ASSERT(kappa1 >= kappa2); m_PrincipleCurvature1->SetValue(triId, kappa1); m_PrincipleCurvature2->SetValue(triId, kappa2); if (computeGaussian == true) { m_GaussianCurvature->SetValue(triId, kappa1*kappa2); } if (computeMean == true) { m_MeanCurvature->SetValue(triId, (kappa1+kappa2)/2.0); } if (computeDirection == true) { Eigen::Matrix3d e_rot_T; e_rot_T.row(0) = Eigen::Vector3d(up[0], vp[0], np[0]); e_rot_T.row(1) = Eigen::Vector3d(up[1], vp[1], np[1]); e_rot_T.row(2) = Eigen::Vector3d(up[2], vp[2], np[2]); // Rotate our principal directions back into the original coordinate system Eigen::Vector3d dir1 ( eVectors.col(0)(0), eVectors.col(0)(1), 0.0 ); dir1 = e_rot_T * dir1; ::memcpy(m_PrincipleDirection1->GetPointer(triId * 3), dir1.data(), 3*sizeof(double) ); Eigen::Vector3d dir2 ( eVectors.col(1)(0), eVectors.col(1)(1), 0.0 ); dir2 = e_rot_T * dir2; ::memcpy(m_PrincipleDirection2->GetPointer(triId * 3), dir2.data(), 3*sizeof(double) ); } } } // End Loop over this triangle m_ParentFilter->tbbTaskProgress(); }
/** * Compute the scale factor and bias associated with a vector observer * according to the equation: * B_k = (I_{3x3} + D)^{-1} \times (O^T A_k H_k + b + \epsilon_k) * where k is the sample index, * B_k is the kth measurement * I_{3x3} is a 3x3 identity matrix * D is a 3x3 symmetric matrix of scale factors * O is the orthogonal alignment matrix * A_k is the attitude at the kth sample * b is the bias in the global reference frame * \epsilon_k is the noise at the kth sample * * After computing the scale factor and bias matrices, the optimal estimate is * given by * \tilde{B}_k = (I_{3x3} + D)B_k - b * * This implementation makes the assumption that \epsilon is a constant white, * gaussian noise source that is common to all k. The misalignment matrix O * is not computed. * * @param bias[out] The computed bias, in the global frame * @param scale[out] The computed scale factor matrix, in the sensor frame. * @param samples[in] An array of measurement samples. * @param n_samples The number of samples in the array. * @param referenceField The magnetic field vector at this location. * @param noise The one-sigma squared standard deviation of the observed noise * in the sensor. */ void twostep_bias_scale(Vector3f & bias, Matrix3f & scale, const Vector3f samples[], const size_t n_samples, const Vector3f & referenceField, const float noise) { // Define L_k by eq 51 for k = 1 .. n_samples Matrix<double, Dynamic, 9> fullSamples(n_samples, 9); // \hbar{L} by eq 52, simplified by observing that the common noise term // makes this a simple average. Matrix<double, 1, 9> centerSample = Matrix<double, 1, 9>::Zero(); // Define the sample differences z_k by eq 23 a) double sampleDeltaMag[n_samples]; // The center value \hbar{z} double sampleDeltaMagCenter = 0; // The squared norm of the reference vector double refSquaredNorm = referenceField.squaredNorm(); for (size_t i = 0; i < n_samples; ++i) { fullSamples.row(i) << 2 * samples[i].transpose().cast<double>(), -(samples[i][0] * samples[i][0]), -(samples[i][1] * samples[i][1]), -(samples[i][2] * samples[i][2]), -2 * (samples[i][0] * samples[i][1]), -2 * (samples[i][0] * samples[i][2]), -2 * (samples[i][1] * samples[i][2]); centerSample += fullSamples.row(i); sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; sampleDeltaMagCenter += sampleDeltaMag[i]; } sampleDeltaMagCenter /= n_samples; centerSample /= n_samples; // Define \tilde{L}_k for k = 0 .. n_samples Matrix<double, Dynamic, 9> centeredSamples(n_samples, 9); // Define \tilde{z}_k for k = 0 .. n_samples double centeredMags[n_samples]; // Compute the term under the summation of eq 57a Matrix<double, 9, 1> estimateSummation = Matrix<double, 9, 1>::Zero(); for (size_t i = 0; i < n_samples; ++i) { centeredSamples.row(i) = fullSamples.row(i) - centerSample; centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); } estimateSummation /= noise; // By eq 57b Matrix<double, 9, 9> P_theta_theta_inv = (1.0f / noise) * centeredSamples.transpose() * centeredSamples; #ifdef PRINTF_DEBUGGING SelfAdjointEigenSolver<Matrix<double, 9, 9> > eig(P_theta_theta_inv); std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() << "\n"; std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; #endif // The current value of the estimate. Initialized to \tilde{\theta}^* Matrix<double, 9, 1> estimate; // By eq 57a P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradient(theta) // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} size_t count = 0; double eta = 10000; while (count++ < 200 && eta > 1e-8) { static bool warned = false; if (hasNaN(estimate)) { std::cout << "WARNING: found NaN at time " << count << "\n"; warned = true; } #if 0 SelfAdjointEigenSolver<Matrix3d> eig_E(E_theta(estimate)); Vector3d S = eig_E.eigenvalues(); Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), -1 + sqrt(1 + S.coeff(1)), -1 + sqrt(1 + S.coeff(2)); scale = (eig_E.eigenvectors() * W.asDiagonal() * eig_E.eigenvectors().transpose()).cast<float>(); (Matrix3f::Identity() + scale).ldlt().solve( estimate.start<3>().cast<float>(), &bias); std::cout << "\n\nestimated bias: " << bias.transpose() << "\nestimated scale:\n" << scale; #endif Matrix<double, 1, 9> db_dtheta = dnormb_dtheta(estimate); Matrix<double, 9, 1> dJ_dtheta = ::dJ_dtheta(centerSample, sampleDeltaMagCenter, estimate, db_dtheta, -3 * noise, noise / n_samples); // Eq 59, with reused storage on db_dtheta db_dtheta = centerSample - db_dtheta; Matrix<double, 9, 9> scale = P_theta_theta_inv + (double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta; // eq 14b, mutatis mutandis (grumble, grumble) Matrix<double, 9, 1> increment; scale.ldlt().solve(dJ_dtheta, &increment); estimate -= increment; eta = increment.dot(scale * increment); std::cout << "eta: " << eta << "\n"; } std::cout << "terminated at eta = " << eta << " after " << count << " iterations\n"; if (!isnan(eta) && !isinf(eta)) { // Transform the estimated parameters from [c | E] back into [b | D]. // See eq 63-65 SelfAdjointEigenSolver<Matrix3d> eig_E(E_theta(estimate)); Vector3d S = eig_E.eigenvalues(); Vector3d W; W << -1 + sqrt(1 + S.coeff(0)), -1 + sqrt(1 + S.coeff(1)), -1 + sqrt(1 + S.coeff(2)); scale = (eig_E.eigenvectors() * W.asDiagonal() * eig_E.eigenvectors().transpose()).cast<float>(); (Matrix3f::Identity() + scale).ldlt().solve( estimate.start<3>().cast<float>(), &bias); } else { // return nonsense data. The eigensolver can fall ingo // an infinite loop otherwise. // TODO: Add error code return scale = Matrix3f::Ones() * std::numeric_limits<float>::quiet_NaN(); bias = Vector3f::Zero(); } }
/** * Compute the scale factor and bias associated with a vector observer * according to the equation: * B_k = (I_{3x3} + D)^{-1} \times (O^T A_k H_k + b + \epsilon_k) * where k is the sample index, * B_k is the kth measurement * I_{3x3} is a 3x3 identity matrix * D is a 3x3 diagonal matrix of scale factors * O is the orthogonal alignment matrix * A_k is the attitude at the kth sample * b is the bias in the global reference frame * \epsilon_k is the noise at the kth sample * This implementation makes the assumption that \epsilon is a constant white, * gaussian noise source that is common to all k. The misalignment matrix O * is not computed, and the off-diagonal elements of D, corresponding to the * misalignment scale factors are not either. * * @param bias[out] The computed bias, in the global frame * @param scale[out] The computed scale factor, in the sensor frame * @param samples[in] An array of measurement samples. * @param n_samples The number of samples in the array. * @param referenceField The magnetic field vector at this location. * @param noise The one-sigma squared standard deviation of the observed noise * in the sensor. */ void twostep_bias_scale(Vector3f & bias, Vector3f & scale, const Vector3f samples[], const size_t n_samples, const Vector3f & referenceField, const float noise) { // Initial estimate for gradiant descent starts at eq 37a of ref 2. // Define L_k by eq 30 and 28 for k = 1 .. n_samples Matrix<double, Dynamic, 6> fullSamples(n_samples, 6); // \hbar{L} by eq 33, simplified by obesrving that the Matrix<double, 1, 6> centerSample = Matrix<double, 1, 6>::Zero(); // Define the sample differences z_k by eq 23 a) double sampleDeltaMag[n_samples]; // The center value \hbar{z} double sampleDeltaMagCenter = 0; double refSquaredNorm = referenceField.squaredNorm(); for (size_t i = 0; i < n_samples; ++i) { fullSamples.row(i) << 2 * samples[i].transpose().cast<double>(), -(samples[i][0] * samples[i][0]), -(samples[i][1] * samples[i][1]), -(samples[i][2] * samples[i][2]); centerSample += fullSamples.row(i); sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm; sampleDeltaMagCenter += sampleDeltaMag[i]; } sampleDeltaMagCenter /= n_samples; centerSample /= n_samples; // True for all k. // double mu = -3*noise; // The center value of mu, \bar{mu} // double centerMu = mu; // The center value of mu, \tilde{mu} // double centeredMu = 0; // Define \tilde{L}_k for k = 0 .. n_samples Matrix<double, Dynamic, 6> centeredSamples(n_samples, 6); // Define \tilde{z}_k for k = 0 .. n_samples double centeredMags[n_samples]; // Compute the term under the summation of eq 37a Matrix<double, 6, 1> estimateSummation = Matrix<double, 6, 1>::Zero(); for (size_t i = 0; i < n_samples; ++i) { centeredSamples.row(i) = fullSamples.row(i) - centerSample; centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter; estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose(); } estimateSummation /= noise; // note: paper supplies 1/noise // By eq 37 b). Note, paper supplies 1/noise here Matrix<double, 6, 6> P_theta_theta_inv = (1.0f / noise) * centeredSamples.transpose() * centeredSamples; #ifdef PRINTF_DEBUGGING SelfAdjointEigenSolver<Matrix<double, 6, 6> > eig(P_theta_theta_inv); std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n"; std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose() << "\n"; std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n"; #endif // The Fisher information matrix has a small eigenvalue, with a // corresponding eigenvector of about [1e-2 1e-2 1e-2 0.55, 0.55, // 0.55]. This means that there is relatively little information // about the common gain on the scale factor, which has a // corresponding effect on the bias, too. The fixup is performed by // the first iteration of the second stage of TWOSTEP, as documented in // [1]. Matrix<double, 6, 1> estimate; // By eq 37 a), \tilde{Fisher}^-1 P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate); #ifdef PRINTF_DEBUGGING Matrix<double, 6, 6> P_theta_theta; P_theta_theta_inv.ldlt().solve(Matrix<double, 6, 6>::Identity(), &P_theta_theta); SelfAdjointEigenSolver<Matrix<double, 6, 6> > eig2(P_theta_theta); std::cout << "eigenvalues: " << eig2.eigenvalues().transpose() << "\n"; std::cout << "eigenvectors: \n" << eig2.eigenvectors() << "\n"; std::cout << "estimate summation: \n" << estimateSummation.normalized() << "\n"; #endif // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradiant(theta) // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1} size_t count = 3; while (count-- > 0) { // eq 40 Matrix<double, 1, 6> db_dtheta = dnormb_dtheta(estimate); Matrix<double, 6, 1> dJ_dtheta = dJdb(centerSample, sampleDeltaMagCenter, estimate, db_dtheta, -3 * noise, noise / n_samples); // Eq 39 (with double-negation on term inside parens) db_dtheta = centerSample - db_dtheta; Matrix<double, 6, 6> scale = P_theta_theta_inv + (double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta; // eq 14b, mutatis mutandis (grumble, grumble) Matrix<double, 6, 1> increment; scale.ldlt().solve(dJ_dtheta, &increment); estimate -= increment; } // Transform the estimated parameters from [c | e] back into [b | d]. for (size_t i = 0; i < 3; ++i) { scale.coeffRef(i) = -1 + sqrt(1 + estimate.coeff(3 + i)); bias.coeffRef(i) = estimate.coeff(i) / sqrt(1 + estimate.coeff(3 + i)); } }
Eigsym::Eigsym(const MatDoub &A, MatDoub &V, VecDoub &lambda) { eig(A,V,lambda); }
/* Function Definitions */ void magCali(const real_T data[6000], creal_T A_i[9], creal_T B[3]) { real_T D[20000]; int32_T i; int32_T i0; real_T S[100]; int32_T ind; real_T L_S22[16]; real_T L_S22_i[16]; real_T b_L_S22[16]; real_T S22_i[16]; real_T A[36]; real_T b_S[24]; real_T c_S[36]; real_T norm_v1; static const real_T b_A[36] = { 0.0, 0.5, 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25 }; creal_T L[36]; creal_T V[36]; creal_T b_max; creal_T v1[6]; real_T norm_VV2; creal_T b_S22_i[24]; creal_T v2[4]; creal_T v[100]; creal_T Q[9]; creal_T L_Q[9]; creal_T L_Q_i[9]; creal_T b_L_Q[9]; creal_T b_v[3]; creal_T QB[3]; creal_T b_Q[9]; real_T norm_VV3; creal_T b_L_Q_i[9]; real_T ar; real_T ai; /* magnetometer calibration */ /* */ /* Jungtaek Kim */ /* [email protected] */ /* */ memset(&D[0], 0, 20000U * sizeof(real_T)); for (i = 0; i < 2000; i++) { D[i] = data[i] * data[i]; D[2000 + i] = data[2000 + i] * data[2000 + i]; D[4000 + i] = data[4000 + i] * data[4000 + i]; D[6000 + i] = 2.0 * data[2000 + i] * data[4000 + i]; D[8000 + i] = 2.0 * data[i] * data[4000 + i]; D[10000 + i] = 2.0 * data[i] * data[2000 + i]; D[12000 + i] = 2.0 * data[i]; D[14000 + i] = 2.0 * data[2000 + i]; D[16000 + i] = 2.0 * data[4000 + i]; D[18000 + i] = 1.0; } for (i0 = 0; i0 < 10; i0++) { for (i = 0; i < 10; i++) { S[i0 + 10 * i] = 0.0; for (ind = 0; ind < 2000; ind++) { S[i0 + 10 * i] += D[ind + 2000 * i0] * D[ind + 2000 * i]; } } } for (i0 = 0; i0 < 4; i0++) { for (i = 0; i < 4; i++) { L_S22[i + (i0 << 2)] = S[(i + 10 * (6 + i0)) + 6]; } } chol(L_S22); inv(L_S22, L_S22_i); for (i0 = 0; i0 < 4; i0++) { for (i = 0; i < 4; i++) { b_L_S22[i + (i0 << 2)] = L_S22[i0 + (i << 2)]; } } inv(b_L_S22, L_S22); for (i0 = 0; i0 < 4; i0++) { for (i = 0; i < 4; i++) { S22_i[i0 + (i << 2)] = 0.0; for (ind = 0; ind < 4; ind++) { S22_i[i0 + (i << 2)] += L_S22[i0 + (ind << 2)] * L_S22_i[ind + (i << 2)]; } } } /* S22_i = pinv(S22);%I4\S22; */ for (i0 = 0; i0 < 6; i0++) { for (i = 0; i < 4; i++) { b_S[i0 + 6 * i] = 0.0; for (ind = 0; ind < 4; ind++) { b_S[i0 + 6 * i] += S[i0 + 10 * (6 + ind)] * S22_i[ind + (i << 2)]; } } } for (i0 = 0; i0 < 6; i0++) { for (i = 0; i < 6; i++) { norm_v1 = 0.0; for (ind = 0; ind < 4; ind++) { norm_v1 += b_S[i0 + 6 * ind] * S[(ind + 10 * i) + 6]; } c_S[i0 + 6 * i] = S[i0 + 10 * i] - norm_v1; } } for (i0 = 0; i0 < 6; i0++) { for (i = 0; i < 6; i++) { A[i0 + 6 * i] = 0.0; for (ind = 0; ind < 6; ind++) { A[i0 + 6 * i] += b_A[i0 + 6 * ind] * c_S[ind + 6 * i]; } } } eig(A, V, L); b_max = L[0]; ind = 0; for (i = 0; i < 6; i++) { if (b_max.re < L[i + 6 * i].re) { b_max = L[i + 6 * i]; ind = i; } } memcpy(&v1[0], &V[6 * ind], 6U * sizeof(creal_T)); if (V[6 * ind].re < 0.0) { for (i0 = 0; i0 < 6; i0++) { v1[i0].re = -V[i0 + 6 * ind].re; v1[i0].im = -V[i0 + 6 * ind].im; } } norm_v1 = norm(v1); for (i0 = 0; i0 < 6; i0++) { norm_VV2 = v1[i0].im; if (v1[i0].im == 0.0) { v1[i0].re /= norm_v1; v1[i0].im = 0.0; } else if (v1[i0].re == 0.0) { v1[i0].re = 0.0; v1[i0].im = norm_VV2 / norm_v1; } else { v1[i0].re /= norm_v1; v1[i0].im = norm_VV2 / norm_v1; } } for (i0 = 0; i0 < 4; i0++) { for (i = 0; i < 6; i++) { norm_v1 = 0.0; for (ind = 0; ind < 4; ind++) { norm_v1 += S22_i[i0 + (ind << 2)] * S[(ind + 10 * i) + 6]; } b_S22_i[i0 + (i << 2)].re = norm_v1; b_S22_i[i0 + (i << 2)].im = 0.0; } } for (i0 = 0; i0 < 4; i0++) { v2[i0].re = 0.0; v2[i0].im = 0.0; for (i = 0; i < 6; i++) { v2[i0].re += b_S22_i[i0 + (i << 2)].re * v1[i].re - 0.0 * v1[i].im; v2[i0].im += b_S22_i[i0 + (i << 2)].re * v1[i].im + 0.0 * v1[i].re; } } for (i0 = 0; i0 < 100; i0++) { v[i0].re = 0.0; v[i0].im = 0.0; } v[0] = v1[0]; v[1] = v1[1]; v[2] = v1[2]; v[3] = v1[3]; v[4] = v1[4]; v[5] = v1[5]; v[6].re = -v2[0].re; v[6].im = -v2[0].im; v[7].re = -v2[1].re; v[7].im = -v2[1].im; v[8].re = -v2[2].re; v[8].im = -v2[2].im; v[9].re = -v2[3].re; v[9].im = -v2[3].im; Q[0] = v[0]; Q[3] = v[5]; Q[6] = v[4]; Q[1] = v[5]; Q[4] = v[1]; Q[7] = v[3]; Q[2] = v[4]; Q[5] = v[3]; Q[8] = v[2]; memcpy(&L_Q[0], &Q[0], 9U * sizeof(creal_T)); b_chol(L_Q); b_inv(L_Q, L_Q_i); for (i0 = 0; i0 < 3; i0++) { for (i = 0; i < 3; i++) { b_L_Q[i + 3 * i0].re = L_Q[i0 + 3 * i].re; b_L_Q[i + 3 * i0].im = -L_Q[i0 + 3 * i].im; } } b_inv(b_L_Q, L_Q); /* Q_i = pinv(Q);%I3\Q; */ for (i0 = 0; i0 < 3; i0++) { for (i = 0; i < 3; i++) { b_L_Q[i0 + 3 * i].re = 0.0; b_L_Q[i0 + 3 * i].im = 0.0; for (ind = 0; ind < 3; ind++) { b_L_Q[i0 + 3 * i].re += L_Q[i0 + 3 * ind].re * L_Q_i[ind + 3 * i].re - L_Q[i0 + 3 * ind].im * L_Q_i[ind + 3 * i].im; b_L_Q[i0 + 3 * i].im += L_Q[i0 + 3 * ind].re * L_Q_i[ind + 3 * i].im + L_Q[i0 + 3 * ind].im * L_Q_i[ind + 3 * i].re; } } } b_v[0] = v[6]; b_v[1] = v[7]; b_v[2] = v[8]; for (i0 = 0; i0 < 3; i0++) { norm_v1 = 0.0; norm_VV2 = 0.0; for (i = 0; i < 3; i++) { norm_v1 += b_L_Q[i0 + 3 * i].re * b_v[i].re - b_L_Q[i0 + 3 * i].im * b_v[i] .im; norm_VV2 += b_L_Q[i0 + 3 * i].re * b_v[i].im + b_L_Q[i0 + 3 * i].im * b_v[i].re; } B[i0].re = -norm_v1; B[i0].im = -norm_VV2; } for (i0 = 0; i0 < 3; i0++) { QB[i0].re = 0.0; QB[i0].im = 0.0; for (i = 0; i < 3; i++) { QB[i0].re += Q[i0 + 3 * i].re * B[i].re - Q[i0 + 3 * i].im * B[i].im; QB[i0].im += Q[i0 + 3 * i].re * B[i].im + Q[i0 + 3 * i].im * B[i].re; } b_v[i0].re = B[i0].re; b_v[i0].im = -B[i0].im; } b_max.re = 0.0; b_max.im = 0.0; for (i = 0; i < 3; i++) { b_max.re += b_v[i].re * QB[i].re - b_v[i].im * QB[i].im; b_max.im += b_v[i].re * QB[i].im + b_v[i].im * QB[i].re; } b_max.re -= v[9].re; b_max.im -= v[9].im; b_sqrt(&b_max); memcpy(&b_Q[0], &Q[0], 9U * sizeof(creal_T)); b_eig(b_Q, L_Q, Q); norm_v1 = b_norm(*(creal_T (*)[3])&L_Q[0]); norm_VV2 = b_norm(*(creal_T (*)[3])&L_Q[3]); norm_VV3 = b_norm(*(creal_T (*)[3])&L_Q[6]); for (i0 = 0; i0 < 3; i0++) { if (L_Q[i0].im == 0.0) { L_Q_i[i0].re = L_Q[i0].re / norm_v1; L_Q_i[i0].im = 0.0; } else if (L_Q[i0].re == 0.0) { L_Q_i[i0].re = 0.0; L_Q_i[i0].im = L_Q[i0].im / norm_v1; } else { L_Q_i[i0].re = L_Q[i0].re / norm_v1; L_Q_i[i0].im = L_Q[i0].im / norm_v1; } if (L_Q[3 + i0].im == 0.0) { L_Q_i[3 + i0].re = L_Q[3 + i0].re / norm_VV2; L_Q_i[3 + i0].im = 0.0; } else if (L_Q[3 + i0].re == 0.0) { L_Q_i[3 + i0].re = 0.0; L_Q_i[3 + i0].im = L_Q[3 + i0].im / norm_VV2; } else { L_Q_i[3 + i0].re = L_Q[3 + i0].re / norm_VV2; L_Q_i[3 + i0].im = L_Q[3 + i0].im / norm_VV2; } if (L_Q[6 + i0].im == 0.0) { L_Q_i[6 + i0].re = L_Q[6 + i0].re / norm_VV3; L_Q_i[6 + i0].im = 0.0; } else if (L_Q[6 + i0].re == 0.0) { L_Q_i[6 + i0].re = 0.0; L_Q_i[6 + i0].im = L_Q[6 + i0].im / norm_VV3; } else { L_Q_i[6 + i0].re = L_Q[6 + i0].re / norm_VV3; L_Q_i[6 + i0].im = L_Q[6 + i0].im / norm_VV3; } for (i = 0; i < 3; i++) { b_L_Q[i0 + 3 * i].re = 0.0; b_L_Q[i0 + 3 * i].im = 0.0; for (ind = 0; ind < 3; ind++) { b_L_Q[i0 + 3 * i].re += L_Q_i[i0 + 3 * ind].re * Q[ind + 3 * i].re - L_Q_i[i0 + 3 * ind].im * Q[ind + 3 * i].im; b_L_Q[i0 + 3 * i].im += L_Q_i[i0 + 3 * ind].re * Q[ind + 3 * i].im + L_Q_i[i0 + 3 * ind].im * Q[ind + 3 * i].re; } } } for (i0 = 0; i0 < 3; i0++) { for (i = 0; i < 3; i++) { b_L_Q_i[i0 + 3 * i].re = 0.0; b_L_Q_i[i0 + 3 * i].im = 0.0; for (ind = 0; ind < 3; ind++) { b_L_Q_i[i0 + 3 * i].re += b_L_Q[i0 + 3 * ind].re * L_Q_i[i + 3 * ind].re - b_L_Q[i0 + 3 * ind].im * -L_Q_i[i + 3 * ind].im; b_L_Q_i[i0 + 3 * i].im += b_L_Q[i0 + 3 * ind].re * -L_Q_i[i + 3 * ind]. im + b_L_Q[i0 + 3 * ind].im * L_Q_i[i + 3 * ind].re; } } } for (i0 = 0; i0 < 3; i0++) { for (i = 0; i < 3; i++) { ar = b_L_Q_i[i + 3 * i0].re * 0.569; ai = b_L_Q_i[i + 3 * i0].im * 0.569; if (b_max.im == 0.0) { if (ai == 0.0) { A_i[i + 3 * i0].re = ar / b_max.re; A_i[i + 3 * i0].im = 0.0; } else if (ar == 0.0) { A_i[i + 3 * i0].re = 0.0; A_i[i + 3 * i0].im = ai / b_max.re; } else { A_i[i + 3 * i0].re = ar / b_max.re; A_i[i + 3 * i0].im = ai / b_max.re; } } else if (b_max.re == 0.0) { if (ar == 0.0) { A_i[i + 3 * i0].re = ai / b_max.im; A_i[i + 3 * i0].im = 0.0; } else if (ai == 0.0) { A_i[i + 3 * i0].re = 0.0; A_i[i + 3 * i0].im = -(ar / b_max.im); } else { A_i[i + 3 * i0].re = ai / b_max.im; A_i[i + 3 * i0].im = -(ar / b_max.im); } } else { norm_VV3 = fabs(b_max.re); norm_v1 = fabs(b_max.im); if (norm_VV3 > norm_v1) { norm_v1 = b_max.im / b_max.re; norm_VV2 = b_max.re + norm_v1 * b_max.im; A_i[i + 3 * i0].re = (ar + norm_v1 * ai) / norm_VV2; A_i[i + 3 * i0].im = (ai - norm_v1 * ar) / norm_VV2; } else if (norm_v1 == norm_VV3) { norm_v1 = b_max.re > 0.0 ? 0.5 : -0.5; norm_VV2 = b_max.im > 0.0 ? 0.5 : -0.5; A_i[i + 3 * i0].re = (ar * norm_v1 + ai * norm_VV2) / norm_VV3; A_i[i + 3 * i0].im = (ai * norm_v1 - ar * norm_VV2) / norm_VV3; } else { norm_v1 = b_max.re / b_max.im; norm_VV2 = b_max.im + norm_v1 * b_max.re; A_i[i + 3 * i0].re = (norm_v1 * ar + ai) / norm_VV2; A_i[i + 3 * i0].im = (norm_v1 * ai - ar) / norm_VV2; } } } } }