1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55
|
void FilterNoise::checkOutliers(const MedianAbsoluteDeviation& signalMedian,
const MedianAbsoluteDeviation& signalMad,
Eigen::Vector3d acclData,
Eigen::Vector3d gyroData)
{
signalOutliersFlags.accX = checkOutlier(acclData(thetaXIdx), signalMedian.accX, signalMad.accX);
signalOutliersFlags.accY = checkOutlier(acclData(thetaYIdx), signalMedian.accY, signalMad.accY);
signalOutliersFlags.accZ = checkOutlier(acclData(thetaZIdx), signalMedian.accZ, signalMad.accZ);
signalOutliersFlags.gyroX = checkOutlier(gyroData(thetaXIdx), signalMedian.gyroX, signalMad.gyroX);
signalOutliersFlags.gyroY = checkOutlier(gyroData(thetaYIdx), signalMedian.gyroY, signalMad.gyroY);
signalOutliersFlags.gyroZ = checkOutlier(gyroData(thetaZIdx), signalMedian.gyroZ, signalMad.gyroZ);
}
bool FilterNoise::checkOutlier(double dataValue, double signalMedian, double Mad)
{
if (abs(dataValue - signalMedian) < s_madFactor * s_sigmaFactor * Mad || Mad == s_zeroMad) {
return false;
} else {
return true;
}
}
void FilterNoise::replaceOutliers(OutliersFlags signalOutliersFlags,
DataHistoryForOutliers &dataHistory,
Eigen::Vector3d& acclData,
Eigen::Vector3d& gyroData)
{
// Replacing the Outliers with the previous non-outlier value
if (signalOutliersFlags.accX) {
acclData(thetaXIdx) = dataHistory.accX(dataHistory.accX.rows() - 2);
dataHistory.accX(dataHistory.accX.rows() - 1) = dataHistory.accX(dataHistory.accX.rows() - 2);
}
if (signalOutliersFlags.accY) {
acclData(thetaYIdx) = dataHistory.accY(dataHistory.accY.rows() - 2);
dataHistory.accY(dataHistory.accY.rows() - 1) = dataHistory.accY(dataHistory.accX.rows() - 2);
}
if (signalOutliersFlags.accZ) {
acclData(thetaZIdx) = dataHistory.accZ(dataHistory.accZ.rows() - 2);
dataHistory.accZ(dataHistory.accZ.rows() - 1) = dataHistory.accZ(dataHistory.accX.rows() - 2);
}
if (signalOutliersFlags.gyroX) {
gyroData(thetaXIdx) = dataHistory.gyroX(dataHistory.gyroX.rows() - 2);
dataHistory.gyroX(dataHistory.gyroX.rows() - 1) = dataHistory.gyroX(dataHistory.accX.rows() - 2);
}
if (signalOutliersFlags.gyroY) {
gyroData(thetaYIdx) = dataHistory.gyroY(dataHistory.gyroY.rows() - 2);
dataHistory.gyroY(dataHistory.gyroY.rows() - 1) = dataHistory.gyroY(dataHistory.accX.rows() - 2);
}
if (signalOutliersFlags.gyroZ) {
gyroData(thetaZIdx) = dataHistory.gyroZ(dataHistory.gyroZ.rows() - 2);
dataHistory.gyroZ(dataHistory.gyroZ.rows() - 1) = dataHistory.gyroZ(dataHistory.accX.rows() - 2);
}
}
| |