Stap 15: IMU deel 2: aanvullende Filter
De afbeelding toont het een aanvullende Filter in blok schematisch. De sensor signalen, geconverteerd naar graden en graden per seconde, zijn de ingangen. De hoek van de versnellingsmeter is low-pass gefilterd, vermindering van de invloed van korte duur signalen maar het behoud van een lange-termijn gemiddelde (van de zwaartekracht). De gyro hoekige tarief voor het eerst is geïntegreerd, om een schatting van hoek, en vervolgens hoogdoorlaat gefilterd, schakel alle lange termijn drift. De twee gefilterde signalen worden samen opgeteld om te maken een schatting van één hoek die de beste delen van beide sensor signalen combineert.
Het is een van de weinige gevallen waar de code beknopter dan de blokdiagram of de schriftelijke verklaring is:
Uittreksel uit pd_digitalIMU.pde in 4pcb_ARD:
// -------------------------------------------------------------------
rate_pitch = (float) g_pitch * G_GAIN;
angle_pitch = AA * (angle_pitch + rate_pitch * DT);
angle_pitch += (1.0 - AA) * (float) a_pitch * A_GAIN;
// -------------------------------------------------------------------
De eerste regel zet de ruwe gyro signaal, g_pitch, in º / s, zoals besproken in de vorige stap. Het einde van de tweede regel is de numerieke integratie. DT is de lus tijd, 0.01s. Een kleine verandering in de hoek, rate_pitch * DT, is toegevoegd aan de vorige schatting van de hoek.
De rest van de tweede regel en alle van de derde regel vormen de high-pass en low-pass filters, in afzonderlijke vorm. De high-pass fungeert op de raming van de geïntegreerde hoek en de low-pass op het signaal van de versnellingsmeter, a_pitch. De constante AA bepaalt de tijdschaal van de high - en low-pass filters, en is een waarde die u kunt tweak afhankelijk van uw quadrotor. Grotere AA vertrouwt het signaal van de geïntegreerde gyro langer. Kortere AA wordt het signaal van de versnellingsmeter in sneller samengevoegd.
De waarde voor AA gebruikt in 4pcb komt overeen met een tijdconstante van 1s. Dit is de lengte van de tijd waarvoor de gyro hoek raming vertrouwd wordt, en de lengte van de tijd waarover het accerometer signaal is gemiddeld. Aangezien dit oneindige impuls-respons (IIR) filters, de overgang wordt vloeiend en de tijdconstante alleen vertegenwoordigt waar de weging van deze twee filters is gelijk.
Ik schreef een meer diepgaande uitleg van het aanvullende Filter toegepast op zichzelf balanceren in deze white paper. Het behandelt enkele van de details van hoe AA is gerelateerd aan de tijdconstante van de low-pass en high-pass filters, hoe stem sensor bias is van invloed op het filter, en wat meer details.
