non ho controllato la formula ma direi che sei entrato nello spirito giusto 
Però ti stai complicando la vita: anzichè leggere la velocità reale di rotazione delle ruote stai facendo una stima di vari altri parametri, un sacco di calcoli (e ricorda che l'arduino ha solo i float, quindi numeri a virgola mobile che hano una precisione bassa, 9 decimali al massimo e comunque hai un minimo di errore anche in tutti i numeri non potenze di 2)
insamma con metodo approssimativi rilevi dei valori che poi calcoli in modo approssimativo per ottenere una velocità per ottenere il tempo necessario per una rotazione... In pratica la cosa funziona, ad occhio, ma se proverai a girare per un pò di volte vedrai che l'errore si accumula diventando molto evidente (senza contare l'errore di integrazione, se hai fatto gli integrali!)
anche misurando la velocità della ruota avrai questo errore, ma sarà più piccolo e quindi il sistema più stabile. o usi un sistema della lettura dell'inclinazione assoluto (come un magnetometro, ovvero una bussola) o usi un relativo che però dovrai cercare di rendere il più preciso possibile, quindi evitare il più possibile rigiri matematici/elettromeccanici.