EKF3
SENSORFUSION

Extended Kalman Filter är den viktigaste algoritmen i autonom drönarflygning. Den svarar på två grundläggande frågor: "Var är jag?" och "Åt vilket håll pekar jag?" Denna sida förklarar vad den gör, varför den är viktig, och hur den fungerar i en GPS-fri EW-miljö.

Vad EKF3 gör — enkelt förklarat

Tänk dig att du sitter med ögonbindel i en bil. Du kan KÄNNA att bilen svänger (det är gyroskopet). Du kan KÄNNA acceleration och inbromsning (det är accelerometern). Någon säger lufttrycket (det är barometern — lägre tryck betyder högre höjd). Och du har en kompass.

Var och en av dessa sensorer ger en ledtråd om var du är och vad du gör. Men var och en är ofullkomlig. Gyroskopet driftar sakta — efter 5 minuter tror det att du svängt 3° mer än du faktiskt gjort. Accelerometern reagerar på vibrationer från motorerna. Kompassen förvirras nära metallföremål. Barometern ändras med vädret.

EKF3 är algoritmen som säger: "Jag vet att varje sensor ljuger lite. Jag vet HUR MYCKET varje sensor vanligtvis ljuger. Jag tar alla deras avläsningar, viktar dem efter hur pålitlig var och en är just nu, och beräknar den BÄSTA MÖJLIGA uppskattningen av var jag befinner mig och hur jag rör mig." Den gör detta 400 gånger per sekund.

I praktiken: om gyroskopet säger att du svängt 30° men kompassen säger 27°, och EKF3 vet att gyrot driftar men kompassen är korrekt i detta område, uppskattar den 27,5° — den litar mer på kompassen. Om kompassen är nära metall och ger galna värden minskar EKF3 automatiskt sitt förtroende för kompassen och förlitar sig mer på gyroskopet.

När GPS störs — AHRS-läge

I en GPS-fri miljö (fienden stör alla satellitsignaler) förlorar EKF3 sin bästa positionssensor. Den växlar till AHRS-läge (Attitude and Heading Reference System). I detta läge vet den drönarens attityd — roll, pitch och yaw — från IMU:n (gyroskop + accelerometer) och barometern. Den vet höjden från barometern. Men den vet INTE lateral position (nord/syd/öst/väst).

Vad detta innebär operativt: drönaren kan fortfarande flyga plant, hålla höjd och behålla kurs. Piloten flyger visuellt genom FPV-glasögonen. AI:n kan fortfarande detektera mål och beräkna RELATIV position från detektionsmatematiken (se §6.1). Men absoluta kartkoordinater kräver antingen terrängmatchning mot förladdat ortofoto eller manuell uppskattning av operatören.

Att lägga till en optisk flödessensor (PMW3901, €8) återställer hastighetsuppskattning relativt marken. Drönaren driftar inte okontrollerat — den kan hålla position. Men positionsuppskattningen driftar långsamt utan GPS-korrektioner. Räkna med ~1-3m/minut drift i stabil hovring.

Sensorerna — vad var och en mäter

Matematiken — för ingenjörer

EKF3 kör en prediktion-korrektion-cykel 400 gånger per sekund. Enkelt uttryckt: "Baserat på vad gyrot och accelerometern berättar FÖRUTSÄGER jag var drönaren borde vara nu. Sedan KONTROLLERAR jag den förutsägelsen mot barometer, kompass och eventuella andra tillgängliga sensorer. Om min förutsägelse var fel KORRIGERAR jag den. Korrigeringens storlek beror på hur mycket jag litar på varje sensor just nu."

Kärnekvatonen (tillståndsprediktion):

# Tillståndsprediktion (körs var 2,5ms)
x_predicted = F @ x_previous + B @ u
# F = tillståndsövergångsmatris (fysikmodell)
# x = tillståndsvektor [position, hastighet, attityd, gyrobias, accelbias]
# u = IMU-avläsningar [accel_x/y/z, gyro_x/y/z]

# Kovariansuppdatering (hur osäkra är vi?)
P_predicted = F @ P_previous @ F.T + Q
# Q = processbrus (hur fel vi förväntar oss att modellen är)

# Mätuppdatering (när baro/kompass/GPS-data anländer)
K = P_predicted @ H.T @ inv(H @ P_predicted @ H.T + R)
# K = Kalmanförstärkning (hur mycket vi litar på mätningen)
# H = mätmatris (mappar tillstånd till sensoravläsning)
# R = mätbrus (hur mycket denna sensor vanligtvis ljuger)

x_corrected = x_predicted + K @ (measurement - H @ x_predicted)
P_corrected = (I - K @ H) @ P_predicted

I klartext: Kalmanförstärkningen K är "tillitsfaktorn." När K är hög litar systemet mer på sensorn än på prediktionen. När K är låg litar det mer på prediktionen. K justeras automatiskt baserat på hur konsekvent varje sensor har varit nyligen. Om kompassen börjar ge oregelbundna avläsningar (nära norrsken, till exempel) sjunker K för kompassen mot noll och systemet förlitar sig på gyro + accelerometer istället.

Matematisk härledning — varför Kalman-förstärkningen är optimal

Den fullständiga femstegs-härledningen av Kalman-uppdateringen publiceras på den engelska versionen. Härledningen följer Kalmans originalartikel (1960) och standardläroboken (Beard & McLain, Small Unmanned Aircraft, Princeton 2012, kap. 8; Simon Optimal State Estimation, Wiley 2006). Sidans bidrag: att specialisera den generella teoremen till Fischer 26:s sensoruppsättning och visa varför varje R-matris­värde har just den storlek den har.

Kortfattat: (1) Linjär mätmodell z = H·x + v med v ~ N(0, R). R-entries kommer från datablad: R_baro = 0,25 m², R_mag = 0,008 rad², R_flow = 0,01 m²/s². (2) Minimera trace(P⁺) över K → K = P⁻·Hᵀ · (H·P⁻·Hᵀ + R)⁻¹ är den unika optimala vägningen. (3) I skalärfallet K = P⁻/(P⁻+R) visar att K → 0 när prediktionen är säker, K → 1 när sensorn är mycket bättre. (4) Innovation gating med 5σ-tröskel förhindrar felaktiga mätningar från att korrumpera tillståndet. (5) Linearisering kring x̂⁻ ger EKF:s nonlinear-hantering; kräver initial konvergens (~30 s stationärt innan tillslag) för att P⁻ ska vara litet nog att linjäriseringen gäller.

Bearbetat exempel 1 — barometeruppdatering accepteras

IMU-integration har drivit höjdestimaten till 118,2 m med 1σ = 2,0 m (P⁻ = 4,0 m²). Barometer läser 120,1 m med R = 0,25 m². K = 4,0/4,25 = 0,941. Uppdaterad estimat: 118,2 + 0,941 × 1,9 = 119,99 m. Ny osäkerhet: σ = √((1−0,941)·4,0) = 0,485 m — strikt mindre än både prediktionen (2 m) och sensorn (0,5 m). Detta är den matematiska anledningen till att flersensorfusion slår enskilda sensorer. Verifierat i provable_claims.py under EKF_SCALAR_UPDATE_GAIN och EKF_SCALAR_UPDATE_POSTERIOR.

Bearbetat exempel 2 — felaktig mätning avvisas av innovation gate

Samma prior (118,2 m, P⁻ = 4,0 m²). Felaktig barometerläsning 85 m (tryckvåg från propeller under aggressiv manöver). Innovation y = −33,2 m; innovationvarians S = 4,25 m²; σ-kvot = 33,2/2,06 = 16,1σ. Överstiger ArduPilots 5σ-grind → mätning AVVISAD, tillstånd bevarat. Vid ihållande felaktiga läsningar i > 2 s markerar EKF3 sensorn som felaktig och GCS visar varning «EK3 IMU0 baro inconsistent». Verifierat i EKF_INNOVATION_GATE_REJECTS_OUTLIER.

Varför denna härledning är operativt viktig

Fyra operativa beslut beror på att Kalman-matematiken är korrekt. Sensoranskaffning: R-matris­entries kommer direkt från datablad; en billigare sensor med 3× bruset ger 9× R-värde och försämrar Kalman-förstärkningen förutsägbart — planeraren kan beräkna den resulterande positions­osäkerheten innan €30/flygram sparas. Optiskt flöde-rekommendation: PMW3901 för €8 minskar hover-drift från 200 m/10 min (enbart IMU) till ~10 m/10 min (IMU + flow). Utan Kalman-härledningen är detta bara ett påstående; med den ser vi varför: flow har liten R (0,01 m²/s²) jämfört med IMU:s processbrus Q. Innovation-gating-disciplin: 5σ-grind är skillnaden mellan «robust fusion» och «sensorfel → katastrofal flygning». Initial-alignment-krav: Fischer 26 måste stå stilla 30 s innan tillslag så att P⁻ blir tillräckligt litet för att lineariseringen ska gälla — en befälhavare som hoppar över detta för «snabbare insättning» skickar en flygram som kanske aldrig konvergerar.

ArduPilot-konfiguration för GPS-fritt

Relaterade kapitel

Källor

ArduPilot EKF3-dokumentation (ardupilot.org/dev/docs/ekf3, 2024). "Extended Kalman Filtering for Battery and GPS-Denied Navigation" — Beard & McLain, Small Unmanned Aircraft (Princeton, 2012). BMI270 IMU datablad (Bosch, 2022). BMP390 barometer datablad (Bosch, 2023). PMW3901 optiskt flöde datablad (PixArt, 2023). FOI PM 8336 om autonoma UAS-system (2024).