Der grundlegende Fehler ist deine Mittelwertbildung, glaub es doch endlich. Es bringt Nichts, wenn man in einer Schleife, die in 20ms "durch" ist, die Werte einsammelt und mittelt - dann hast du einen Mittelwert über die 20ms - das ist viel zu schnell und folgt damit immer noch den Sprüngen des Sensorsignales ziemlich direkt.
Mein Vorschlag sah vor, jeweils einen Teil des Mittelwertes bei jedem Haupt-Loop dazuzuholen, deshalb dieses "Schieberegister" (siehe meinen Beitrag oben). Wenn also der gesamte Loop mit einem Delay von z.B. 20ms versehen ist, werden nicht in 20ms alle Messwerte gemittelt, sondern alle 20ms kommt ein Teil dazu, DAS ist der entscheidende Unterschied.
Würde man diesen Puffer z.B. 500 Variablen groß machen, dann hättest du einen Mittelwert über eine Sekunde ...