Kalman filter

Publicat de Simedru Florin pe aprilie 27, 2023

Descriere

Kalman Filter este un algoritm de filtrare și estimarea stării, folosit în mod obișnuit în ingineria sistemelor și în domeniul științelor exacte, precum fizica, astronomia și informatica.

Scopul său este de a estima starea unui sistem în timp real, pe baza datelor de intrare disponibile și a modelelor matematice ale sistemului. Acesta funcționează prin combinarea unei estimări a stării anterioare a sistemului cu măsurători recente, pentru a produce o estimare mai precisă a stării actuale a sistemului.

Kalman Filter se bazează pe modele matematice și utilizează statistici pentru a reduce erorile de măsurare și pentru a furniza o estimare cât mai precisă a stării sistemului. Este utilizat într-o varietate de aplicații, cum ar fi navigarea GPS, detectarea obiectelor, controlul de mișcare și multe altele.

În esență, Kalman Filter este un algoritm puternic care poate fi folosit pentru a furniza estimări precise ale stării unui sistem, ceea ce îl face util într-o varietate de aplicații practice.

Algoritmul Kalman filter

Algoritmul Kalman Filter este un proces iterativ care estimează starea sistemului în fiecare pas de timp, pe baza datelor de intrare disponibile și a modelelor matematice ale sistemului. În general, acest algoritm poate fi divizat în două etape principale: predicție și actualizare.

Etapa de predicție:
- Estimează starea actuală a sistemului pe baza stării anterioare și a modelelor matematice ale sistemului.
- Calculează matricea de covarianță a stării actuale, care reflectă incertitudinea estimării.

Etapa de actualizare:
- Obține o nouă măsurare a sistemului.
- Calculează o estimare actualizată a stării sistemului, care combină măsurătorile actuale și estimarea anterioară.
- Calculează matricea de covarianță actualizată a stării, care reflectă incertitudinea estimării actualizate.

În general, Kalman Filter utilizează matricele de covarianță pentru a reflecta incertitudinea în estimarea stării sistemului și pentru a ajusta ponderile atribuite măsurătorilor și estimării anterioare în estimarea actualizată a stării sistemului. Cu cât incertitudinea este mai mare, cu atât se acordă o mai mare greutate măsurătorilor actuale, iar cu cât incertitudinea este mai mică, cu atât se acordă o mai mare greutate estimării anterioare.

Imaginea de mai sus am împrumutat-o de pe www.bzarg.com.

În general, Kalman Filter este un algoritm puternic și flexibil, care poate fi adaptat pentru o varietate de aplicații practice, dar necesită o cunoaștere detaliată a modelelor matematice ale sistemului și a caracteristicilor datelor de intrare pentru a funcționa corect.

Etapa de predicție

Etapa de predicție a algoritmului Kalman Filter se bazează pe propagarea stării anterioare a sistemului prin intermediul modelului matematic și a măsurătorilor de intrare disponibile. Formula matematică pentru etapa de predicție poate fi reprezentată astfel:

Stare prezisă a sistemului:

$$\hat{x}_k = F_k \hat{x}_{k-1} + B_k u_k$$
Covarianța prezisă a stării:
$P_k = F_k P_{k-1} F_k^T + Q_k$

unde:
- $x̂_k$ este starea prezisă a sistemului la momentul $k$
- $F_k$ este matricea de tranziție a stării, care propagă starea anterioară la momentul curent
- $x̂_{k-1}$ este starea anterioară a sistemului la momentul $(k-1)$
- $B_k$ este matricea de intrare, care modelează efectul intrării $u_k$ asupra stării sistemului
- $u_k$ este intrarea sistemului la momentul k
- $P_k$ este covarianța prezisă a stării sistemului la momentul k
- $P_{k-1}$ este covarianța anterioară a stării la momentul $(k-1)$
- $Q_k$ este matricea de covarianță a zgomotului procesului, care modelează incertitudinea din modelul matematic al sistemului și din intrările $u_k$

Aceste formule permit estimarea prezisă a stării sistemului și a incertitudinii sale la momentul k, pe baza stării anterioare și a modelelor matematice ale sistemului, înainte de a lua în considerare măsurătorile actuale.

Etapa de actualizare

Etapa de actualizare a algoritmului Kalman Filter combină informația disponibilă din măsurătorile actuale cu informația prezisă din etapa de predictie, pentru a obține o estimare mai precisă a stării sistemului și a incertitudinii sale. Formula matematică pentru etapa de actualizare poate fi reprezentată astfel:

Innovația:
$y_k = z_k - H_k\hat{x}_k$

Covarianța innovației:
$S_k = H_k P_k H_k^T + R_k$

Ganimea Kalman:
$K_k = P_k H_k^T S_k^{-1}$

Starea actualizată a sistemului:
$\hat{x}_k = \hat{x}_k + K_k y_k$

Covarianța actualizată a stării:
$P_k = (I - K_k H_k) P_k$

unde:
- $y_k$ este innovația, adică diferența dintre măsurarea actuală $z_k$ și starea prezisă a sistemului $H_k \hat{x}_k$
- $z_k$ este măsurarea actuală a sistemului la momentul k
- $H_k$ este matricea de observație, care modelează cum starea sistemului este măsurată
- $R_k$ este matricea de covarianță a zgomotului de măsurare, care modelează incertitudinea din măsurătorile efective
- $S_k$ este covarianța innovației, adică covarianța diferenței dintre măsurarea actuală și predicția sistemului
- $K_k$ este ganimea Kalman, care reprezintă ponderile ce se acordă măsurătorii actuale și predicției sistemului în obținerea estimării finale a stării
- $I$ este matricea identitate, iar simbolul $^T$ reprezintă transpusa matricei.

Aceste formule permit actualizarea estimării stării sistemului și a incertitudinii sale, pe baza măsurătorilor actuale și a predicției sistemului din etapa anterioară.

Matricea de ponderi Kalman

Matrice de ponderi care este denumită și Ganimea Kalman este utilizată în etapa de actualizare a algoritmului Kalman Filter. Aceasta este o matrice n x m, unde n este dimensiunea stării sistemului, iar m este dimensiunea măsurătorii.

Ganimea Kalman este calculată astfel:

$K_k = P_k H_k^T (H_k P_k H_k^T + R_k)^{-1}$

unde:
- $P_k$ este covarianța stării prezise la momentul k
- $H_k$ este matricea de observație la momentul k, care transformă starea sistemului în spațiul de măsurare
- $R_k$ este covarianța zgomotului de măsurare la momentul $k$

Ganimea Kalman este o matrice de ponderi care determină cât de mult să fie luate în considerare predicția sistemului și măsurarea efectivă în actualizarea estimării stării. Dacă incertitudinea măsurătorilor este mică, ponderea dată măsurătorii efective va fi mai mare, iar dacă incertitudinea prezicerii este mică, ponderea dată predicției sistemului va fi mai mare.

Ganimea Kalman poate fi interpretată ca o matrice de reglare a feedback-ului, care ajustează predicția sistemului în funcție de măsurarea efectivă, astfel încât estimarea finală a stării să fie cât mai precisă.

Exemple de utilizare

Un exemplu comun de utilizare a algoritmului Kalman Filter este în urmărirea poziției și vitezei unui obiect care se mișcă în spațiu, cum ar fi un vehicul, un avion sau o navă.

Kalman Filter este utilizat pentru a estima starea obiectului, bazându-se pe măsurători imprecise și zgomotoase ale poziției și vitezei. Algoritmul ia în considerare datele disponibile din trecut și prezică starea curentă a obiectului, în timp ce reduce eroarea măsurătorilor și îmbunătățește precizia predicțiilor viitoare.

De exemplu, un avion poate fi echipat cu senzori GPS și altimetru, care furnizează date despre poziția sa și altitudinea. În plus, avionul poate fi echipat cu senzori de viteză și accelerometre pentru a măsura viteza și accelerația sa. Aceste date sunt apoi prelucrate prin intermediul algoritmului Kalman Filter pentru a estima starea actuală a avionului și a prezice poziția și viteza sa în viitor.

Algoritmul Kalman Filter poate fi, de asemenea, utilizat într-o gamă largă de alte domenii, cum ar fi robotica, navigația în spațiu, procesarea semnalelor și economia.

Exemplu


#include "stdio.h"

// functie pentru actualizarea estimarii
void update_estimare(float *x, float *P, float z, float R, float Q) {
  // predictie
  float x_pred = *x;
  float P_pred = *P + Q;

  // corectie
  float K = P_pred / (P_pred + R);
  *x = x_pred + K * (z - x_pred);
  *P = (1 - K) * P_pred;
}

int main() {
  float velocity = 0.0;    // viteza initiala
  float variance = 100.0;  // varianta initiala a erorii de estimare

  float measurement = 10.0; // o masuratoare a vitezei
  float measurement_variance = 25.0; // varianta masuratorii

  float process_variance = 1.0; // varianta procesului

  // actualizarea estimei
  update_estimare(&velocity, &variance, measurement, measurement_variance, process_variance);

  // afisare rezultat
  printf("Viteza estimata: %f\n", velocity);

  return 0;
}

Documentație proiect

Mulțumesc pentru atenție!

Pentru întrebări și/sau consultanță tehnică vă stau la dispoziție pe blog mai jos în secțiunea de comentarii sau pe email simedruflorin@automatic-house.ro. O zi plăcută tuturor !

Automatic house blog

Faceți căutări pe acest blog