Took away low pass filtering for testing
Everything works, pretty much