filtered method
Function to filter values
Implementation
double filtered(double value) {
double kalmanGain;
double currentEstimate;
kalmanGain = errorEstimate / (errorEstimate + errorMeasure);
currentEstimate = _lastEstimate + kalmanGain * (value - _lastEstimate);
errorEstimate = (1.0 - kalmanGain) * errorEstimate +
(_lastEstimate - currentEstimate).abs() * q;
_lastEstimate = currentEstimate;
return currentEstimate;
}