robots - thawk/wiki GitHub Wiki
-
The Balance Filter
-
http://www.geekmomprojects.com/gyroscopes-and-accelerometers-on-a-chip/
Filtered Angle = α × (Gyroscope Angle) + (1 − α) × (Accelerometer Angle)
where
α = τ/(τ + Δt) (Gyroscope Angle) = (Last Measured Filtered Angle) + ω×Δt
ω = angular velocity Δt = sampling rate τ = time constant greater than timescale of typical accelerometer noise
I had a sampling rate of about 0.04 seconds and chose a time constant of about 1 second, giving α ≈ 0.96.
Roll = atan2(X, Z) * 180/PI; Pitch = atan2(Y, sqrt(X*X + Z*Z)) * 180/PI;