1 /**
2 * Copyright 2024 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #define LOG_TAG "OneEuroFilter"
18
19 #include <chrono>
20 #include <cmath>
21
22 #include <android-base/logging.h>
23 #include <input/CoordinateFilter.h>
24
25 namespace android {
26 namespace {
27
28 using namespace std::literals::chrono_literals;
29
30 const float kHertzPerGigahertz = 1E9f;
31 const float kGigahertzPerHertz = 1E-9f;
32
33 // filteredSpeed's units are position per nanosecond. beta's units are 1 / position.
cutoffFreq(float minCutoffFreq,float beta,float filteredSpeed)34 inline float cutoffFreq(float minCutoffFreq, float beta, float filteredSpeed) {
35 return kHertzPerGigahertz *
36 ((minCutoffFreq * kGigahertzPerHertz) + beta * std::abs(filteredSpeed));
37 }
38
smoothingFactor(std::chrono::nanoseconds samplingPeriod,float cutoffFreq)39 inline float smoothingFactor(std::chrono::nanoseconds samplingPeriod, float cutoffFreq) {
40 const float constant = 2.0f * M_PI * samplingPeriod.count() * (cutoffFreq * kGigahertzPerHertz);
41 return constant / (constant + 1);
42 }
43
lowPassFilter(float rawValue,float prevFilteredValue,float smoothingFactor)44 inline float lowPassFilter(float rawValue, float prevFilteredValue, float smoothingFactor) {
45 return smoothingFactor * rawValue + (1 - smoothingFactor) * prevFilteredValue;
46 }
47
48 } // namespace
49
OneEuroFilter(float minCutoffFreq,float beta,float speedCutoffFreq)50 OneEuroFilter::OneEuroFilter(float minCutoffFreq, float beta, float speedCutoffFreq)
51 : mMinCutoffFreq{minCutoffFreq}, mBeta{beta}, mSpeedCutoffFreq{speedCutoffFreq} {}
52
filter(std::chrono::nanoseconds timestamp,float rawPosition)53 float OneEuroFilter::filter(std::chrono::nanoseconds timestamp, float rawPosition) {
54 LOG_IF(FATAL, mPrevTimestamp.has_value() && (*mPrevTimestamp >= timestamp))
55 << "Timestamp must be greater than mPrevTimestamp. Timestamp: " << timestamp.count()
56 << "ns. mPrevTimestamp: " << mPrevTimestamp->count() << "ns";
57
58 const std::chrono::nanoseconds samplingPeriod =
59 (mPrevTimestamp.has_value()) ? (timestamp - *mPrevTimestamp) : 1s;
60
61 const float rawVelocity = (mPrevFilteredPosition.has_value())
62 ? ((rawPosition - *mPrevFilteredPosition) / (samplingPeriod.count()))
63 : 0.0f;
64
65 const float speedSmoothingFactor = smoothingFactor(samplingPeriod, mSpeedCutoffFreq);
66
67 const float filteredVelocity = (mPrevFilteredVelocity.has_value())
68 ? lowPassFilter(rawVelocity, *mPrevFilteredVelocity, speedSmoothingFactor)
69 : rawVelocity;
70
71 const float positionCutoffFreq = cutoffFreq(mMinCutoffFreq, mBeta, filteredVelocity);
72
73 const float positionSmoothingFactor = smoothingFactor(samplingPeriod, positionCutoffFreq);
74
75 const float filteredPosition = (mPrevFilteredPosition.has_value())
76 ? lowPassFilter(rawPosition, *mPrevFilteredPosition, positionSmoothingFactor)
77 : rawPosition;
78
79 mPrevTimestamp = timestamp;
80 mPrevRawPosition = rawPosition;
81 mPrevFilteredVelocity = filteredVelocity;
82 mPrevFilteredPosition = filteredPosition;
83
84 return filteredPosition;
85 }
86
87 } // namespace android
88