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"; } }