void
basic_ins_qkf::obs_gps_pv_report(
		const Vector3d& pos, const Vector3d& vel,
		const Vector3d& p_error, const Vector3d v_error)
{
#ifdef TIME_OPS
	timer clock;
	clock.start();
#endif
#if 1
  //std::cout<<"position"<<std::endl;
	obs_gps_p_report(pos, p_error);
  //std::cout<<"velocity"<<std::endl;
	obs_gps_v_report(vel, v_error);
#else

	// The observation model is strictly linear here, so use the linear
	// form of the kalman gain and update
	Matrix<double, 6, 12> obs_matrix;
	obs_matrix << Matrix<double, 6, 6>::Zero(), Matrix<double, 6, 6>::Identity();

	Matrix<double, 6, 1> residual;
	residual.segment<3>(0) = (pos - avg_state.position);
	residual.segment<3>(3) = vel - avg_state.velocity;

	Matrix<double, 6, 6> innovation_cov = cov.corner<6, 6>(Eigen::BottomRight);
	//innovation_cov = obs_matrix * cov * obs_matrix.transpose();
	innovation_cov.corner<3, 3>(Eigen::TopLeft) += p_error.asDiagonal();
	innovation_cov.corner<3, 3>(Eigen::BottomRight) += v_error.asDiagonal();
#if 1
	// Perform matrix inverse by LU decomposition instead of cofactor expansion.
	// K = P*transpose(H)*inverse(S)
	// K = P*transpose(transpose(transpose(H)*inverse(S)))
	// K = P*transpose(transpose(inverse(S))*H)
	// K = P*transpose(inverse(transpose(S))*H)
	// S == transpose(S)
	// K = P*transpose(inverse(S)*H)
	// obs_matrx <- inverse(S)*H
	Matrix<double, 6, 12> inv_s_h;
	innovation_cov.qr().solve(obs_matrix, &inv_s_h);
	Matrix<double, 12, 6> kalman_gain = cov * inv_s_h.transpose();
#else
	Matrix<double, 12, 6> kalman_gain = cov.block<12, 6>(0, 6) * innovation_cov.inverse();
#endif
	Quaterniond rotor = avg_state.apply_kalman_vec_update(kalman_gain * residual);
	cov.part<Eigen::SelfAdjoint>() -= kalman_gain * obs_matrix * cov;
	counter_rotate_cov(rotor);
	assert(is_real());
#endif

#ifdef TIME_OPS
	double time = clock.stop() * 1e6;
	std::cout << "obs_gps_pvt time: " << time << "\n";
#endif
}
void
basic_ins_qkf::obs_gps_v_report(const Vector3d& vel, const Vector3d& v_error)
{
	Vector3d residual = vel - avg_state.velocity;
	//std::cout << "diff_v(" <<residual(0) << ", " << residual(1) << ", " << residual(2) <<")\n";

#ifdef RANK_ONE_UPDATES
	Matrix<double, 12, 1> update = Matrix<double, 12, 1>::Zero();
	for (int i = 0; i < 3; ++i) {
		double innovation_cov_inv = 1.0/(cov(9+i, 9+i) + v_error[i]);
		Matrix<double, 12, 1> gain = cov.block<12, 1>(0, 9+i) * innovation_cov_inv;
		update += gain * (residual[i] - update[9+i]);
		cov -= gain * cov.block<1, 12>(9+i, 0);
	}
#else
	Matrix<double, 3, 3> innovation_cov = cov.block<3, 3>(9, 9);
	innovation_cov += v_error.asDiagonal();
	Matrix<double, 3, 12> kalman_gain_t;
	innovation_cov.qr().solve(cov.block<3, 12>(9, 0), &kalman_gain_t);
	cov.part<Eigen::SelfAdjoint>() -= cov.block<12, 3>(0, 9) * kalman_gain_t; // .transpose() * cov.block<3, 12>(9, 0);
	Matrix<double, 12, 1> update = kalman_gain_t.transpose() * residual;
#endif
	//std::cout << "update(" << update.segment<6>(0).transpose()*180/M_PI << "\t" << update.segment<6>(6).transpose() << ")\n";

	//update.segment<6>(0) = Matrix<double, 6, 1>::Zero(); // only for debugging
	//std::cout << "update(" << update.segment<6>(0).transpose()*180/M_PI << "\t" << update.segment<6>(6).transpose() << ")\n";
	Quaterniond rotor = avg_state.apply_kalman_vec_update(update);
	counter_rotate_cov(rotor);
	assert(is_real());
}
Matrix<double,Dynamic,3>
PatchFit::dfdql (MatrixXd &ql, Vector3d &k3)
{
  MatrixXd zhMat(ql.rows(),3);
  zhMat << MatrixXd::Zero(ql.rows(),3);
  zhMat.col(2).fill(1.0);

  return (2.0*((ql*k3.asDiagonal()) - zhMat));
}
Exemple #4
0
/**
 * 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();
    }
}