void Ukf::correct(const Measurement &measurement) { FB_DEBUG("---------------------- Ukf::correct ----------------------\n" << "State is:\n" << state_ << "\nMeasurement is:\n" << measurement.measurement_ << "\nMeasurement covariance is:\n" << measurement.covariance_ << "\n"); // In our implementation, it may be that after we call predict once, we call correct // several times in succession (multiple measurements with different time stamps). In // that event, the sigma points need to be updated to reflect the current state. if(!uncorrected_) { // Take the square root of a small fraction of the estimateErrorCovariance_ using LL' decomposition weightedCovarSqrt_ = ((STATE_SIZE + lambda_) * estimateErrorCovariance_).llt().matrixL(); // Compute sigma points // First sigma point is the current state sigmaPoints_[0] = state_; // Next STATE_SIZE sigma points are state + weightedCovarSqrt_[ith column] // STATE_SIZE sigma points after that are state - weightedCovarSqrt_[ith column] for(size_t sigmaInd = 0; sigmaInd < STATE_SIZE; ++sigmaInd) { sigmaPoints_[sigmaInd + 1] = state_ + weightedCovarSqrt_.col(sigmaInd); sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = state_ - weightedCovarSqrt_.col(sigmaInd); } } // We don't want to update everything, so we need to build matrices that only update // the measured parts of our state vector // 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_SIZE); // H Eigen::MatrixXd kalmanGainSubset(STATE_SIZE, updateSize); // K Eigen::VectorXd innovationSubset(updateSize); // z - Hx Eigen::VectorXd predictedMeasurement(updateSize); Eigen::VectorXd sigmaDiff(updateSize); Eigen::MatrixXd predictedMeasCovar(updateSize, updateSize); Eigen::MatrixXd crossCovar(STATE_SIZE, updateSize); std::vector<Eigen::VectorXd> sigmaPointMeasurements(sigmaPoints_.size(), Eigen::VectorXd(updateSize)); stateSubset.setZero(); measurementSubset.setZero(); measurementCovarianceSubset.setZero(); stateToMeasurementSubset.setZero(); kalmanGainSubset.setZero(); innovationSubset.setZero(); predictedMeasurement.setZero(); predictedMeasCovar.setZero(); crossCovar.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) { 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"); } } // 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) Generate sigma points, use them to generate a predicted measurement for(size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd) { sigmaPointMeasurements[sigmaInd] = stateToMeasurementSubset * sigmaPoints_[sigmaInd]; predictedMeasurement += stateWeights_[sigmaInd] * sigmaPointMeasurements[sigmaInd]; } // (2) Use the sigma point measurements and predicted measurement to compute a predicted // measurement covariance matrix P_zz and a state/measurement cross-covariance matrix P_xz. for(size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd) { sigmaDiff = sigmaPointMeasurements[sigmaInd] - predictedMeasurement; predictedMeasCovar += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose()); crossCovar += covarWeights_[sigmaInd] * ((sigmaPoints_[sigmaInd] - state_) * sigmaDiff.transpose()); } // (3) Compute the Kalman gain, making sure to use the actual measurement covariance: K = P_xz * (P_zz + R)^-1 Eigen::MatrixXd invInnovCov = (predictedMeasCovar + measurementCovarianceSubset).inverse(); kalmanGainSubset = crossCovar * invInnovCov; // (4) Apply the gain to the difference between the actual and predicted measurements: x = x + K(z - z_hat) innovationSubset = (measurementSubset - predictedMeasurement); // (5) Check Mahalanobis distance of innovation if (checkMahalanobisThreshold(innovationSubset, invInnovCov, measurement.mahalanobisThresh_)) { // 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; } } } state_ = state_ + kalmanGainSubset * innovationSubset; // (6) Compute the new estimate error covariance P = P - (K * P_zz * K') estimateErrorCovariance_ = estimateErrorCovariance_.eval() - (kalmanGainSubset * predictedMeasCovar * kalmanGainSubset.transpose()); wrapStateAngles(); // Mark that we need to re-compute sigma points for successive corrections uncorrected_ = false; FB_DEBUG("Predicated measurement covariance is:\n" << predictedMeasCovar << "\nCross covariance is:\n" << crossCovar << "\nKalman gain subset is:\n" << kalmanGainSubset << "\nInnovation:\n" << innovationSubset << "\nCorrected full state is:\n" << state_ << "\nCorrected full estimate error covariance is:\n" << estimateErrorCovariance_ << "\n\n---------------------- /Ukf::correct ----------------------\n"); } }
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"); } }