void projectorFromSvd(const Eigen::MatrixXd& jac,
Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
Eigen::VectorXd& svdSingular,
Eigen::MatrixXd& preResult,
Eigen::MatrixXd& result,
double epsilon=std::numeric_limits<double>::epsilon(),
double minTol=1e-8)
{
// we are force to compute the Full matrix because of
// the nullspace matrix computation
svd.compute(jac, Eigen::ComputeFullU | Eigen::ComputeFullV);
double tolerance =
epsilon*double(std::max(jac.cols(), jac.rows()))*
std::abs(svd.singularValues()[0]);
tolerance = std::max(tolerance, minTol);
svdSingular.setOnes();
for(int i = 0; i < svd.singularValues().rows(); ++i)
{
svdSingular[i] = svd.singularValues()[i] > tolerance ? 0. : 1.;
}
preResult.noalias() = svd.matrixV()*svdSingular.asDiagonal();
result.noalias() = preResult*svd.matrixV().adjoint();
}
void projectorDummy(const Eigen::MatrixXd& pseudoInv,
const Eigen::MatrixXd& jac,
Eigen::MatrixXd& result)
{
result.setIdentity();
result.noalias() -= pseudoInv*jac;
}
void pseudoInverse(const Eigen::MatrixXd& jac,
Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
Eigen::VectorXd& svdSingular,
Eigen::MatrixXd& prePseudoInv,
Eigen::MatrixXd& result,
double epsilon=std::numeric_limits<double>::epsilon(),
double minTol=1e-8)
{
svd.compute(jac, Eigen::ComputeThinU | Eigen::ComputeThinV);
// singular values are sorted in decreasing order
// so the first one is the max one
double tolerance =
epsilon*double(std::max(jac.cols(), jac.rows()))*
std::abs(svd.singularValues()[0]);
tolerance = std::max(tolerance, minTol);
svdSingular = ((svd.singularValues().array().abs() > tolerance).
select(svd.singularValues().array().inverse(), 0.));
prePseudoInv.noalias() = svd.matrixV()*svdSingular.asDiagonal();
result.noalias() = prePseudoInv*svd.matrixU().adjoint();
}
void Ekf::correct(const Measurement &measurement)
{
FB_DEBUG("---------------------- Ekf::correct ----------------------\n" <<
"State is:\n" << state_ << "\n"
"Topic is:\n" << measurement.topicName_ << "\n"
"Measurement is:\n" << measurement.measurement_ << "\n"
"Measurement topic name is:\n" << measurement.topicName_ << "\n\n"
"Measurement covariance is:\n" << measurement.covariance_ << "\n");
// We don't want to update everything, so we need to build matrices that only update
// the measured parts of our state vector. Throughout prediction and correction, we
// attempt to maximize efficiency in Eigen.
// First, determine how many state vector values we're updating
std::vector<size_t> updateIndices;
for (size_t i = 0; i < measurement.updateVector_.size(); ++i)
{
if (measurement.updateVector_[i])
{
// Handle nan and inf values in measurements
if (std::isnan(measurement.measurement_(i)))
{
FB_DEBUG("Value at index " << i << " was nan. Excluding from update.\n");
}
else if (std::isinf(measurement.measurement_(i)))
{
FB_DEBUG("Value at index " << i << " was inf. Excluding from update.\n");
}
else
{
updateIndices.push_back(i);
}
}
}
FB_DEBUG("Update indices are:\n" << updateIndices << "\n");
size_t updateSize = updateIndices.size();
// Now set up the relevant matrices
Eigen::VectorXd stateSubset(updateSize); // x (in most literature)
Eigen::VectorXd measurementSubset(updateSize); // z
Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); // R
Eigen::MatrixXd stateToMeasurementSubset(updateSize, state_.rows()); // H
Eigen::MatrixXd kalmanGainSubset(state_.rows(), updateSize); // K
Eigen::VectorXd innovationSubset(updateSize); // z - Hx
stateSubset.setZero();
measurementSubset.setZero();
measurementCovarianceSubset.setZero();
stateToMeasurementSubset.setZero();
kalmanGainSubset.setZero();
innovationSubset.setZero();
// Now build the sub-matrices from the full-sized matrices
for (size_t i = 0; i < updateSize; ++i)
{
measurementSubset(i) = measurement.measurement_(updateIndices[i]);
stateSubset(i) = state_(updateIndices[i]);
for (size_t j = 0; j < updateSize; ++j)
{
measurementCovarianceSubset(i, j) = measurement.covariance_(updateIndices[i], updateIndices[j]);
}
// Handle negative (read: bad) covariances in the measurement. Rather
// than exclude the measurement or make up a covariance, just take
// the absolute value.
if (measurementCovarianceSubset(i, i) < 0)
{
FB_DEBUG("WARNING: Negative covariance for index " << i <<
" of measurement (value is" << measurementCovarianceSubset(i, i) <<
"). Using absolute value...\n");
measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i));
}
// If the measurement variance for a given variable is very
// near 0 (as in e-50 or so) and the variance for that
// variable in the covariance matrix is also near zero, then
// the Kalman gain computation will blow up. Really, no
// measurement can be completely without error, so add a small
// amount in that case.
if (measurementCovarianceSubset(i, i) < 1e-9)
{
FB_DEBUG("WARNING: measurement had very small error covariance for index " << updateIndices[i] <<
". Adding some noise to maintain filter stability.\n");
measurementCovarianceSubset(i, i) = 1e-9;
}
}
// The state-to-measurement function, h, will now be a measurement_size x full_state_size
// matrix, with ones in the (i, i) locations of the values to be updated
for (size_t i = 0; i < updateSize; ++i)
{
stateToMeasurementSubset(i, updateIndices[i]) = 1;
}
FB_DEBUG("Current state subset is:\n" << stateSubset <<
"\nMeasurement subset is:\n" << measurementSubset <<
"\nMeasurement covariance subset is:\n" << measurementCovarianceSubset <<
"\nState-to-measurement subset is:\n" << stateToMeasurementSubset << "\n");
// (1) Compute the Kalman gain: K = (PH') / (HPH' + R)
Eigen::MatrixXd pht = estimateErrorCovariance_ * stateToMeasurementSubset.transpose();
Eigen::MatrixXd hphrInv = (stateToMeasurementSubset * pht + measurementCovarianceSubset).inverse();
kalmanGainSubset.noalias() = pht * hphrInv;
innovationSubset = (measurementSubset - stateSubset);
// Wrap angles in the innovation
for (size_t i = 0; i < updateSize; ++i)
{
if (updateIndices[i] == StateMemberRoll ||
updateIndices[i] == StateMemberPitch ||
updateIndices[i] == StateMemberYaw)
{
while (innovationSubset(i) < -PI)
{
innovationSubset(i) += TAU;
}
while (innovationSubset(i) > PI)
{
innovationSubset(i) -= TAU;
}
}
}
// (2) Check Mahalanobis distance between mapped measurement and state.
if (checkMahalanobisThreshold(innovationSubset, hphrInv, measurement.mahalanobisThresh_))
{
// (3) Apply the gain to the difference between the state and measurement: x = x + K(z - Hx)
state_.noalias() += kalmanGainSubset * innovationSubset;
// (4) Update the estimate error covariance using the Joseph form: (I - KH)P(I - KH)' + KRK'
Eigen::MatrixXd gainResidual = identity_;
gainResidual.noalias() -= kalmanGainSubset * stateToMeasurementSubset;
estimateErrorCovariance_ = gainResidual * estimateErrorCovariance_ * gainResidual.transpose();
estimateErrorCovariance_.noalias() += kalmanGainSubset *
measurementCovarianceSubset *
kalmanGainSubset.transpose();
// Handle wrapping of angles
wrapStateAngles();
FB_DEBUG("Kalman gain subset is:\n" << kalmanGainSubset <<
"\nInnovation is:\n" << innovationSubset <<
"\nCorrected full state is:\n" << state_ <<
"\nCorrected full estimate error covariance is:\n" << estimateErrorCovariance_ <<
"\n\n---------------------- /Ekf::correct ----------------------\n");
}
}
