void FilterBase::processMeasurement(const Measurement &measurement)
{
if (debug_)
{
*debugStream_ << "------ FilterBase::processMeasurement ------\n";
}
// If we've had a previous reading, then go through the predict/update
// cycle. Otherwise, set our state and covariance to whatever we get
// from this measurement.
if (initialized_)
{
if (debug_)
{
*debugStream_ << "Filter is already initialized. Carrying out EKF loop...\n";
}
// Could use msg->header.stamp here, but we may never get updates from
// slower sensors
double delta = measurement.time_ - lastMeasurementTime_;
// Only want to carry out a prediction if it's
// forward in time. Otherwise, just correct.
if (delta > 0)
{
validateDelta(delta);
predict(delta);
}
correct(measurement);
}
else
{
if (debug_)
{
*debugStream_ << "First measurement. Initializing filter.\n";
}
state_ = measurement.measurement_;
initialized_ = true;
}
if (debug_)
{
*debugStream_ << "\n----- /FilterBase::processMeasurement ------\n";
}
}
void FilterBase::integrateMeasurements(double currentTime)
{
if (debug_)
{
*debugStream_ << "------ FilterBase::integrateMeasurements ------\n\n";
}
// If we have any measurements in the queue, process them
if (!measurementQueue_.empty())
{
while (!measurementQueue_.empty())
{
Measurement measurement = measurementQueue_.top();
measurementQueue_.pop();
processMeasurement(measurement);
// Update the last measurement and update time.
// The measurement time is based on the time stamp of the
// measurement, whereas the update time is based on this
// node's current ROS time. The update time is used to
// determine if we have a sensor timeout, whereas the
// measurement time is used to calculate time deltas for
// prediction and correction.
lastMeasurementTime_ = measurement.time_;
}
lastUpdateTime_ = currentTime;
}
else if (initialized_)
{
// In the event that we don't get any measurements for a long time,
// we still need to continue to estimate our state. Therefore, we
// should project the state forward here.
double lastUpdateDelta = currentTime - lastUpdateTime_;
// If we get a large delta, then continuously predict until
if(lastUpdateDelta >= sensorTimeout_)
{
double projectTime = sensorTimeout_ * std::floor(lastUpdateDelta / sensorTimeout_);
if (debug_)
{
*debugStream_ << "Sensor timeout! Last measurement was " << lastMeasurementTime_ << ", current time is " <<
currentTime << ", delta is " << lastUpdateDelta << ", " << "projection time is " << projectTime << "\n";
}
validateDelta(projectTime);
predict(projectTime);
// Update the last measurement time and last update time
lastMeasurementTime_ += projectTime;
lastUpdateTime_ += projectTime;
}
}
else
{
if (debug_)
{
*debugStream_ << "Filter not yet initialized\n";
}
}
if (debug_)
{
*debugStream_ << "\n----- /FilterBase::integrateMeasurements ------\n";
}
}
void FilterBase::processMeasurement(const Measurement &measurement)
{
if (debug_)
{
*debugStream_ << "------ FilterBase::processMeasurement ------\n";
}
double delta = 0.0;
// If we've had a previous reading, then go through the predict/update
// cycle. Otherwise, set our state and covariance to whatever we get
// from this measurement.
if (initialized_)
{
// Determine how much time has passed since our last measurement
delta = measurement.time_ - lastMeasurementTime_;
if (debug_)
{
*debugStream_ << "Filter is already initialized. Carrying out predict/correct loop...\n";
*debugStream_ << "Measurement time is " << std::setprecision(20) << measurement.time_ <<
", last measurement time is " << lastMeasurementTime_ << ", delta is " << delta << "\n";
}
// Only want to carry out a prediction if it's
// forward in time. Otherwise, just correct.
if (delta > 0)
{
validateDelta(delta);
predict(delta);
// Return this to the user
predictedState_ = state_;
}
correct(measurement);
}
else
{
if (debug_)
{
*debugStream_ << "First measurement. Initializing filter.\n";
}
// Initialize the filter, but only with the values we're using
size_t measurementLength = measurement.updateVector_.size();
for(size_t i = 0; i < measurementLength; ++i)
{
state_[i] = (measurement.updateVector_[i] ? measurement.measurement_[i] : state_[i]);
}
// Same for covariance
for(size_t i = 0; i < measurementLength; ++i)
{
for(size_t j = 0; j < measurementLength; ++j)
{
estimateErrorCovariance_(i, j) = (measurement.updateVector_[i] && measurement.updateVector_[j] ?
measurement.covariance_(i, j) :
estimateErrorCovariance_(i, j));
}
}
initialized_ = true;
}
if(delta >= 0.0)
{
// Update the last measurement and update time.
// The measurement time is based on the time stamp of the
// measurement, whereas the update time is based on this
// node's current ROS time. The update time is used to
// determine if we have a sensor timeout, whereas the
// measurement time is used to calculate time deltas for
// prediction and correction.
lastMeasurementTime_ = measurement.time_;
}
if (debug_)
{
*debugStream_ << "\n----- /FilterBase::processMeasurement ------\n";
}
}
