Stap 10: Een erg vleiend filter.
hoek = A * (hoek + tarief * DT) + (1 - A) * (float) accel_raw * A_GAIN;
Oke, dus het is een lange lijn. U kunt het ook doen met een opamp of twee.
Dit filter doet precies wat nodig is in dit scenario is: het gunsten de gyroscoop lezen voor korte tijdsduur en het gemiddelde van de versnellingsmeter lezen voor lange tijdsduur. Laten we breken:
A is de factor die de cutoff tijd bepaalt voor vertrouwen de gyro en filteren in de versnellingsmeter. Het is altijd tussen 0 en 1, meestal dicht bij 1. In dit geval wordt A gedefinieerd als 0.962. Dit betekent dat bij elke keer stap, 96,2% van de nieuwe hoekmeting van de oude hoekmeting plus de geïntegreerde gyro-meting komt. De resterende 3,8% is afkomstig van de versnellingsmeter. Dit gemiddelde langzaam in de versnellingsmeter over veel tijdstappen.
DT is de tijd in seconden tussen programma lussen, de tijd stap. Het wordt hier gedefinieerd als 0.020 en wordt ingesteld door de vertraging(20) aan het einde van de lus. De code in de lus zelf nemen veel minder dan 20ms, zodat de vertraging domineert de tijd stap.
tarief is de gyro lezen, worden geconverteerd naar graden per seconde.
accel_raw * A_GAIN is de versnellingsmeter lezing, geconverteerd naar graden. Het is heel belangrijk dat deze twee in dezelfde eenheid basis vóór de optelling. (U kunt het toevoegen van appels aan sinaasappelen.)
De tijdconstante van het filter is de duur van het tijdstip waarop de gyroscoop lezing begint te zwaar worden uitgefilterd en de versnellingsmeter lezing begint te worden gemiddeld zwaar. Het is eigenlijk een continu proces, maar de tijdconstante is één maatregel van waar het evenwicht begint te verschuiven. De tijdconstante is:
tau = DT*(A)/(1-A) = 0.5s
Dus, voor dit filter, is de gyro vertrouwd voor over 0,5 seconde en het begin van de versnellingsmeter tot het gemiddelde aanzienlijk daarna. Deze waarde kan worden getweaked door het veranderen van A.
