Chapitre 13
Filtre de Kalman linéaire

1. Intuition

Un robot ne connaît jamais son état exact. Les capteurs sont bruités (un GPS oscille de quelques mètres, un accéléromètre dérive) et le modèle de mouvement n'est qu'une approximation. Le filtre de Kalman résout ce dilemme : il combine deux sources d'information imparfaites — ce que le modèle prédit et ce que les mesures observent — pour produire une estimation meilleure que chacune prise isolément.

L'idée maîtresse : on ne suit pas seulement une valeur, mais une distribution de probabilité. Pour un système linéaire dont les bruits sont gaussiens, cette distribution reste gaussienne à chaque instant, entièrement décrite par sa moyenne (l'état estimé x) et sa covariance (la matrice P, notre incertitude). Sous ces hypothèses, le filtre de Kalman est l'estimateur optimal au sens de l'erreur quadratique moyenne : aucun autre estimateur ne fait mieux.

En robotique on le rencontre partout : suivi de position/vitesse d'un mobile, fusion de capteurs (IMU + odométrie + GPS), estimation d'attitude, poursuite de cible. À chaque cycle, le filtre prédit puis corrige, indéfiniment.

2. Le modèle

On modélise un système dynamique linéaire à temps discret. À l'instant k :

• État x (vecteur n) : les grandeurs à estimer (ex. position et vitesse).
• Matrice de transition F (n×n) : comment l'état évolue d'un pas au suivant selon la physique.
• Commande u et matrice de commande B : l'effet des entrées connues (accélération commandée, couple).
• Bruit de process Q (n×n, covariance) : ce que le modèle ne capture pas (forces non modélisées, discrétisation).
• Mesure z (vecteur m) : ce que les capteurs renvoient.
• Matrice d'observation H (m×n) : relie l'état à ce qu'on mesure (z = H·x en l'absence de bruit).
• Bruit de mesure R (m×m, covariance) : le bruit propre des capteurs.
• Covariance d'état P (n×n) : notre incertitude courante sur x.

Le système obéit à deux équations, l'une de dynamique, l'autre d'observation :

Les bruits w et v sont supposés blancs, gaussiens, de moyenne nulle et mutuellement indépendants. Ce sont ces hypothèses qui rendent le filtre optimal.

3. Les équations

Chaque cycle enchaîne deux phases. On note x⁻, P⁻ l'état et la covariance prédits (avant mesure, « a priori »), et x, P les valeurs corrigées (après mesure, « a posteriori »).

3.1 Prédiction

On propage l'état à travers la dynamique. La covariance est transportée par F·P·Fᵀ (une covariance se transforme en « sandwich » F … Fᵀ, jamais un simple produit) puis augmentée de Q : prédire, c'est perdre de la certitude.

3.2 Mise à jour (correction)

L'innovation y mesure l'écart entre la mesure réelle et la mesure attendue depuis la prédiction : c'est la nouvelle information. S est l'incertitude totale de cet écart (incertitude prédite projetée dans l'espace des mesures, plus le bruit capteur R). Le gain K dose enfin la correction, et l'état est ajusté proportionnellement à l'innovation.

4. Le sens du gain de Kalman

Tout le filtre tient dans l'arbitrage porté par K = P⁻·Hᵀ·S⁻¹. Regardons les deux régimes extrêmes sur un cas scalaire où K = P⁻ / (P⁻ + R) :

• Capteur très précis (R → 0) : K → 1. Le filtre fait confiance à la mesure et corrige fortement, x ≈ z.
• Capteur très bruité (R → ∞) : K → 0. Le filtre ignore la mesure et garde sa prédiction, x ≈ x⁻.

Le gain arbitre donc en permanence entre croire le modèle et croire la mesure, en fonction de leurs incertitudes respectives. C'est un pont de Wheatstone probabiliste.

5. Exemple concret : suivi 1D position-vitesse

Suivons un mobile sur un axe. On mesure seulement sa position (un capteur bruité), mais on veut aussi estimer sa vitesse. C'est là que Kalman brille : la vitesse n'est jamais mesurée directement, elle est inférée de l'évolution des positions.

État : x = [p, v]ᵀ (position, vitesse). Modèle à vitesse constante sur un pas dt : p ← p + v·dt, v ← v. D'où :

H = [1, 0] traduit « je ne mesure que la position ». Déroulons l'intuition sur quelques cycles (sans commande, u = 0) :

1. Départ. On initialise x = [0, 0] mais on ne sait pas vraiment : P est grand (forte incertitude).
2. Prédiction. Avec v = 0, la position prédite ne bouge pas, mais P⁻ = F·P·Fᵀ + Q grandit : on doute davantage.
3. Mesure. Le capteur annonce z = 1.0. L'innovation y = 1.0 − 0 = 1.0 est forte. Comme P⁻ est grand, K est proche de 1 : le filtre saute presque à la mesure.
4. Cycles suivants. Les positions successives montent régulièrement. Le terme croisé de P couple position et vitesse : le filtre déduit qu'une vitesse non nulle explique le mouvement, et son estimation de v converge vers la vraie vitesse — alors qu'aucun capteur ne l'a jamais mesurée.

Après quelques dizaines de cycles, P se stabilise (régime permanent) et K devient constant : le filtre a trouvé son équilibre entre modèle et mesure.

6. Implémentation Rust avec nalgebra

On utilise nalgebra avec des dimensions statiques (Const<N> via les alias SMatrix/SVector) : les matrices vivent sur la pile, sans allocation, et les dimensions incompatibles sont rejetées à la compilation. La struct est générique sur la dimension d'état N et la dimension de mesure M (const generics).

use nalgebra::{SMatrix, SVector};

/// Filtre de Kalman linéaire générique.
/// N = dimension de l'état, M = dimension de la mesure.
pub struct FiltreKalman<const N: usize, const M: usize> {
    pub x: SVector<f64, N>,       // état estimé
    pub p: SMatrix<f64, N, N>,    // covariance d'état
    pub f: SMatrix<f64, N, N>,    // transition
    pub q: SMatrix<f64, N, N>,    // bruit de process
    pub h: SMatrix<f64, M, N>,    // observation
    pub r: SMatrix<f64, M, M>,    // bruit de mesure
}

impl<const N: usize, const M: usize> FiltreKalman<N, M> {
    pub fn new(
        x0: SVector<f64, N>,
        p0: SMatrix<f64, N, N>,
        f: SMatrix<f64, N, N>,
        q: SMatrix<f64, N, N>,
        h: SMatrix<f64, M, N>,
        r: SMatrix<f64, M, M>,
    ) -> Self {
        Self { x: x0, p: p0, f, q, h, r }
    }

    /// Phase de prédiction (commande nulle ici pour rester simple).
    /// x⁻ = F·x        P⁻ = F·P·Fᵀ + Q
    pub fn predict(&mut self) {
        self.x = self.f * self.x;
        self.p = self.f * self.p * self.f.transpose() + self.q;
    }

    /// Phase de correction avec une mesure z.
    /// y = z − H·x⁻      S = H·P⁻·Hᵀ + R      K = P⁻·Hᵀ·S⁻¹
    /// x = x⁻ + K·y      P = (I − K·H)·P⁻
    /// Renvoie false si S n'est pas inversible (correction ignorée).
    pub fn update(&mut self, z: SVector<f64, M>) -> bool {
        let y = z - self.h * self.x;                       // innovation (M)
        let s = self.h * self.p * self.h.transpose() + self.r; // (M×M)

        let s_inv = match s.try_inverse() {
            Some(inv) => inv,
            None => return false, // S singulière : on saute la correction
        };

        let k = self.p * self.h.transpose() * s_inv;       // gain (N×M)
        self.x += k * y;

        let i = SMatrix::<f64, N, N>::identity();
        self.p = (i - k * self.h) * self.p;

        // On resymétrise pour contrer la dérive numérique.
        self.p = (self.p + self.p.transpose()) * 0.5;
        true
    }
}

use nalgebra::{Matrix1, Matrix2, Vector1, Vector2};

fn main() {
    let dt = 1.0_f64;

    // Modèle à vitesse constante : état [position, vitesse].
    let f = Matrix2::new(1.0, dt,
                         0.0, 1.0);
    let h = nalgebra::Matrix1x2::new(1.0, 0.0); // on ne mesure que la position

    // Bruit de process : petite incertitude sur le modèle.
    let q = Matrix2::new(0.001, 0.0,
                         0.0,   0.001);
    // Bruit de mesure : variance du capteur de position.
    let r = Matrix1::new(0.5);

    // Initialisation : on ne sait presque rien -> P grand.
    let x0 = Vector2::new(0.0, 0.0);
    let p0 = Matrix2::new(100.0, 0.0,
                          0.0,   100.0);

    let mut kf: FiltreKalman<2, 1> =
        FiltreKalman::new(x0, p0, f, q, h, r);

    // Vérité terrain : le mobile part de 0 et avance à 1 m/s.
    // On simule des mesures de position bruitées (bruit pseudo-aléatoire simple).
    let vraie_vitesse = 1.0;
    let mut graine = 12345u64;
    let mut bruit = || {
        // générateur congruentiel linéaire -> bruit centré dans [-0.7, 0.7]
        graine = graine.wrapping_mul(6364136223846793005).wrapping_add(1);
        ((graine >> 33) as f64 / u32::MAX as f64 - 0.5) * 1.4
    };

    println!(" k |  z (mesure) | p estimée | v estimée");
    for k in 1..=10 {
        let vraie_position = vraie_vitesse * k as f64;
        let z = Vector1::new(vraie_position + bruit());

        kf.predict();
        kf.update(z);

        println!(
            "{:2} |   {:7.3}   |  {:6.3}   |  {:6.3}",
            k, z[0], kf.x[0], kf.x[1]
        );
    }
}

À l'exécution, la position estimée suit les mesures en les lissant, et l'estimation de vitesse converge vers 1.0 alors qu'aucune mesure de vitesse n'a jamais été fournie : la vitesse est reconstruite par le couplage dans P et le modèle F.

7. Temps réel et stabilité numérique

8. Et pour le non linéaire ? L'EKF

Le filtre linéaire suppose F et H constantes. Or beaucoup de systèmes robotiques sont non linéaires : la dynamique x = g(x, u) et l'observation z = h(x) sont des fonctions non linéaires (angles, distances euclidiennes, cinématique).

Le filtre de Kalman étendu (EKF) garde exactement la même structure prédiction/mise à jour, mais linéarise localement ces fonctions à chaque cycle via leurs jacobiennes :

C'est le même calcul de jacobienne que celui rencontré pour la cinématique inverse au chapitre 12 : on remplace la matrice constante par la matrice des dérivées partielles évaluée à l'estimation courante. L'EKF perd la garantie d'optimalité (la linéarisation introduit une erreur) mais reste l'outil de fusion de capteurs le plus répandu en robotique mobile. Nous le détaillerons dans un chapitre ultérieur.

Exercices

pub fn predict(&mut self) {
    // Propagation de l'état.
    self.x = self.f * self.x;
    // Propagation de la covariance : sandwich F·P·Fᵀ puis + Q.
    self.p = self.f * self.p * self.f.transpose() + self.q;
}

pub fn update(&mut self, z: SVector<f64, M>) -> bool {
    let y = z - self.h * self.x;                            // innovation
    let s = self.h * self.p * self.h.transpose() + self.r;  // covariance innovation

    // Inversion de S : gérer le cas singulier plutôt que paniquer.
    let s_inv = match s.try_inverse() {
        Some(inv) => inv,
        None => return false,
    };

    let k = self.p * self.h.transpose() * s_inv;            // gain de Kalman
    self.x += k * y;

    let i = SMatrix::<f64, N, N>::identity();
    self.p = (i - k * self.h) * self.p;
    self.p = (self.p + self.p.transpose()) * 0.5;           // resymétrisation
    true
}

use nalgebra::{Matrix1, Matrix1x2, Matrix2, Vector1, Vector2};

fn main() {
    let dt = 1.0;
    let f = Matrix2::new(1.0, dt, 0.0, 1.0);
    let h = Matrix1x2::new(1.0, 0.0);
    let q = Matrix2::new(0.001, 0.0, 0.0, 0.001);
    let r = Matrix1::new(0.3);
    let x0 = Vector2::new(0.0, 0.0);
    let p0 = Matrix2::new(100.0, 0.0, 0.0, 100.0);

    let mut kf: FiltreKalman<2, 1> = FiltreKalman::new(x0, p0, f, q, h, r);

    for z in [1.0, 2.1, 2.9, 4.2, 5.0] {
        kf.predict();
        kf.update(Vector1::new(z));
        println!("pos = {:.3}, vit = {:.3}", kf.x[0], kf.x[1]);
    }
}

Récapitulatif