bool RandomAccel2D::generatePosition(oat::Position2D &position)
{
if (it_ < num_samples_) {
// Simulate one step of random, but smooth, motion
simulateMotion();
// Simulated position info
position.position_valid = true;
position.position.x = state_(0);
position.position.y = state_(2);
// We have access to the velocity info for comparison
position.velocity_valid = true;
position.velocity.x = state_(1);
position.velocity.y = state_(3);
it_++;
return false;
}
return true;
}
void RandomAccel2D::simulateMotion()
{
// Generate random acceleration
accel_vec_(0) = accel_distribution_(accel_generator_);
accel_vec_(1) = accel_distribution_(accel_generator_);
// Apply acceleration and transition matrix to the simulated position
state_ = state_transition_mat_ * state_ + input_mat_ * accel_vec_;
// Apply circular boundary (not technically correct since positive test
// condition should result in state_(0) = 2*room_.x + room_.width - state_(0),
// but takes care of endless oscillation that would result if
// |state_(0) - room_.x | > room.width.
if (state_(0) < room_.x)
state_(0) = room_.x + room_.width;
if (state_(0) > room_.x + room_.width)
state_(0) = room_.x;
if (state_(2) < room_.y)
state_(2) = room_.y + room_.height;
if (state_(2) > room_.y + room_.height)
state_(2) = room_.y;
}
FunctionResults LuaVariantImpl<Derived>::operator()(Args&&... args)
{
int t = type();
if (t == LUA_TTHREAD)
{
return this->template resume<FunctionResults>(std::forward<Args>(args)...);
}
else if (t == LUA_TFUNCTION)
{
return this->template call<FunctionResults>(std::forward<Args>(args)...);
}
else
{
except::typeMismatchError(state_(), " is not function or thread");
return FunctionResults(state_());
}
}
result_type
operator()(
Message const &_message
) const
{
return
state_(
id_,
_message
);
}
void
UrDevice::control(jointMap_t &jm) {
try {
increment(timestep_);
}
catch (...) {}
sotDEBUG (25) << "state = " << state_ << std::endl;
unsigned jointId = 0;
jointMap_t::const_iterator it;
for (it=jm.begin(); it!=jm.end(); ++it, ++jointId) {
if (jointId+6 >= state_.size() || !it->second.second)
continue;
it->second.second->commanded_effort_ = state_(jointId+6);
}
}
void Ekf::predict(const double delta)
{
if (getDebug())
{
*debugStream_ << "---------------------- Ekf::predict ----------------------\n";
*debugStream_ << "delta is " << delta << "\n";
*debugStream_ << "state is " << state_ << "\n";
}
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
double xVel = state_(StateMemberVx);
double yVel = state_(StateMemberVy);
double zVel = state_(StateMemberVz);
double xAcc = state_(StateMemberAx);
double yAcc = state_(StateMemberAy);
double zAcc = state_(StateMemberAz);
// We'll need these trig calculations a lot.
double cr = cos(roll);
double cp = cos(pitch);
double cy = cos(yaw);
double sr = sin(roll);
double sp = sin(pitch);
double sy = sin(yaw);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = delta;
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;
// Prepare the transfer function Jacobian. This function is analytically derived from the
// transfer function.
double xCoeff = 0.0;
double yCoeff = 0.0;
double zCoeff = 0.0;
double oneHalfATSquared = 0.5 * delta * delta;
yCoeff = cy * sp * cr + sy * sr;
zCoeff = -cy * sp * sr + sy * cr;
double dF0dr = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -cy * sp;
yCoeff = cy * cp * sr;
zCoeff = cy * cp * cr;
double dF0dp = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -sy * cp;
yCoeff = -sy * sp * sr - cy * cr;
zCoeff = -sy * sp * cr + cy * sr;
double dF0dy = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
yCoeff = sy * sp * cr - cy * sr;
zCoeff = -sy * sp * sr - cy * cr;
double dF1dr = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -sy * sp;
yCoeff = sy * cp * sr;
zCoeff = sy * cp * cr;
double dF1dp = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = cy * cp;
yCoeff = cy * sp * sr - sy * cr;
zCoeff = cy * sp * cr + sy * sr;
double dF1dy = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
yCoeff = cp * cr;
zCoeff = -cp * sr;
double dF2dr = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -cp;
yCoeff = -sp * sr;
zCoeff = -sp * cr;
double dF2dp = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
// Much of the transfer function Jacobian is identical to the transfer function
transferFunctionJacobian_ = transferFunction_;
transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dF0dr;
transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dF0dp;
transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dF0dy;
transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dF1dr;
transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dF1dp;
transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dF1dy;
transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dF2dr;
transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dF2dp;
if (getDebug())
{
*debugStream_ << "Transfer function is:\n";
*debugStream_ << transferFunction_ << "\n";
*debugStream_ << "Transfer function Jacobian is:\n";
*debugStream_ << transferFunctionJacobian_ << "\n";
*debugStream_ << "Process noise covariance is " << "\n";
*debugStream_ << processNoiseCovariance_ << "\n";
*debugStream_ << "Current state is:\n";
*debugStream_ << state_ << "\n";
}
// (1) Project the state forward: x = Ax (really, x = f(x))
state_ = transferFunction_ * state_;
// Handle wrapping
wrapStateAngles();
if (getDebug())
{
*debugStream_ << "Predicted state is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Current estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
}
// (2) Project the error forward: P = J * P * J' + Q
estimateErrorCovariance_ = (transferFunctionJacobian_ * estimateErrorCovariance_ * transferFunctionJacobian_.transpose())
+ (processNoiseCovariance_ * delta);
if (getDebug())
{
*debugStream_ << "Predicted estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
*debugStream_ << "\n--------------------- /Ekf::predict ----------------------\n";
}
}
void Ukf::predict(const double delta)
{
FB_DEBUG("---------------------- Ukf::predict ----------------------\n" <<
"delta is " << delta <<
"\nstate is " << state_ << "\n");
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
// We'll need these trig calculations a lot.
double cr = cos(roll);
double cp = cos(pitch);
double cy = cos(yaw);
double sr = sin(roll);
double sp = sin(pitch);
double sy = sin(yaw);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = transferFunction_(StateMemberX, StateMemberVx);
transferFunction_(StateMemberRoll, StateMemberVpitch) = transferFunction_(StateMemberX, StateMemberVy);
transferFunction_(StateMemberRoll, StateMemberVyaw) = transferFunction_(StateMemberX, StateMemberVz);
transferFunction_(StateMemberPitch, StateMemberVroll) = transferFunction_(StateMemberY, StateMemberVx);
transferFunction_(StateMemberPitch, StateMemberVpitch) = transferFunction_(StateMemberY, StateMemberVy);
transferFunction_(StateMemberPitch, StateMemberVyaw) = transferFunction_(StateMemberY, StateMemberVz);
transferFunction_(StateMemberYaw, StateMemberVroll) = transferFunction_(StateMemberZ, StateMemberVx);
transferFunction_(StateMemberYaw, StateMemberVpitch) = transferFunction_(StateMemberZ, StateMemberVy);
transferFunction_(StateMemberYaw, StateMemberVyaw) = transferFunction_(StateMemberZ, StateMemberVz);
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;
// (1) Take the square root of a small fraction of the estimateErrorCovariance_ using LL' decomposition
weightedCovarSqrt_ = ((STATE_SIZE + lambda_) * estimateErrorCovariance_).llt().matrixL();
// (2) Compute sigma points *and* pass them through the transfer function to save
// the extra loop
// First sigma point is the current state
sigmaPoints_[0] = transferFunction_ * 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] = transferFunction_ * (state_ + weightedCovarSqrt_.col(sigmaInd));
sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = transferFunction_ * (state_ - weightedCovarSqrt_.col(sigmaInd));
}
// (3) Sum the weighted sigma points to generate a new state prediction
state_.setZero();
for(size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
{
state_ += stateWeights_[sigmaInd] * sigmaPoints_[sigmaInd];
}
// (4) Now us the sigma points and the predicted state to compute a predicted covariance
estimateErrorCovariance_.setZero();
Eigen::VectorXd sigmaDiff(STATE_SIZE);
for(size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
{
sigmaDiff = (sigmaPoints_[sigmaInd] - state_);
estimateErrorCovariance_ += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
}
// (5) Not strictly in the theoretical UKF formulation, but necessary here
// to ensure that we actually incorporate the processNoiseCovariance_
estimateErrorCovariance_ += delta * processNoiseCovariance_;
// Keep the angles bounded
wrapStateAngles();
// Mark that we can keep these sigma points
uncorrected_ = true;
FB_DEBUG("Predicted state is:\n" << state_ <<
"\nPredicted estimate error covariance is:\n" << estimateErrorCovariance_ <<
"\n\n--------------------- /Ukf::predict ----------------------\n");
}
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");
}
}
node *parse (const rules_char_type *start_,
const rules_char_type * const end_, const id_type id_,
const id_type user_id_, const id_type next_dfa_,
const id_type push_dfa_, const bool pop_dfa_,
const std::size_t flags_, id_type &nl_id_, const bool seen_bol_,
const bool macro_)
{
node *root_ = 0;
state state_ (start_, end_, id_, flags_, _locale, macro_);
token *lhs_token_ = 0;
std::auto_ptr<token> rhs_token_ (new token);
char action_ = 0;
_token_stack->push (static_cast<token *>(0));
_token_stack->top () = rhs_token_.release ();
rhs_token_.reset (new token);
tokeniser::next (_token_stack->top (), state_, rhs_token_.get ());
do
{
lhs_token_ = _token_stack->top ();
action_ = lhs_token_->precedence (rhs_token_->_type);
switch (action_)
{
case '<':
case '=':
_token_stack->push (static_cast<token *>(0));
_token_stack->top () = rhs_token_.release ();
rhs_token_.reset (new token);
tokeniser::next (_token_stack->top (), state_,
rhs_token_.get ());
break;
case '>':
reduce (state_);
break;
default:
{
std::ostringstream ss_;
ss_ << "A syntax error occurred: '" <<
lhs_token_->precedence_string () <<
"' against '" << rhs_token_->precedence_string () <<
"' preceding index " << state_.index () <<
" in rule id " <<
state_._id << '.';
throw runtime_error (ss_.str ());
break;
}
}
} while (!_token_stack->empty ());
if (_tree_node_stack.empty ())
{
std::ostringstream ss_;
ss_ << "Empty rules are not allowed in rule id " <<
state_._id << '.';
throw runtime_error (ss_.str ());
}
assert (_tree_node_stack.size () == 1);
node *lhs_node_ = _tree_node_stack.top ();
_tree_node_stack.pop ();
if (macro_)
{
// Macros have no end state...
root_ = lhs_node_;
}
else
{
_node_ptr_vector->push_back (static_cast<end_node *>(0));
node *rhs_node_ = new end_node (id_, user_id_, next_dfa_,
push_dfa_, pop_dfa_);
_node_ptr_vector->back () = rhs_node_;
_node_ptr_vector->push_back (static_cast<sequence_node *>(0));
_node_ptr_vector->back () = new sequence_node
(lhs_node_, rhs_node_);
root_ = _node_ptr_vector->back ();
}
if (seen_bol_)
{
fixup_bol (root_);
}
if (state_._nl_id != static_cast<id_type>(~0))
{
nl_id_ = state_._nl_id;
}
if ((flags_ & match_zero_len) == 0)
{
const typename node::node_vector &firstpos_ = root_->firstpos();
typename node::node_vector::const_iterator iter_ =
firstpos_.begin ();
typename node::node_vector::const_iterator end_ =
firstpos_.end ();
for (; iter_ != end_; ++iter_)
{
const node *node_ = *iter_;
if (node_->end_state ())
{
std::ostringstream ss_;
ss_ << "Rules that match zero characters are not allowed "
"as this can cause an infinite loop in user code. The "
"match_zero_len flag overrides this check. Rule id " <<
state_._id << '.';
throw runtime_error (ss_.str ());
}
}
}
return root_;
}
void FilterBase::wrapStateAngles()
{
while (state_(StateMemberRoll) < -pi_)
{
state_(StateMemberRoll) += tau_;
}
while (state_(StateMemberRoll) > pi_)
{
state_(StateMemberRoll) -= tau_;
}
while (state_(StateMemberPitch) < -pi_)
{
state_(StateMemberPitch) += tau_;
}
while (state_(StateMemberPitch) > pi_)
{
state_(StateMemberPitch) -= tau_;
}
while (state_(StateMemberYaw) < -pi_)
{
state_(StateMemberYaw) += tau_;
}
while (state_(StateMemberYaw) > pi_)
{
state_(StateMemberYaw) -= tau_;
}
}
void Ekf::predict(const double delta)
{
if (getDebug())
{
*debugStream_ << "---------------------- Ekf::predict ----------------------\n";
*debugStream_ << "delta is " << delta << "\n";
*debugStream_ << "state is " << state_ << "\n";
}
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
double xVel = state_(StateMemberVx);
double yVel = state_(StateMemberVy);
double zVel = state_(StateMemberVz);
// We'll need these trig calculations a lot.
double cr = cos(roll);
double cp = cos(pitch);
double cy = cos(yaw);
double sr = sin(roll);
double sp = sin(pitch);
double sy = sin(yaw);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = delta;
// Prepare the transfer function Jacobian. This function is analytically derived from the
// transfer function.
double dF0dr = (cy * sp * cr + sy * sr) * delta * yVel + (-cy * sp * sr + sy * cr) * delta * zVel;
double dF0dp = -cy * sp * delta * xVel + cy * cp * sr * delta * yVel + cy * cp * cr * delta * zVel;
double dF0dy = -sy * cp * delta * xVel + (-sy * sp * sr - cy * cr) * delta * yVel + (-sy * sp * cr + cy * sr) * delta * zVel;
double dF1dr = (sy * sp * cr - cy * sr) * delta * yVel + (-sy * sp * sr - cy * cr) * delta * zVel;
double dF1dp = -sy * sp * delta * xVel + sy * cp * sr * delta * yVel + sy * cp * cr * delta * zVel;
double dF1dy = cy * cp * delta * xVel + (cy * sp * sr - sy * cr) * delta * yVel + (cy * sp * cr + sy * sr) * delta * zVel;
double dF2dr = cp * cr * delta * yVel - cp * sr * delta * zVel;
double dF2dp = -cp * delta * xVel - sp * sr * delta * yVel - sp * cr * delta * zVel;
// Much of the transfer function Jacobian is identical to the transfer function
transferFunctionJacobian_ = transferFunction_;
transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dF0dr;
transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dF0dp;
transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dF0dy;
transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dF1dr;
transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dF1dp;
transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dF1dy;
transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dF2dr;
transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dF2dp;
if (getDebug())
{
*debugStream_ << "Transfer function is:\n";
*debugStream_ << transferFunction_ << "\n";
*debugStream_ << "Transfer function Jacobian is:\n";
*debugStream_ << transferFunctionJacobian_ << "\n";
*debugStream_ << "Process noise covariance is " << "\n";
*debugStream_ << processNoiseCovariance_ << "\n";
*debugStream_ << "Current state is:\n";
*debugStream_ << state_ << "\n";
}
// (1) Project the state forward: x = Ax (really, x = f(x))
state_ = transferFunction_ * state_;
// Handle wrapping
wrapStateAngles();
if (getDebug())
{
*debugStream_ << "Predicted state is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Current estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
}
// (2) Project the error forward: P = J * P * J' + Q
estimateErrorCovariance_ = (transferFunctionJacobian_ * estimateErrorCovariance_ * transferFunctionJacobian_.transpose())
+ (processNoiseCovariance_ * delta);
if (getDebug())
{
*debugStream_ << "Predicted estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
*debugStream_ << "Last measurement time is:\n";
*debugStream_ << std::setprecision(20) << lastMeasurementTime_ << "\n";
*debugStream_ << "\n--------------------- /Ekf::predict ----------------------\n";
}
}
void Ekf::correct(const Measurement &measurement)
{
if (getDebug())
{
*debugStream_ << "---------------------- Ekf::correct ----------------------\n";
*debugStream_ << "Measurement is:\n";
*debugStream_ << measurement.measurement_ << "\n";
*debugStream_ << "Measurement covariance is:\n";
*debugStream_ << 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
// 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)) || std::isnan(measurement.covariance_(i,i)))
{
if (getDebug())
{
*debugStream_ << "Value at index " << i << " was nan. Excluding from update.\n";
}
}
else if (std::isinf(measurement.measurement_(i)) || std::isinf(measurement.covariance_(i,i)))
{
if (getDebug())
{
*debugStream_ << "Value at index " << i << " was inf. Excluding from update.\n";
}
}
else
{
updateIndices.push_back(i);
}
}
}
if (getDebug())
{
*debugStream_ << "Update indices are:\n";
*debugStream_ << 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)
{
if (getDebug())
{
*debugStream_ << "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-6)
{
measurementCovarianceSubset(i, i) = 1e-6;
if (getDebug())
{
*debugStream_ << "WARNING: measurement had very small error covariance for index " << 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;
}
if (getDebug())
{
*debugStream_ << "Current state subset is:\n";
*debugStream_ << stateSubset << "\n";
*debugStream_ << "Measurement subset is:\n";
*debugStream_ << measurementSubset << "\n";
*debugStream_ << "Measurement covariance subset is:\n";
*debugStream_ << measurementCovarianceSubset << "\n";
*debugStream_ << "State-to-measurement subset is:\n";
*debugStream_ << stateToMeasurementSubset << "\n";
}
// (1) Compute the Kalman gain: K = (PH') / (HPH' + R)
kalmanGainSubset = estimateErrorCovariance_ * stateToMeasurementSubset.transpose()
* (stateToMeasurementSubset * estimateErrorCovariance_ * stateToMeasurementSubset.transpose() + measurementCovarianceSubset).inverse();
// (2) Apply the gain to the difference between the state and measurement: x = x + K(z - Hx)
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)
{
if (innovationSubset(i) < -pi_)
{
innovationSubset(i) += tau_;
}
else if (innovationSubset(i) > pi_)
{
innovationSubset(i) -= tau_;
}
}
}
state_ = state_ + kalmanGainSubset * innovationSubset;
// (3) Update the estimate error covariance using the Joseph form: (I - KH)P(I - KH)' + KRK'
Eigen::MatrixXd gainResidual = identity_ - kalmanGainSubset * stateToMeasurementSubset;
estimateErrorCovariance_ = gainResidual * estimateErrorCovariance_ * gainResidual.transpose() +
kalmanGainSubset * measurementCovarianceSubset * kalmanGainSubset.transpose();
// Handle wrapping of angles
wrapStateAngles();
if (getDebug())
{
*debugStream_ << "Kalman gain subset is:\n";
*debugStream_ << kalmanGainSubset << "\n";
*debugStream_ << "Innovation:\n";
*debugStream_ << innovationSubset << "\n\n";
*debugStream_ << "Corrected full state is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Corrected full estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
*debugStream_ << "Last measurement time is:\n";
*debugStream_ << std::setprecision(20) << lastMeasurementTime_ << "\n";
*debugStream_ << "\n---------------------- /Ekf::correct ----------------------\n";
}
}
void Ekf::predict(const double delta)
{
FB_DEBUG("---------------------- Ekf::predict ----------------------\n" <<
"delta is " << delta << "\n" <<
"state is " << state_ << "\n");
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
double xVel = state_(StateMemberVx);
double yVel = state_(StateMemberVy);
double zVel = state_(StateMemberVz);
double rollVel = state_(StateMemberVroll);
double pitchVel = state_(StateMemberVpitch);
double yawVel = state_(StateMemberVyaw);
double xAcc = state_(StateMemberAx);
double yAcc = state_(StateMemberAy);
double zAcc = state_(StateMemberAz);
// We'll need these trig calculations a lot.
double sp = 0.0;
double cp = 0.0;
::sincos(pitch, &sp, &cp);
double sr = 0.0;
double cr = 0.0;
::sincos(roll, &sr, &cr);
double sy = 0.0;
double cy = 0.0;
::sincos(yaw, &sy, &cy);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = transferFunction_(StateMemberX, StateMemberVx);
transferFunction_(StateMemberRoll, StateMemberVpitch) = transferFunction_(StateMemberX, StateMemberVy);
transferFunction_(StateMemberRoll, StateMemberVyaw) = transferFunction_(StateMemberX, StateMemberVz);
transferFunction_(StateMemberPitch, StateMemberVroll) = transferFunction_(StateMemberY, StateMemberVx);
transferFunction_(StateMemberPitch, StateMemberVpitch) = transferFunction_(StateMemberY, StateMemberVy);
transferFunction_(StateMemberPitch, StateMemberVyaw) = transferFunction_(StateMemberY, StateMemberVz);
transferFunction_(StateMemberYaw, StateMemberVroll) = transferFunction_(StateMemberZ, StateMemberVx);
transferFunction_(StateMemberYaw, StateMemberVpitch) = transferFunction_(StateMemberZ, StateMemberVy);
transferFunction_(StateMemberYaw, StateMemberVyaw) = transferFunction_(StateMemberZ, StateMemberVz);
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;
// Prepare the transfer function Jacobian. This function is analytically derived from the
// transfer function.
double xCoeff = 0.0;
double yCoeff = 0.0;
double zCoeff = 0.0;
double oneHalfATSquared = 0.5 * delta * delta;
yCoeff = cy * sp * cr + sy * sr;
zCoeff = -cy * sp * sr + sy * cr;
double dFx_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFR_dR = 1 + (yCoeff * pitchVel + zCoeff * yawVel) * delta;
xCoeff = -cy * sp;
yCoeff = cy * cp * sr;
zCoeff = cy * cp * cr;
double dFx_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFR_dP = (xCoeff * rollVel + yCoeff * pitchVel + zCoeff * yawVel) * delta;
xCoeff = -sy * cp;
yCoeff = -sy * sp * sr - cy * cr;
zCoeff = -sy * sp * cr + cy * sr;
double dFx_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFR_dY = (xCoeff * rollVel + yCoeff * pitchVel + zCoeff * yawVel) * delta;
yCoeff = sy * sp * cr - cy * sr;
zCoeff = -sy * sp * sr - cy * cr;
double dFy_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFP_dR = (yCoeff * pitchVel + zCoeff * yawVel) * delta;
xCoeff = -sy * sp;
yCoeff = sy * cp * sr;
zCoeff = sy * cp * cr;
double dFy_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFP_dP = 1 + (xCoeff * rollVel + yCoeff * pitchVel + zCoeff * yawVel) * delta;
xCoeff = cy * cp;
yCoeff = cy * sp * sr - sy * cr;
zCoeff = cy * sp * cr + sy * sr;
double dFy_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFP_dY = (xCoeff * rollVel + yCoeff * pitchVel + zCoeff * yawVel) * delta;
yCoeff = cp * cr;
zCoeff = -cp * sr;
double dFz_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFY_dR = (yCoeff * pitchVel + zCoeff * yawVel) * delta;
xCoeff = -cp;
yCoeff = -sp * sr;
zCoeff = -sp * cr;
double dFz_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFY_dP = (xCoeff * rollVel + yCoeff * pitchVel + zCoeff * yawVel) * delta;
// Much of the transfer function Jacobian is identical to the transfer function
transferFunctionJacobian_ = transferFunction_;
transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dFx_dR;
transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dFx_dP;
transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dFx_dY;
transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dFy_dR;
transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dFy_dP;
transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dFy_dY;
transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dFz_dR;
transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dFz_dP;
transferFunctionJacobian_(StateMemberRoll, StateMemberRoll) = dFR_dR;
transferFunctionJacobian_(StateMemberRoll, StateMemberPitch) = dFR_dP;
transferFunctionJacobian_(StateMemberRoll, StateMemberYaw) = dFR_dY;
transferFunctionJacobian_(StateMemberPitch, StateMemberRoll) = dFP_dR;
transferFunctionJacobian_(StateMemberPitch, StateMemberPitch) = dFP_dP;
transferFunctionJacobian_(StateMemberPitch, StateMemberYaw) = dFP_dY;
transferFunctionJacobian_(StateMemberYaw, StateMemberRoll) = dFY_dR;
transferFunctionJacobian_(StateMemberYaw, StateMemberPitch) = dFY_dP;
FB_DEBUG("Transfer function is:\n" << transferFunction_ <<
"\nTransfer function Jacobian is:\n" << transferFunctionJacobian_ <<
"\nProcess noise covariance is:\n" << processNoiseCovariance_ <<
"\nCurrent state is:\n" << state_ << "\n");
// (1) Project the state forward: x = Ax (really, x = f(x))
state_ = transferFunction_ * state_;
// Handle wrapping
wrapStateAngles();
FB_DEBUG("Predicted state is:\n" << state_ <<
"\nCurrent estimate error covariance is:\n" << estimateErrorCovariance_ << "\n");
// (2) Project the error forward: P = J * P * J' + Q
estimateErrorCovariance_ = (transferFunctionJacobian_ *
estimateErrorCovariance_ *
transferFunctionJacobian_.transpose());
estimateErrorCovariance_.noalias() += (processNoiseCovariance_ * delta);
FB_DEBUG("Predicted estimate error covariance is:\n" << estimateErrorCovariance_ <<
"\n\n--------------------- /Ekf::predict ----------------------\n");
}
void FilterBase::wrapStateAngles()
{
state_(StateMemberRoll) = FilterUtilities::clampRotation(state_(StateMemberRoll));
state_(StateMemberPitch) = FilterUtilities::clampRotation(state_(StateMemberPitch));
state_(StateMemberYaw) = FilterUtilities::clampRotation(state_(StateMemberYaw));
}
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");
}
}
int type() const
{
lua_State* state = state_();
util::ScopedSavedStack save(state);
return lua_type(state, pushStackIndex_(state));
}
const char* typeName()const
{
return lua_typename(state_(), type());
}
