1+ /*
2+ File: AccelerometerFilter.m
3+ Abstract: Implements a low and high pass filter with optional adaptive filtering.
4+ Version: 2.5
5+
6+ Disclaimer: IMPORTANT: This Apple software is supplied to you by Apple
7+ Inc. ("Apple") in consideration of your agreement to the following
8+ terms, and your use, installation, modification or redistribution of
9+ this Apple software constitutes acceptance of these terms. If you do
10+ not agree with these terms, please do not use, install, modify or
11+ redistribute this Apple software.
12+
13+ In consideration of your agreement to abide by the following terms, and
14+ subject to these terms, Apple grants you a personal, non-exclusive
15+ license, under Apple's copyrights in this original Apple software (the
16+ "Apple Software"), to use, reproduce, modify and redistribute the Apple
17+ Software, with or without modifications, in source and/or binary forms;
18+ provided that if you redistribute the Apple Software in its entirety and
19+ without modifications, you must retain this notice and the following
20+ text and disclaimers in all such redistributions of the Apple Software.
21+ Neither the name, trademarks, service marks or logos of Apple Inc. may
22+ be used to endorse or promote products derived from the Apple Software
23+ without specific prior written permission from Apple. Except as
24+ expressly stated in this notice, no other rights or licenses, express or
25+ implied, are granted by Apple herein, including but not limited to any
26+ patent rights that may be infringed by your derivative works or by other
27+ works in which the Apple Software may be incorporated.
28+
29+ The Apple Software is provided by Apple on an "AS IS" basis. APPLE
30+ MAKES NO WARRANTIES, EXPRESS OR IMPLIED, INCLUDING WITHOUT LIMITATION
31+ THE IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY AND FITNESS
32+ FOR A PARTICULAR PURPOSE, REGARDING THE APPLE SOFTWARE OR ITS USE AND
33+ OPERATION ALONE OR IN COMBINATION WITH YOUR PRODUCTS.
34+
35+ IN NO EVENT SHALL APPLE BE LIABLE FOR ANY SPECIAL, INDIRECT, INCIDENTAL
36+ OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
37+ SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
38+ INTERRUPTION) ARISING IN ANY WAY OUT OF THE USE, REPRODUCTION,
39+ MODIFICATION AND/OR DISTRIBUTION OF THE APPLE SOFTWARE, HOWEVER CAUSED
40+ AND WHETHER UNDER THEORY OF CONTRACT, TORT (INCLUDING NEGLIGENCE),
41+ STRICT LIABILITY OR OTHERWISE, EVEN IF APPLE HAS BEEN ADVISED OF THE
42+ POSSIBILITY OF SUCH DAMAGE.
43+
44+ Copyright (C) 2010 Apple Inc. All Rights Reserved.
45+
46+ */
47+
48+ #import " AccelerometerFilter.h"
49+
50+ // Implementation of the basic filter. All it does is mirror input to output.
51+
52+ @implementation AccelerometerFilter
53+
54+ @synthesize x, y, z, adaptive;
55+
56+ -(void )addAcceleration : (UIAcceleration*)accel
57+ {
58+ x = accel.x ;
59+ y = accel.y ;
60+ z = accel.z ;
61+ }
62+
63+ -(NSString *)name
64+ {
65+ return @" You should not see this"
66+ }
67+
68+ @end
69+
70+ #define kAccelerometerMinStep 0.02
71+ #define kAccelerometerNoiseAttenuation 3.0
72+
73+ double Norm (double x, double y, double z)
74+ {
75+ return sqrt (x * x + y * y + z * z);
76+ }
77+
78+ double Clamp (double v, double min, double max)
79+ {
80+ if (v > max)
81+ return max;
82+ else if (v < min)
83+ return min;
84+ else
85+ return v;
86+ }
87+
88+ // See http://en.wikipedia.org/wiki/Low-pass_filter for details low pass filtering
89+ @implementation LowpassFilter
90+
91+ -(id )initWithSampleRate : (double )rate cutoffFrequency : (double )freq
92+ {
93+ self = [super init ];
94+ if (self != nil )
95+ {
96+ double dt = 1.0 / rate;
97+ double RC = 1.0 / freq;
98+ filterConstant = dt / (dt + RC);
99+ }
100+ return self;
101+ }
102+
103+ -(void )addAcceleration : (UIAcceleration*)accel
104+ {
105+ double alpha = filterConstant;
106+
107+ if (adaptive)
108+ {
109+ double d = Clamp (fabs (Norm (x, y, z) - Norm (accel.x , accel.y , accel.z )) / kAccelerometerMinStep - 1.0 , 0.0 , 1.0 );
110+ alpha = (1.0 - d) * filterConstant / kAccelerometerNoiseAttenuation + d * filterConstant;
111+ }
112+
113+ x = accel.x * alpha + x * (1.0 - alpha);
114+ y = accel.y * alpha + y * (1.0 - alpha);
115+ z = accel.z * alpha + z * (1.0 - alpha);
116+ }
117+
118+ -(NSString *)name
119+ {
120+ return adaptive ? @" Adaptive Lowpass Filter" @" Lowpass Filter"
121+ }
122+
123+ @end
124+
125+ // See http://en.wikipedia.org/wiki/High-pass_filter for details on high pass filtering
126+ @implementation HighpassFilter
127+
128+ -(id )initWithSampleRate : (double )rate cutoffFrequency : (double )freq
129+ {
130+ self = [super init ];
131+ if (self != nil )
132+ {
133+ double dt = 1.0 / rate;
134+ double RC = 1.0 / freq;
135+ filterConstant = RC / (dt + RC);
136+ }
137+ return self;
138+ }
139+
140+ -(void )addAcceleration : (UIAcceleration*)accel
141+ {
142+ double alpha = filterConstant;
143+
144+ if (adaptive)
145+ {
146+ double d = Clamp (fabs (Norm (x, y, z) - Norm (accel.x , accel.y , accel.z )) / kAccelerometerMinStep - 1.0 , 0.0 , 1.0 );
147+ alpha = d * filterConstant / kAccelerometerNoiseAttenuation + (1.0 - d) * filterConstant;
148+ }
149+
150+ x = alpha * (x + accel.x - lastX);
151+ y = alpha * (y + accel.y - lastY);
152+ z = alpha * (z + accel.z - lastZ);
153+
154+ lastX = accel.x ;
155+ lastY = accel.y ;
156+ lastZ = accel.z ;
157+ }
158+
159+ -(NSString *)name
160+ {
161+ return adaptive ? @" Adaptive Highpass Filter" @" Highpass Filter"
162+ }
163+
164+ @end
0 commit comments