Got speed calculation working.
Playing with Gains trying to find something reasonable, yet I fail.