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));
}
/**
* 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();
}
}
