Accelerometer class with low pass filter

[AnatolParticipant]

Hi,

I think this is worth sharing. I’ve written an accelerometer class with a couple of features that I find useful:

• device rotation in radians (calculated from the raw data)
• normalised device rotation in the range -1.0 to 1.0
• low pass filtered output (optional but enabled by default to reduce noise from the raw data)
• manual or “automatic” calibration (optional)
• detection if the device provides accelerometer data (to fall back to other controls if not)
• other data: raw data, filtered G (force) data

You’ll notice a very slight delay of device rotation and the dependent rotation of a sprite when you use the accelerometer. This is due to the low pass filter and you’ll always have to compromise between smoothness (less noise) and delay, but I think the default filter values are acceptable. The rotation based on unfiltered raw data has no delay but looks very jittery.

I tested this on iOS and Android and both looked the same. iOS seems to produce a different raw value range, but the class handles this (fingers crossed this is the same across all iOS devices).

You’ll need the LowPassFilter and the Accelerometer classes below.

Low pass filter:

Monkey

Class LowPassFilter Field Value:Float Field Alpha:Float ' dt : time interval ' rc : time constant Method New ( startValue:Float = 0, dt:Float = 0.05, rc:Float = 0.3 ) Value = startValue Alpha = dt / ( rc + dt ) End Method OnUpdate:Float ( input:Float ) Value = ( Alpha * input ) + ( 1.0 - Alpha ) * Value Return Value End End

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18

Class LowPassFilter Field Value:Float Field Alpha:Float ' dt : time interval ' rc : time constant Method New ( startValue:Float = 0, dt:Float = 0.05, rc:Float = 0.3 ) Value = startValue Alpha = dt / ( rc + dt ) End Method OnUpdate:Float ( input:Float ) Value = ( Alpha * input ) + ( 1.0 - Alpha ) * Value Return Value End End

Accelerometer (with plenty of comments in case anyone wants to understand what’s going on; it took me a while to figure this out):

Monkey

#Import "../utils/low_pass_filter" Class Accelerometer ' X, Y, Z are the device rotation in radians on the respective axes, in the range -Pi to Pi. ' If set the low pass filter is applied to these values. Field X:Float Field Y:Float Field Z:Float ' RawX, RawY, RawZ is the raw and unaltered accelerometer data. No filter is applied. Field RawX:Float Field RawY:Float Field RawZ:Float ' GX, GY, GZ is the G (force) value in the range -1.0 to 1.0. ' If set the low pass filter is applied to these values. ' This value is simply a raw value multiplied by the device multiplier so that it matches the -1.0 to 1.0 range. Field GX:Float Field GY:Float Field GZ:Float ' CenterX, CenterY, CenterZ are the gravitational center offsets for calibration. ' The default of all values is 0. The possible range is -Pi to Pi. ' There are two ways to calibrate this accelerometer: ' 1. by calling Calibrate() which will set the gravitational centre to the current device rotation. ' 2. by setting CenterX , CenterY, CenterZ manually (could be useful if the app allows device orientation changes). Field CenterX:Float Field CenterY:Float Field CenterZ:Float ' Supported is True if an Accelerometer can be found for this device. Field Supported:Bool ' Creates an Accelerometer instance for easy access to the current device rotation values. ' useLowPassFilter: filters out any spikes that come from the raw data which can lead to jittery results ' dt: low pass filter time interval ' rc: low pass filter time constant ' deviceMultiplier: iOS seems to produce raw data in the range -0.2 to 0.2, but we need values in the range -1.0 to 1.0 ' Note: The low pass filter will cause a slight delay of values which is the compromise for smoother results. ' Adjust dt and rc for different results, the smoother the more delay, the closer to the raw data the more jitter. #If __TARGET__ = "ios" Method New ( useLowPassFilter:Bool = True, dt:Float = 0.05, rc:Float = 0.3, deviceMultiplier:Float = 5 ) #Else Method New ( useLowPassFilter:Bool = True, dt:Float = 0.05, rc:Float = 0.3, deviceMultiplier:Float = 1 ) #Endif _DeviceMultiplier = deviceMultiplier _Joystick = _GetAccelerometerJoystick() Supported = _Joystick <> Null If useLowPassFilter _LowPassFilterX = New LowPassFilter( 0, dt, rc ) _LowPassFilterY = New LowPassFilter( 0, dt, rc ) _LowPassFilterZ = New LowPassFilter( 0, dt, rc ) End End ' Normalises the X value to the range -1.0 to 1.0. ' This is helpful if you want to use the device rotation to control something that is not a rotation, e.g. to change the z position in space. Property NormalX:Float () Return X / Pi End Property NormalY:Float () Return Y / Pi End Property NormalZ:Float () Return Z / Pi End ' Sets the gravitational centre to the current device rotation. Method Calibrate () CalibrateX() CalibrateY() CalibrateZ() End Method CalibrateX () CenterX = ATan2( GY, GZ ) End Method CalibrateY () CenterY = ATan2( GX, GY ) End Method CalibrateZ () CenterZ = ATan2( GZ, GY ) End ' Resets the gravitational centre to the default device centre. Method ResetCalibrate () CenterX = 0 CenterY = 0 CenterZ = 0 End ' Call this in every OnRender loop. Method OnUpdate () If Not _Joystick Then Return RawX = _Joystick.GetAxis( 0 ) RawY = _Joystick.GetAxis( 1 ) RawZ = _Joystick.GetAxis( 2 ) GX = RawX * _DeviceMultiplier GY = RawY * _DeviceMultiplier GZ = RawZ * _DeviceMultiplier If _LowPassFilterX ' The following explanation is clearer in degrees, so a G value of -1 results in -180 degrees, +1 results in 180 degrees. ' For the low pass filter we need to avoid the scenario that the value jumps by about 360 degrees. ' Instead we calculate the difference of previous and current value and update the lowpass filter value accordingly. ' E.g. if on consecutive frames the value is -179 and then +179 we change the lowpass filter value to +181, ' otherwise the filter would calculate a quickly tweened 358 degree clockwise rotation ' which actually is a counter clockwise rotation of only 2 degrees. ' We assume for this case that a device rotation of more than 180 degrees (G value difference > 1) ' cannot be done within one frame, so a difference of > 180 degrees will trigger the adjustment. Local diffX := GX - _LowPassFilterX.Value Local diffY := GY - _LowPassFilterY.Value Local diffZ := GZ - _LowPassFilterZ.Value If Abs( diffX ) > 1 Then _LowPassFilterX.Value = GX + Sgn( diffX ) * ( 2 - Abs( diffX ) ) If Abs( diffY ) > 1 Then _LowPassFilterY.Value = GY + Sgn( diffY ) * ( 2 - Abs( diffY ) ) If Abs( diffZ ) > 1 Then _LowPassFilterZ.Value = GZ + Sgn( diffZ ) * ( 2 - Abs( diffZ ) ) GX = _LowPassFilterX.OnUpdate( GX ) GY = _LowPassFilterY.OnUpdate( GY ) GZ = _LowPassFilterZ.OnUpdate( GZ ) End ' Let's turn the G (force) values into radians. X = ATan2( GY, GZ ) - CenterX Y = ATan2( GX, GY ) - CenterY Z = ATan2( GZ, GY ) - CenterZ ' For calibrated values where CenterX/Y/Z is not 0 we need to ensure that X, Y and Z stay within the range -Pi to Pi. If Not ( -Pi <= X <= Pi ) Then X = -Sgn( X ) + X Mod Pi If Not ( -Pi <= Y <= Pi ) Then Y = -Sgn( Y ) + Y Mod Pi If Not ( -Pi <= Z <= Pi ) Then Z = -Sgn( Z ) + Z Mod Pi End Private Field _Joystick:JoystickDevice Field _DeviceMultiplier:Float Field _LowPassFilterX:LowPassFilter Field _LowPassFilterY:LowPassFilter Field _LowPassFilterZ:LowPassFilter ' Check if the device has an accelerometer and return the joystick or Null. Method _GetAccelerometerJoystick:JoystickDevice () For Local i := 0 Until JoystickDevice.NumJoysticks() Local joystick := JoystickDevice.Open( i ) If joystick.Name.ToLower().Contains( "accelerometer" ) Return joystick End Next Return Null End End

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 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166

#Import  "../utils/low_pass_filter"     Class Accelerometer ' X, Y, Z are the device rotation in radians on the respective axes, in the range -Pi to Pi. ' If set the low pass filter is applied to these values. Field X:Float Field Y:Float Field Z:Float ' RawX, RawY, RawZ is the raw and unaltered accelerometer data. No filter is applied. Field RawX:Float Field RawY:Float Field RawZ:Float ' GX, GY, GZ is the G (force) value in the range -1.0 to 1.0. ' If set the low pass filter is applied to these values. ' This value is simply a raw value multiplied by the device multiplier so that it matches the -1.0 to 1.0 range. Field GX:Float Field GY:Float Field GZ:Float ' CenterX, CenterY, CenterZ are the gravitational center offsets for calibration. ' The default of all values is 0. The possible range is -Pi to Pi. ' There are two ways to calibrate this accelerometer: ' 1. by calling Calibrate() which will set the gravitational centre to the current device rotation. ' 2. by setting CenterX , CenterY, CenterZ manually (could be useful if the app allows device orientation changes). Field CenterX:Float Field CenterY:Float Field CenterZ:Float ' Supported is True if an Accelerometer can be found for this device. Field Supported:Bool ' Creates an Accelerometer instance for easy access to the current device rotation values. ' useLowPassFilter: filters out any spikes that come from the raw data which can lead to jittery results ' dt: low pass filter time interval ' rc: low pass filter time constant ' deviceMultiplier: iOS seems to produce raw data in the range -0.2 to 0.2, but we need values in the range -1.0 to 1.0 ' Note: The low pass filter will cause a slight delay of values which is the compromise for smoother results. '       Adjust dt and rc for different results, the smoother the more delay, the closer to the raw data the more jitter. #If __TARGET__ = "ios" Method New ( useLowPassFilter:Bool = True, dt:Float = 0.05, rc:Float = 0.3, deviceMultiplier:Float = 5 ) #Else Method New ( useLowPassFilter:Bool = True, dt:Float = 0.05, rc:Float = 0.3, deviceMultiplier:Float = 1 ) #Endif _DeviceMultiplier = deviceMultiplier _Joystick = _GetAccelerometerJoystick() Supported = _Joystick <> Null If useLowPassFilter _LowPassFilterX = New LowPassFilter( 0, dt, rc ) _LowPassFilterY = New LowPassFilter( 0, dt, rc ) _LowPassFilterZ = New LowPassFilter( 0, dt, rc ) End End ' Normalises the X value to the range -1.0 to 1.0. ' This is helpful if you want to use the device rotation to control something that is not a rotation, e.g. to change the z position in space. Property NormalX:Float () Return X / Pi End Property NormalY:Float () Return Y / Pi End Property NormalZ:Float () Return Z / Pi End ' Sets the gravitational centre to the current device rotation. Method Calibrate () CalibrateX() CalibrateY() CalibrateZ() End Method CalibrateX () CenterX = ATan2( GY, GZ ) End Method CalibrateY () CenterY = ATan2( GX, GY ) End Method CalibrateZ () CenterZ = ATan2( GZ, GY ) End ' Resets the gravitational centre to the default device centre. Method ResetCalibrate () CenterX = 0 CenterY = 0 CenterZ = 0 End ' Call this in every OnRender loop. Method OnUpdate () If Not _Joystick Then Return RawX = _Joystick.GetAxis( 0 ) RawY = _Joystick.GetAxis( 1 ) RawZ = _Joystick.GetAxis( 2 ) GX = RawX * _DeviceMultiplier GY = RawY * _DeviceMultiplier GZ = RawZ * _DeviceMultiplier If _LowPassFilterX ' The following explanation is clearer in degrees, so a G value of -1 results in -180 degrees, +1 results in 180 degrees. ' For the low pass filter we need to avoid the scenario that the value jumps by about 360 degrees. ' Instead we calculate the difference of previous and current value and update the lowpass filter value accordingly. ' E.g. if on consecutive frames the value is -179 and then +179 we change the lowpass filter value to +181, ' otherwise the filter would calculate a quickly tweened 358 degree clockwise rotation ' which actually is a counter clockwise rotation of only 2 degrees. ' We assume for this case that a device rotation of more than 180 degrees (G value difference > 1) ' cannot be done within one frame, so a difference of > 180 degrees will trigger the adjustment. Local diffX := GX - _LowPassFilterX.Value Local diffY := GY - _LowPassFilterY.Value Local diffZ := GZ - _LowPassFilterZ.Value If Abs( diffX ) > 1 Then _LowPassFilterX.Value = GX + Sgn( diffX ) * ( 2 - Abs( diffX ) ) If Abs( diffY ) > 1 Then _LowPassFilterY.Value = GY + Sgn( diffY ) * ( 2 - Abs( diffY ) ) If Abs( diffZ ) > 1 Then _LowPassFilterZ.Value = GZ + Sgn( diffZ ) * ( 2 - Abs( diffZ ) ) GX = _LowPassFilterX.OnUpdate( GX ) GY = _LowPassFilterY.OnUpdate( GY ) GZ = _LowPassFilterZ.OnUpdate( GZ ) End ' Let's turn the G (force) values into radians. X = ATan2( GY, GZ ) - CenterX Y = ATan2( GX, GY ) - CenterY Z = ATan2( GZ, GY ) - CenterZ ' For calibrated values where CenterX/Y/Z is not 0 we need to ensure that X, Y and Z stay within the range -Pi to Pi. If Not ( -Pi <= X <= Pi ) Then X = -Sgn( X ) + X Mod Pi If Not ( -Pi <= Y <= Pi ) Then Y = -Sgn( Y ) + Y Mod Pi If Not ( -Pi <= Z <= Pi ) Then Z = -Sgn( Z ) + Z Mod Pi End Private Field _Joystick:JoystickDevice Field _DeviceMultiplier:Float Field _LowPassFilterX:LowPassFilter Field _LowPassFilterY:LowPassFilter Field _LowPassFilterZ:LowPassFilter ' Check if the device has an accelerometer and return the joystick or Null. Method _GetAccelerometerJoystick:JoystickDevice () For Local i := 0 Until JoystickDevice.NumJoysticks() Local joystick := JoystickDevice.Open( i ) If joystick.Name.ToLower().Contains( "accelerometer" ) Return joystick End Next Return Null End End

Usage:

It’s very simple, just import the accelerometer class and initialise an instance:

Monkey

#Import "accelerometer"

1	#Import "accelerometer"

Monkey

Accel = New Accelerometer()

1	Accel = New Accelerometer()

and call Accel.OnUpdate() in the render loop.

Monkey

Accel.OnUpdate()

1	Accel.OnUpdate()

You can then access Accel.X / Y / Z or Accel.NormalX / NormalY / NormalZ or even the raw data or filtered G value (force) for the three axes. E.g.

Monkey

MySprite.Rotation = -Accel.Y

1	MySprite.Rotation = -Accel.Y

I also added a Github repo with the source files and demos: https://github.com/anatolbogun/monkey2-utils

The screenshot is from that demo with an arrow always pointing to the ground, regardless of device rotation.

I hope this is useful for some projects. I’m just getting into Monkey2 after a few years break from MonkeyX and I love it. Hopefully the community will become as active and helpful as it was for MonkeyX.

Attachments:

1. 

   accelerometer-demo-screenshot.png
   

[CopperCircleParticipant]

Nice thanks.

[Mark SiblyKeymaster]

Very nice!

[DiffrenzyKeymaster]

Great stuff, thank you for sharing.

Yesterday I attended a presentation by Danish firm Rokoko about their Smartsuit Pro, and the presenter mentioned they use Kalman filters to clean up their sensor data, including the accelerometer. Could using that remove the delay?

https://en.m.wikipedia.org/wiki/Kalman_filter

[DruggedBunnyParticipant]

I’d really like to see someone implement the Kalman filter! I’ve thought for some time that this would be great for network lag prediction. Could be wrong, but it seems like it would work well — I gather it’s used in a lot of robotic/automated applications for similar purposes.

All the implementations on Github are completely impenetrable and I couldn’t figure out how to transfer them!

(This does look like a very useful piece of code, BTW, Anatol!)

[AnatolParticipant]

Thanks for the info Diffrenzy and DruggedBunny. I’ll have a look into the Kalman filter if I find the time. The delay with the low pass filter isn’t too bad for my needs, but no delay would be better. I have no idea about Kalman filters, so if anyone else knows more and wants to come up with a Monkey2 implementation that’d be nice, too. But yes, I’m keen to improve this further if I can make sense of Kalman.

[AnatolParticipant]

OK, I’ve done a bit of research about the Kalman filter and there are quite some implementation samples out there, but for now I still put it in the too hard basket. The main reason is that pretty much all implementations use accelerometer and gyroscope combined. I’m not sure how to access gyroscope raw data, I could get the accelerometer data via JoystickDevice, but no idea if gyro is accessible in any way. I also read that when the Kalman filter is set to be very smooth, you also end up with a delay. How much, and how it compares to a low pass filter I don’t know.

Well, anyway, I’m still very interested in this and will probably come back to it at some point, but for now I think I’ll focus on other parts. I’m just very uncertain if it’s worth the effort in the end. Apparently the Kalman filter isn’t really a filter in that sense, but rather a predictor model with biases, sensibilities, noise, etc. Complex stuff. I also read that for simple noise filtering a low pass filter should be enough. There’s no knowing without trying and comparing, and that’ll bug me for sure, but maybe later.

If the delay of the low pass filter is too great for your taste I suggest to play with the dt and rc values of the filter.

Having said that, if anyone is a Kalman genius, please do post an implementation or possible approach in this forum. Or your opinion if it’s even the appropriate approach to reduce noise from the accelerometer.

[GwenelParticipant]

Nice. When the next stable and fixed Monkey2 and I’ve finished my current projects I will show some really nice and innovative filtering method that I think It will suit perfectly.

[Author]

Posts