Stikkord: matematikk

Kalman-filteret er kanskje det mest elegante verktøyet vi har for å få orden på støyete sensordata. La oss demystifisere denne kraftige algoritmen og se hvordan du kan implementere den i dine egne prosjekter.

Intuisjonen bak Kalman-filteret

Kalman-filteret fungerer som en intelligent balanse mellom to informasjonskilder: din prediksjon av hvor systemet burde være, og målingen av hvor sensoren sier det faktisk er. Tenk på det som å navigere i tåke – du har et kart (modellen) og et kompass (sensoren), men begge kan ha feil.

Filterets genialitet ligger i at det kontinuerlig vurderer hvor mye det skal stole på hver kilde. Er sensoren unøyaktig? Filteret legger mer vekt på prediksjonene. Er modellen usikker? Da får sensor-målingene større innflytelse.

De fem kjernelikningene

Kalman-filteret bygger på fem matematiske ligninger som håndterer to faser: predict og update. Ikke la matematikken skremme deg – vi skal se hvordan dette oversettes til Python-kode.

Predict-Update syklusen

Hver iterasjon av Kalman-filteret består av to distinkte faser:

1. Predict-fasen

Her bruker vi systemmodellen til å predikere neste tilstand basert på forrige estimat. Vi beregner også usikkerheten som akkumuleres over tid.


x_pred = F * x_prev + B * u
P_pred = F * P_prev * F.T + Q


2. Update-fasen

Når vi får nye sensormålinger, oppdaterer vi estimatet vårt ved å veie prediksjon mot måling.


K = P_pred * H.T / (H * P_pred * H.T + R)
x_new = x_pred + K * (z - H * x_pred)
P_new = (I - K * H) * P_pred


Tips: Start alltid med å definere din tilstandsvektor (hva vil du estimere?) og målevektoren (hva kan sensorene dine måle?). Dette er grunnmuren i implementasjonen din.

Python-implementasjon

La oss implementere et enkelt 1D Kalman-filter for å spore posisjonen til et objekt:


import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter:
    def __init__(self, F, H, Q, R, P, x):
        self.F = F # Tilstandsovergangsmatrise
        self.H = H # Observasjonsmatrise
        self.Q = Q # Prosessstøy
        self.R = R # Målestøy
        self.P = P # Kovariansmatrise
        self.x = x # Tilstandsvektor

    def predict(self):
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q

    def update(self, z):
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        y = z - self.H @ self.x
        self.x = self.x + K @ y
        self.P = self.P - K @ self.H @ self.P


Praktisk eksempel: Temperatursensor

La oss bruke filteret på en støyete temperatursensor. Vi antar at temperaturen endres sakte over tid:


# Initialiser filter for temperatur
dt = 1.0 # Tidssteg
F = np.array([[1]]) # Temperatur endres ikke
H = np.array([[1]]) # Vi måler temperaturen direkte
Q = np.array([[0.1]]) # Liten prosessstøy
R = np.array([[1.0]]) # Målestøy fra sensor
P = np.array([[1.0]]) # Inicial usikkerhet
x = np.array([[20.0]]) # Start temperatur

kf = KalmanFilter(F, H, Q, R, P, x)

# Simuler støyete målinger
true_temp = 22.0
measurements = true_temp + np.random.normal(0, 1, 100)
estimates = []

for z in measurements:
    kf.predict()
    kf.update(np.array([[z]]))
    estimates.append(kf.x[0, 0])


Tips: Bruk RoboNordic sine sensorer som DHT22 eller DS18B20 for å teste implementasjonen din med ekte data. Sammenlign filtrerte og ufiltrerte målinger for å se effekten.

Tuning av parametere

Suksessen til Kalman-filteret avhenger sterkt av korrekt tuning av støyparameterne Q og R:

Prosessstøy (Q)

Representerer hvor mye systemet kan endre seg mellom målinger. For høy Q gjør filteret nervøst, for lav gjør det tregt.

Målestøy (R)

Beskriver hvor nøyaktige sensorene dine er. Høy R betyr at filteret stoler mindre på sensormålingene.

Tips: Start med å karakterisere sensoren din ved å ta mange målinger av en konstant verdi. Standardavviket gir deg et godt estimat for R.

Automatisk tuning

Du kan bruke innovasjonssekvensen til å adaptivt justere parameterne:


def adaptive_tuning(self, innovation, window_size=10):
    if len(self.innovations) > window_size:
        innovation_var = np.var(self.innovations[-window_size:])
        expected_var = self.H @ self.P @ self.H.T + self.R
        if innovation_var > 1.5 * expected_var:
            self.Q *= 1.1 # Øk prosessstøy


Vanlige feil

• Feil dimensjoner på matriser: Dobbeltsjekk at F, H, Q, R og P har kompatible dimensjoner. NumPy gir ofte kryptiske feilmeldinger hvis dimensjonene ikke stemmer.
• For lav prosessstøy: Filteret blir «låst» til første estimat og reagerer ikke på endringer. Øk Q-verdiene gradvis.
• Numerisk ustabilitet: Bruk Joseph-formen for kovariansoppdateringen: P = (I - KH)P(I - KH)' + KRK' for bedre numerisk stabilitet.
• Glemmer å kalle predict(): Mange glemmer predict-steget når det ikke er kontrollinngang. Dette er likevel nødvendig for å oppdatere usikkerheten.
• Feil initialisering: Start med høy initial usikkerhet (P) hvis du er usikker på startverdien. Filteret vil konvergere raskt.

Videre utvikling

Når du mestrer det lineære Kalman-filteret, kan du utforske Extended Kalman Filter (EKF) for ikke-lineære systemer, eller Unscented Kalman Filter (UKF) for enda bedre håndtering av ikke-lineariteter.

For sensorfusjon av flere sensorer, som kombinasjon av akselerometer og gyro, blir Kalman-filteret et uvurderlig verktøy for å få stabile og nøyaktige estimater av orientering og posisjon.