void bug_1225() { Matrix3d m1, m2; m1.setRandom(); m1 = m1*m1.transpose(); m2 = m1.triangularView<Upper>(); SelfAdjointEigenSolver<Matrix3d> eig1(m1); SelfAdjointEigenSolver<Matrix3d> eig2(m2.selfadjointView<Upper>()); VERIFY_IS_APPROX(eig1.eigenvalues(), eig2.eigenvalues()); }
/** * 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)); } }