Filtro complementare vs filtro di Kalman

Ho provato a fare un po' di pastrocchi con il filtro complementare e il filtro di Kalman applicati ad un accelerometro e un giroscopio finalizzati ad un self-balancing.
Il risultato è che un filtro complementare se la batte ad armi pari con il filtro di kalman. Invece un filtro complementare del secondo ordine non sono riuscito a settarlo bene. E' un po' 'nervosetto' e filtra peggio di quello del primo ordine. E questo è strano perchè dovrebbe essere migliore. Devo settare meglio i coefficienti oppure ho sbagliato qualcosa nell'implementazione.
Chi è interessato può dare una occhiata a questo link: