Module L19 · Partie I · Robotique et systèmes embarqués

Perception : estimer l’état du monde à partir de capteurs qui mentent.

Un gyroscope dérive, un accéléromètre tremble, un GPS saute de 3 mètres, une caméra voit des pixels et pas des objets. Le robot doit pourtant savoir où il est et ce qui l’entoure. La perception est l’art de combiner des mesures bruitées avec un modèle du mouvement pour obtenir une estimation meilleure que chaque source. Ce module construit le filtre de Kalman, fusionne une IMU, et pose les bases du traitement d’images.

Durée : 4 séances · Prérequis : L10, L11, L12, séance 28. Objectifs : modèle capteur et bruit, filtres simples (moyenne, médiane, passe-bas, complémentaire), filtre de Kalman 1D puis multidimensionnel (prédiction/correction, covariances), EKF pour un modèle non linéaire, fusion IMU (attitude), calibration, images (convolution, gradients, seuillage, détection de contours et de droites), caméra (modèle sténopé, calibration, homographie), un détecteur de ligne pour robot suiveur.

Ce que vous saurez faire à la fin

Implémenter un filtre de Kalman et régler ses matrices de bruit avec méthode.
Estimer l’attitude d’un robot à partir d’une IMU 6 axes.
Détecter une ligne, un marqueur ou un obstacle dans une image avec NumPy.
Expliquer ce qu’une caméra mesure vraiment et comment passer des pixels aux mètres.

Références : Probabilistic Robotics (Thrun, Burgard, Fox) chapitres 2-3 et 6-7, « Kalman and Bayesian Filters in Python » (Labbe, gratuit), Computer Vision: Algorithms and Applications (Szeliski, gratuit), cours CS231a.

Fiche de cours · Définitions

Perception, filtrage, vision : définitions

Définition (estimation d’état). Un système a un état caché x_t (position, vitesse) qui évolue selon x_t = f(x_t−1, u_t) + w_t (bruit de processus) et produit des mesures z_t = h(x_t) + v_t (bruit de mesure). Un filtre calcule la croyance bel(x_t) = p(x_t | z_1:t, u_1:t) récursivement.

Définition (filtre bayésien). Deux étapes : prédiction bel⁻(x_t) = ∫ p(x_t | x_t−1, u_t) bel(x_t−1) dx_t−1 ; correction bel(x_t) = η·p(z_t | x_t)·bel⁻(x_t). Kalman = filtre bayésien pour un système linéaire à bruits gaussiens ; EKF = linéarisation ; filtre particulaire = représentation par échantillons.

Définition (gaussienne multivariée). N(μ, Σ) de densité ∝ exp(−½(x − μ)ᵀΣ⁻¹(x − μ)). Σ (covariance, symétrique définie positive) décrit l’incertitude : ses vecteurs propres sont les axes de l’ellipse d’incertitude, ses valeurs propres les variances le long de ces axes. Stable par transformation affine : si x ∼ N(μ, Σ), Ax + b ∼ N(Aμ + b, AΣAᵀ).

Définition (filtre linéaire, convolution, réponse impulsionnelle). Un filtre linéaire invariant est une convolution y = h ∗ x. Passe-bas (moyenne glissante, exponentiel) lisse ; passe-haut (dérivée) détecte les variations. Le filtre médian n’est pas linéaire mais supprime les valeurs aberrantes.

Définition (image, gradient, contour, descripteur). Image = fonction I(x, y) (niveaux de gris) ou 3 canaux. Gradient ∇I = (∂I/∂x, ∂I/∂y) estimé par Sobel ; les contours sont les maxima de ‖∇I‖ (Canny). Un point d’intérêt (coin de Harris, FAST) est une position localement distinctive ; un descripteur (ORB, SIFT) résume son voisinage pour l’apparier entre images.

Définition (caméra sténopé, calibration). Un point 3D (X, Y, Z) se projette en (u, v) = (f_xX/Z + c_x, f_yY/Z + c_y) : matrice intrinsèque K. La calibration estime K et la distorsion (damier) ; l’extrinsèque [R | t] situe la caméra dans le monde.

Fiche de cours · Formules

Le filtre de Kalman et les formules de vision

Modèle : x_t = A x_t−1 + B u_t + w, w ∼ N(0, Q) ; z_t = H x_t + v, v ∼ N(0, R)

Prédiction : x̂⁻ = A x̂ + B u ; P⁻ = A P Aᵀ + Q

Correction : K = P⁻Hᵀ(H P⁻ Hᵀ + R)⁻¹ ; x̂ = x̂⁻ + K(z − H x̂⁻) ; P = (I − K H) P⁻ innovation z − Hx̂⁻ ; K ∈ [0, 1] pondère mesure vs prédiction selon leurs incertitudes

Cas scalaire : K = σ²_pred/(σ²_pred + σ²_mes) ; σ²_post = σ²_predσ²_mes/(σ²_pred + σ²_mes) — fusionner deux mesures indépendantes : 1/σ² = 1/σ₁² + 1/σ₂²

EKF : remplacer A par le jacobien F = ∂f/∂x et H par ∂h/∂x évalués en x̂ ; prédire avec f et h non linéaires

Filtre particulaire : poids w_i ∝ p(z | x_i) ; rééchantillonner quand N_eff = 1/Σw_i² < N/2

Filtre exponentiel : y_k = αx_k + (1 − α)y_k−1 ; constante de temps τ ≈ T_s(1 − α)/α ; fréquence de coupure f_c = 1/(2πτ)

Sobel : G_x = [[−1, 0, 1], [−2, 0, 2], [−1, 0, 1]] ∗ I ; ‖∇I‖ = √(G_x² + G_y²) ; orientation atan2(G_y, G_x)

Projection : u = f_x X/Z + c_x ; taille apparente ∝ 1/Z ; stéréo : Z = f·b/d (b : base, d : disparité en pixels)

Fiche de cours · Théorèmes et démonstrations

Démonstrations à savoir refaire (1/2)

Théorème 1 (fusion optimale de deux mesures gaussiennes). Deux estimations indépendantes x₁ ∼ N(μ₁, σ₁²) et x₂ ∼ N(μ₂, σ₂²) d’une même grandeur se combinent en μ = (σ₂²μ₁ + σ₁²μ₂)/(σ₁² + σ₂²), σ² = σ₁²σ₂²/(σ₁² + σ₂²) — la variance combinée est plus petite que chacune.

Par Bayes, la densité a posteriori est ∝ exp(−(x − μ₁)²/2σ₁²)·exp(−(x − μ₂)²/2σ₂²). L’exposant est un polynôme du second degré en x : −½[x²(1/σ₁² + 1/σ₂²) − 2x(μ₁/σ₁² + μ₂/σ₂²)] + const. On l’identifie à −(x − μ)²/2σ² : 1/σ² = 1/σ₁² + 1/σ₂² et μ/σ² = μ₁/σ₁² + μ₂/σ₂², d’où les formules (les précisions 1/σ² s’additionnent). Comme 1/σ² > 1/σ₁², σ < σ₁. Réécriture : μ = μ₁ + K(μ₂ − μ₁) avec K = σ₁²/(σ₁² + σ₂²) — c’est exactement la correction de Kalman scalaire : la mesure « tire » l’estimation d’autant plus qu’elle est précise par rapport à la prédiction.

Théorème 2 (le gain de Kalman minimise la variance a posteriori). Parmi les estimateurs linéaires x̂ = x̂⁻ + K(z − Hx̂⁻), le gain K = P⁻Hᵀ(HP⁻Hᵀ + R)⁻¹ minimise tr(P), la somme des variances des composantes.

Erreur e = x − x̂ = (I − KH)e⁻ − Kv avec e⁻ l’erreur de prédiction (covariance P⁻) et v le bruit de mesure (covariance R), indépendants. Donc P = (I − KH)P⁻(I − KH)ᵀ + KRKᵀ (forme de Joseph, valable pour tout K). Dérivons tr(P) par rapport à K : ∂tr(P)/∂K = −2(I − KH)P⁻Hᵀ + 2KR = 0 ⇔ K(HP⁻Hᵀ + R) = P⁻Hᵀ ⇔ K = P⁻Hᵀ(HP⁻Hᵀ + R)⁻¹. En substituant dans la forme de Joseph, on obtient P = (I − KH)P⁻. Le filtre de Kalman est donc l’estimateur linéaire de variance minimale ; si les bruits sont gaussiens, c’est aussi l’estimateur bayésien exact (la gaussienne reste gaussienne sous les opérations linéaires et le produit — Théorème 1).

Fiche de cours · Théorèmes et démonstrations

Démonstrations à savoir refaire (2/2)

Théorème 3 (propagation de la covariance). Si x ∼ N(μ, Σ) et y = Ax + b, alors Cov(y) = AΣAᵀ ; pour y = f(x) non linéaire, Cov(y) ≈ JΣJᵀ avec J = ∂f/∂x(μ).

Cov(y) = E[(y − E[y])(y − E[y])ᵀ] = E[A(x − μ)(x − μ)ᵀAᵀ] = AΣAᵀ. Non linéaire : f(x) ≈ f(μ) + J(x − μ) (Taylor à l’ordre 1), donc y − f(μ) ≈ J(x − μ) et on applique le cas linéaire. C’est l’étape de prédiction de Kalman (P⁻ = APAᵀ + Q) et le fondement de l’EKF ; l’approximation est mauvaise si f est très courbée à l’échelle de Σ (d’où le filtre « unscented » qui propage des points sigma).

Théorème 4 (la moyenne glissante est un passe-bas, et sa réponse en fréquence). La moyenne de N échantillons a pour gain |H(f)| = |sin(πNfT_s)/(N sin(πfT_s))| : 1 en f = 0, nul en f = k/(NT_s).

y_k = (1/N)Σ_j=0^N−1 x_k−j. Pour x_k = e^i2πfkT_s, y_k = x_k·(1/N)Σ_j e^−i2πfjT_s ; la somme géométrique vaut (1 − e^−i2πfNT_s)/(1 − e^−i2πfT_s), de module sin(πNfT_s)/sin(πfT_s). Le premier zéro est à f = 1/(NT_s) : moyenner N = 10 échantillons à 100 Hz annule exactement un parasite à 10 Hz (et ses harmoniques) — utile pour rejeter 50 Hz avec N = 20 à 1 kHz. Le bruit blanc de variance σ² est réduit à σ²/N (Théorème 2 de L11).

Théorème 5 (détecteur de Harris). Un pixel est un coin si la matrice M = Σ_voisinage [[I_x², I_xI_y], [I_xI_y, I_y²]] a deux valeurs propres grandes.

Le changement d’apparence pour un déplacement (u, v) est E(u, v) = Σ [I(x+u, y+v) − I(x, y)]² ≈ Σ (uI_x + vI_y)² = (u, v)M(u, v)ᵀ (Taylor à l’ordre 1). E est une forme quadratique ; ses valeurs propres λ₁, λ₂ mesurent la variation d’apparence dans les deux directions principales. Zone plate : λ₁ ≈ λ₂ ≈ 0. Contour : une seule grande (glisser le long du contour ne change rien). Coin : les deux grandes — le motif est localisable dans toutes les directions, donc appariable. Score de Harris : det(M) − k·tr(M)² = λ₁λ₂ − k(λ₁ + λ₂)², sans calculer les valeurs propres.

Fiche de cours · Méthodes

Méthodes et pièges

Méthode — concevoir un filtre de Kalman. (1) Choisir l’état (assez pour être markovien : position et vitesse). (2) Écrire A (physique : x += v·T_s), B, H (ce que mesure chaque capteur). (3) Q : incertitude sur le modèle (accélérations non modélisées : Q ∝ T_s²) ; R : variance des capteurs (fiche technique ou mesure à l’arrêt). (4) Vérifier sur simulation : l’erreur doit rester dans ±2√P (cohérence) ; sinon Q ou R est faux. (5) Tester la robustesse aux mesures aberrantes (seuil sur l’innovation normalisée, test du χ²).

Méthode — régler un filtre passe-bas. Identifier la bande utile du signal (un robot ne change pas de vitesse à 50 Hz) et les parasites ; choisir f_c entre les deux ; accepter la latence induite (τ ≈ 1/(2πf_c)) et la vérifier dans la boucle de contrôle (un retard trop grand déstabilise un PID).

Méthode — pipeline de vision classique. Niveaux de gris → flou gaussien (réduire le bruit) → gradient/contours ou seuillage (HSV pour la couleur) → morphologie (ouverture/fermeture) → composantes connexes → filtrage par aire/forme → mesure (centroïde, boîte). Toujours regarder les images intermédiaires.

Pièges : R trop petit (le filtre suit le bruit) ou Q trop petit (le filtre ignore les mesures, « confiance excessive ») ; angles non ramenés dans ]−π, π] dans l’innovation ; fusionner deux capteurs corrélés comme s’ils étaient indépendants (variance sous-estimée) ; seuils de couleur fixés sous un éclairage et testés sous un autre ; distorsion non corrigée avant toute mesure géométrique.

Fiche de cours · Exercices corrigés

Exercices corrigés

Exercice 1. Un robot avance sur un rail. État (x, v), T_s = 0,1 s, mesure de position par ultrason σ = 2 cm à chaque pas, bruit de processus sur l’accélération σ_a = 0,5 m/s². Écrire A, H, Q, R. Après une longue durée, la variance de position converge vers ≈ 1,2 cm² ; expliquer pourquoi elle est inférieure à la variance de mesure (4 cm²).

Correction. A = [[1, T_s], [0, 1]], H = [1, 0], R = [0,02²] = 4·10⁻⁴ m². Bruit de processus issu d’une accélération aléatoire a : Δx = ½aT_s², Δv = aT_s ⇒ Q = σ_a²·[[T_s⁴/4, T_s³/2], [T_s³/2, T_s²]] = 0,25·[[2,5·10⁻⁵, 5·10⁻⁴], [5·10⁻⁴, 10⁻²]]. La variance stationnaire est inférieure à R parce que le filtre combine chaque mesure avec la prédiction issue de toutes les mesures passées (Théorème 1 : les précisions s’ajoutent) ; elle ne tend pas vers 0 car Q réinjecte de l’incertitude à chaque pas — l’équilibre (équation de Riccati) dépend du rapport Q/R.

Exercice 2. Une caméra f = 800 px, c = (320, 240), à 1,2 m de haut, axe horizontal. Un objet est vu en (520, 400). Il est au sol : calculer sa position (X, Z) devant la caméra.

Correction. Rayon : (u − c_x, v − c_y, f) = (200, 160, 800). Le sol est le plan Y = 1,2 m (axe Y vers le bas). Le point est λ·(200, 160, 800) avec λ·160 = 1,2 ⇒ λ = 0,0075. Donc Z = 6,0 m, X = 1,5 m (à droite), Y = 1,2 m ✓. Sensibilité : ∂Z/∂v = −fh/(v − c_y)² = −800·1,2/160² = −3,75 cm par pixel — l’erreur croît comme Z² : à 6 m, un pixel vaut ~4 cm ; à 12 m, ~15 cm. D’où l’intérêt du lidar ou de la stéréo à distance.

Exercice 3. Un signal de gyroscope est intégré pour obtenir un angle. Avec un biais constant b = 0,01 rad/s et un bruit blanc σ = 0,02 rad/√s, donner l’erreur d’angle après 100 s, et expliquer comment un accéléromètre corrige le problème (filtre complémentaire).

Correction. Biais : erreur = b·t = 1 rad après 100 s (dérive linéaire). Bruit blanc intégré (marche aléatoire) : σ_θ = σ√t = 0,2 rad (croissance en √t). Le biais domine. L’accéléromètre donne l’inclinaison absolue (via la gravité) sans dérive mais avec beaucoup de bruit haute fréquence (accélérations du robot). Filtre complémentaire : θ = α(θ + ω·T_s) + (1 − α)θ_acc avec α ≈ 0,98 — le gyroscope filtré passe-haut (dérive éliminée) + l’accéléromètre filtré passe-bas (bruit éliminé), les deux filtres se complétant à 1. Un filtre de Kalman avec l’état (θ, b) estime en plus le biais explicitement.

01 / Filtrer

Le problème : une vraie mesure de capteur

import numpy as np
import matplotlib.pyplot as plt
rng = np.random.default_rng(0)
dt = 0.02; t = np.arange(0, 10, dt)                    # 50 Hz pendant 10 s
# Un robot qui accélère puis freine : vraie position et vitesse
acc_vraie = np.where(t < 4, 0.5, np.where(t < 7, 0, -0.6))
v_vraie = np.cumsum(acc_vraie) * dt; x_vraie = np.cumsum(v_vraie) * dt
# Capteurs : un télémètre (position, bruit 0.3 m, 10 Hz avec des sauts) et un accéléromètre (bruit 0.2 m/s², biais 0.05)
mesure_pos = x_vraie + rng.normal(0, 0.3, len(t)); mesure_pos[::5] += rng.normal(0, 0.8, len(t[::5])) * (rng.random(len(t[::5])) < 0.1)
dispo_pos = (np.arange(len(t)) % 5 == 0)               # disponible seulement à 10 Hz
mesure_acc = acc_vraie + 0.05 + rng.normal(0, 0.2, len(t))

# Deux approches naïves : n'utiliser QUE la position (bruyante) ou QUE l'accéléromètre (intégré deux fois : dérive)
x_integre = np.cumsum(np.cumsum(mesure_acc) * dt) * dt
fig, ax = plt.subplots(figsize=(8, 3.2))
ax.plot(t[dispo_pos], mesure_pos[dispo_pos], ".", ms=3, alpha=0.5, label="télémètre (10 Hz)")
ax.plot(t, x_integre, label="accéléromètre intégré 2× (dérive)")
ax.plot(t, x_vraie, "w--", lw=1, label="vérité"); ax.legend(); ax.set_ylabel("position (m)"); plt.show()
print(f"erreur RMS télémètre brut : {np.sqrt(np.mean((mesure_pos[dispo_pos] - x_vraie[dispo_pos])**2)):.2f} m ; accéléro intégré à t=10 s : {x_integre[-1] - x_vraie[-1]:+.2f} m")

Chaque capteur a sa faiblesse : le télémètre est bruyant mais ne dérive pas ; l’accéléromètre est précis à court terme mais son biais, intégré deux fois, donne une erreur en t². L’idée de la fusion : prendre le court terme de l’un et le long terme de l’autre.

01 / Filtrer

Filtres simples : passe-bas, médian, complémentaire

import numpy as np
import matplotlib.pyplot as plt
def passe_bas(x, alpha):
    y = np.zeros_like(x); y[0] = x[0]
    for k in range(1, len(x)): y[k] = y[k - 1] + alpha * (x[k] - y[k - 1])       # y += α (x − y)
    return y
def median_glissant(x, k=5):
    return np.array([np.median(x[max(0, i - k + 1):i + 1]) for i in range(len(x))])

pos10 = np.repeat(mesure_pos[dispo_pos], 5)[:len(t)]        # bloqueur d'ordre 0 : garder la dernière mesure
fig, axes = plt.subplots(1, 2, figsize=(10, 3.2))
for alpha in (0.3, 0.05):
    e = np.sqrt(np.mean((passe_bas(pos10, alpha) - x_vraie)**2))
    axes[0].plot(t, passe_bas(pos10, alpha), label=f"passe-bas α={alpha} (RMS {e:.2f})")
axes[0].plot(t, x_vraie, "w--", lw=1); axes[0].legend(fontsize=8); axes[0].set_title("passe-bas : lisse mais retarde")
med = median_glissant(pos10, 7); axes[1].plot(t, pos10, alpha=0.3, label="brut"); axes[1].plot(t, med, label="médian 7")
axes[1].plot(t, x_vraie, "w--", lw=1); axes[1].legend(fontsize=8); axes[1].set_title("médian : tue les sauts"); plt.show()

# Filtre complémentaire : haute fréquence de l'accéléromètre + basse fréquence du télémètre (la base des IMU pas chères)
x_c, v_c = 0.0, 0.0; xs = []
for k in range(len(t)):
    v_c += mesure_acc[k] * dt; x_c += v_c * dt                        # prédiction par l'accéléromètre
    if dispo_pos[k]:
        x_c += 0.05 * (mesure_pos[k] - x_c); v_c += 0.02 * (mesure_pos[k] - x_c)   # correction douce par la position
    xs.append(x_c)
print(f"complémentaire : RMS {np.sqrt(np.mean((np.array(xs) - x_vraie)**2)):.2f} m — mais les gains 0.05 et 0.02 sont choisis à la main. Kalman va les calculer.")

Fréquence de coupure et retard

Le passe-bas y += α(x − y) a une constante de temps τ ≈ dt/α : α = 0,05 à 50 Hz lisse sur 0,4 s et retarde d’autant — mortel pour un robot qui doit réagir. Le retard de phase est le prix de tout filtre linéaire causal ; c’est pourquoi on préfère un modèle (« le robot ne peut pas sauter de 1 m en 20 ms ») à un simple lissage. Le médian n’est pas linéaire : il supprime les valeurs aberrantes sans les étaler, mais ne réduit pas le bruit gaussien.

02 / Kalman

Le filtre de Kalman en 1D : prédire, puis corriger avec le bon gain

import numpy as np
# État : position x et vitesse v. Modèle : x' = x + v dt + ½ a dt², v' = v + a dt, avec l'accéléromètre comme entrée u = a.
# Incertitude : matrice de covariance P (2×2). Bruit de modèle Q (l'accéléro est imparfait), bruit de mesure R (télémètre).
F = np.array([[1, dt], [0, 1]]); B = np.array([0.5 * dt**2, dt]); H = np.array([[1, 0]])
Q = np.diag([1e-4, 1e-2]); R = np.array([[0.3**2]])
x = np.array([0.0, 0.0]); P = np.eye(2); estim, gains, sigmas = [], [], []
for k in range(len(t)):
    # Prédiction : propager l'état et l'incertitude
    x = F @ x + B * mesure_acc[k]; P = F @ P @ F.T + Q
    if dispo_pos[k]:
        # Correction : le gain de Kalman K pondère la mesure selon les incertitudes relatives
        S = H @ P @ H.T + R; K = P @ H.T @ np.linalg.inv(S)
        x = x + (K @ (mesure_pos[k] - H @ x)).ravel(); P = (np.eye(2) - K @ H) @ P
        gains.append(K[0, 0])
    estim.append(x.copy()); sigmas.append(np.sqrt(P[0, 0]))
estim = np.array(estim)
print(f"Kalman : RMS position {np.sqrt(np.mean((estim[:, 0] - x_vraie)**2)):.3f} m, RMS vitesse {np.sqrt(np.mean((estim[:, 1] - v_vraie)**2)):.3f} m/s")
print(f"gain K sur la position : {gains[0]:.2f} au début → {gains[-1]:.2f} en régime établi (il se règle tout seul)")
fig, ax = plt.subplots(figsize=(8, 3.2))
ax.fill_between(t, estim[:, 0] - 2 * np.array(sigmas), estim[:, 0] + 2 * np.array(sigmas), alpha=0.25, label="±2σ")
ax.plot(t[dispo_pos], mesure_pos[dispo_pos], ".", ms=3, alpha=0.4, label="mesures"); ax.plot(t, estim[:, 0], label="Kalman"); ax.plot(t, x_vraie, "w--", lw=1, label="vérité")
ax.legend(); plt.show()

Ce que fait chaque ligne

Prédiction : x ← Fx + Bu (la physique), P ← FPFᵀ + Q (l’incertitude grandit : on n’a pas mesuré). Correction : innovation y = z − Hx (ce que la mesure dit de neuf), S = HPHᵀ + R (incertitude de l’innovation), gain K = PHᵀS⁻¹ (confiance dans la mesure relativement au modèle), x ← x + Ky, P ← (I − KH)P (l’incertitude diminue). Si R est grand (capteur mauvais), K → 0 et on ignore la mesure ; si P est grand (on est perdu), K → 1 et on croit la mesure. C’est le filtre bayésien du module L11 pour des gaussiennes et un modèle linéaire, et il est optimal dans ce cas (Kalman, 1960 — il a guidé Apollo). Le module L10 donne les outils : produits matriciels, inverses (petites), covariances.

02 / Kalman

Régler Q et R, détecter les aberrations, tester la cohérence

import numpy as np
def kalman(Q, R, rejet=None):
    x = np.array([0.0, 0.0]); P = np.eye(2); estim, nis = [], []
    for k in range(len(t)):
        x = F @ x + B * mesure_acc[k]; P = F @ P @ F.T + Q
        if dispo_pos[k]:
            S = H @ P @ H.T + R; y = mesure_pos[k] - (H @ x)[0]
            d2 = y**2 / S[0, 0]                                 # distance de Mahalanobis² de l'innovation
            nis.append(d2)
            if rejet is None or d2 < rejet:                     # test du χ² à 1 degré de liberté : 3.84 pour 95 %
                K = P @ H.T @ np.linalg.inv(S); x = x + (K * y).ravel(); P = (np.eye(2) - K @ H) @ P
        estim.append(x.copy())
    estim = np.array(estim)
    return np.sqrt(np.mean((estim[:, 0] - x_vraie)**2)), np.mean(nis)

print(" Q_v     R      RMS(m)  NIS moyen (≈1 si le filtre est cohérent)")
for qv, r in [(1e-2, 0.3**2), (1e-4, 0.3**2), (1, 0.3**2), (1e-2, 0.05**2), (1e-2, 1.0**2)]:
    rms, n = kalman(np.diag([1e-4, qv]), np.array([[r]]))
    print(f"{qv:6.0e} {np.sqrt(r):5.2f}   {rms:.3f}    {n:.2f}")
rms, n = kalman(np.diag([1e-4, 1e-2]), np.array([[0.3**2]]), rejet=9.0)
print(f"\navec rejet des innovations > 3σ : RMS {rms:.3f} (les sauts du télémètre sont ignorés)")

Le NIS (normalized innovation squared) moyen doit valoir ≈ 1 : s’il est ≫ 1, le filtre est trop sûr de lui (Q ou R sous-estimés) ; ≪ 1, trop prudent. C’est le test standard de cohérence. R se mesure sur le capteur immobile ; Q se règle pour obtenir NIS ≈ 1. La distance de Mahalanobis rejette les mesures impossibles selon le filtre lui-même — bien mieux qu’un seuil fixe.

02 / Kalman

EKF : quand le modèle n’est pas linéaire — un robot différentiel

import numpy as np
import matplotlib.pyplot as plt
rng = np.random.default_rng(2)
# État (x, y, θ). Commande (v, ω) mesurée par odométrie (bruitée). Mesure : distance et angle vers une balise fixe (bruités).
balise = np.array([5.0, 3.0]); dt = 0.1; N = 300
def f(s, u):                                              # modèle de mouvement (non linéaire : cos, sin)
    x, y, th = s; v, w = u
    return np.array([x + v * dt * np.cos(th), y + v * dt * np.sin(th), th + w * dt])
def h(s):                                                 # modèle de mesure : distance et angle relatif
    dx, dy = balise - s[:2]
    return np.array([np.hypot(dx, dy), np.arctan2(dy, dx) - s[2]])
def jac_f(s, u):
    x, y, th = s; v, w = u
    return np.array([[1, 0, -v * dt * np.sin(th)], [0, 1, v * dt * np.cos(th)], [0, 0, 1]])
def jac_h(s):
    dx, dy = balise - s[:2]; q = dx**2 + dy**2
    return np.array([[-dx / np.sqrt(q), -dy / np.sqrt(q), 0], [dy / q, -dx / q, -1]])
angle = lambda a: (a + np.pi) % (2 * np.pi) - np.pi     # ramener dans ]−π, π] : indispensable pour les angles

vrai = np.array([0.0, 0.0, 0.0]); est = vrai.copy(); P = np.eye(3) * 0.1
Q = np.diag([0.01, 0.01, 0.005]); R = np.diag([0.2**2, 0.05**2])
traj_v, traj_e, traj_o = [], [], []; odo = vrai.copy()
for k in range(N):
    u = np.array([1.0, 0.4 * np.sin(k * dt)])
    vrai = f(vrai, u + rng.normal(0, [0.05, 0.02]))       # le vrai robot glisse un peu
    u_mes = u + rng.normal(0, [0.05, 0.02]); odo = f(odo, u_mes)       # odométrie seule : dérive
    # EKF prédiction
    Fk = jac_f(est, u_mes); est = f(est, u_mes); P = Fk @ P @ Fk.T + Q
    # EKF correction (balise vue à 5 Hz)
    if k % 2 == 0:
        z = h(vrai) + rng.normal(0, [0.2, 0.05]); Hk = jac_h(est)
        y = z - h(est); y[1] = angle(y[1])
        S = Hk @ P @ Hk.T + R; K = P @ Hk.T @ np.linalg.inv(S)
        est = est + K @ y; est[2] = angle(est[2]); P = (np.eye(3) - K @ Hk) @ P
    traj_v.append(vrai.copy()); traj_e.append(est.copy()); traj_o.append(odo.copy())
traj_v, traj_e, traj_o = map(np.array, (traj_v, traj_e, traj_o))
print(f"erreur finale odométrie seule : {np.hypot(*(traj_o[-1, :2] - traj_v[-1, :2])):.2f} m | EKF : {np.hypot(*(traj_e[-1, :2] - traj_v[-1, :2])):.2f} m")
plt.figure(figsize=(6, 4)); plt.plot(*traj_v[:, :2].T, "w--", lw=1, label="vérité"); plt.plot(*traj_o[:, :2].T, label="odométrie"); plt.plot(*traj_e[:, :2].T, label="EKF")
plt.plot(*balise, "y*", ms=12, label="balise"); plt.axis("equal"); plt.legend(); plt.show()

Linéariser autour de l’estimation

L’EKF remplace F et H par les jacobiennes (module L10) de f et h évaluées à l’estimation courante : le filtre reste celui de Kalman, appliqué à l’approximation linéaire locale. Il n’est plus optimal ni garanti (il peut diverger si la linéarisation est mauvaise), mais il est le cheval de bataille de la robotique : GPS/INS, SLAM (module L20), fusion IMU. Alternatives : UKF (points sigma, sans jacobienne), filtre particulaire (module L20). Le piège n°1 : oublier de ramener les angles dans ]−π, π] — l’innovation « 359° » au lieu de « −1° » fait exploser le filtre.

03 / IMU

Attitude à partir d’une IMU : gyroscope + accéléromètre

import numpy as np
import matplotlib.pyplot as plt
rng = np.random.default_rng(3)
dt = 0.01; t = np.arange(0, 20, dt)
# Vrai angle de tangage (pitch) d'un robot qui bascule, et vraie vitesse angulaire
pitch = 0.4 * np.sin(0.5 * t) + 0.1 * np.sin(3 * t); omega = np.gradient(pitch, dt)
# Gyro : vitesse angulaire + biais (dérive lente) + bruit. Accéléro : gravité projetée + accélérations parasites + bruit
gyro = omega + 0.02 + 0.005 * t + rng.normal(0, 0.05, len(t))
acc_x = -9.81 * np.sin(pitch) + rng.normal(0, 0.5, len(t)) + 2.0 * (np.abs(np.sin(2 * t)) > 0.95)   # secousses
acc_z = 9.81 * np.cos(pitch) + rng.normal(0, 0.5, len(t))
pitch_acc = np.arctan2(-acc_x, acc_z)                     # angle d'après la gravité seule : bruyant, sans dérive
pitch_gyro = np.cumsum(gyro) * dt                         # intégration du gyro : lisse, mais dérive

# Filtre complémentaire : 98 % gyro (haute fréquence), 2 % accéléro (basse fréquence)
pc = np.zeros(len(t))
for k in range(1, len(t)): pc[k] = 0.98 * (pc[k - 1] + gyro[k] * dt) + 0.02 * pitch_acc[k]

# Kalman avec estimation du biais : état (angle, biais gyro), entrée = gyro
x = np.zeros(2); P = np.eye(2); F = np.array([[1, -dt], [0, 1]]); B = np.array([dt, 0]); H = np.array([[1, 0]])
Q = np.diag([1e-5, 1e-7]); R = np.array([[0.05]]); pk = np.zeros(len(t)); biais = np.zeros(len(t))
for k in range(len(t)):
    x = F @ x + B * gyro[k]; P = F @ P @ F.T + Q
    S = H @ P @ H.T + R; K = P @ H.T / S; x = x + (K * (pitch_acc[k] - x[0])).ravel(); P = (np.eye(2) - K @ H) @ P
    pk[k], biais[k] = x
rms = lambda e: np.degrees(np.sqrt(np.mean((e - pitch)**2)))
print(f"RMS en degrés — accéléro seul : {rms(pitch_acc):.2f}, gyro seul : {rms(pitch_gyro):.2f}, complémentaire : {rms(pc):.2f}, Kalman : {rms(pk):.2f}")
print(f"biais gyro estimé à la fin : {biais[-1]:.3f} rad/s (vrai : {0.02 + 0.005 * t[-1]:.3f})")
plt.figure(figsize=(8, 3)); plt.plot(t, np.degrees(pitch_gyro), label="gyro intégré"); plt.plot(t, np.degrees(pitch_acc), alpha=0.3, label="accéléro")
plt.plot(t, np.degrees(pk), label="Kalman"); plt.plot(t, np.degrees(pitch), "w--", lw=1, label="vérité"); plt.legend(fontsize=8); plt.ylabel("tangage (°)"); plt.show()

Vers la 3D

En 3D, l’attitude est une rotation (module L10) : on la représente par un quaternion (4 nombres, pas de blocage de cardan), on l’intègre avec le gyro 3 axes et on la corrige avec l’accéléromètre (direction du bas) et le magnétomètre (direction du nord). Les filtres de Madgwick et Mahony font cela en quelques dizaines de lignes sur un microcontrôleur — ce sont ceux des contrôleurs de vol de drones (Betaflight, PX4). Étapes obligatoires : calibration du gyro (biais au repos), de l’accéléromètre (6 positions) et du magnétomètre (ellipsoïde). Une IMU à 5 € bien calibrée et bien filtrée bat une IMU à 500 € mal utilisée.

04 / Vision

Une image est une matrice : gradients, seuils, contours

import numpy as np
import matplotlib.pyplot as plt
rng = np.random.default_rng(4)
# Image synthétique 80×120 : sol gris, une ligne noire à suivre (courbe), un obstacle clair, bruit
H, W = 80, 120; y, x = np.mgrid[0:H, 0:W]
ligne_x = 60 + 25 * np.sin(y / 20)
img = 0.55 + 0.05 * rng.normal(0, 1, (H, W))
img[np.abs(x - ligne_x) < 4] = 0.1                                  # la ligne
img[(x - 95)**2 + (y - 25)**2 < 100] = 0.95                         # un obstacle
img = np.clip(img, 0, 1)

def conv(img, k):                                                    # convolution 2D (module L14), bords répliqués
    p = k.shape[0] // 2; ip = np.pad(img, p, mode="edge"); out = np.zeros_like(img)
    for i in range(k.shape[0]):
        for j in range(k.shape[1]): out += k[i, j] * ip[i:i + H, j:j + W]
    return out
gauss = np.outer([1, 4, 6, 4, 1], [1, 4, 6, 4, 1]) / 256.0
sobel_x = np.array([[-1, 0, 1], [-2, 0, 2], [-1, 0, 1]]); sobel_y = sobel_x.T
lisse = conv(img, gauss); gx, gy = conv(lisse, sobel_x), conv(lisse, sobel_y)
grad = np.hypot(gx, gy)
contours = grad > 0.15                                               # seuil (Canny fait mieux : hystérésis + amincissement)
seuil_ligne = lisse < 0.3                                            # segmentation par seuil d'intensité

fig, axes = plt.subplots(1, 4, figsize=(11, 2.4))
for ax, im, titre in zip(axes, [img, grad, contours, seuil_ligne], ["image", "norme du gradient", "contours", "seuil : la ligne"]):
    ax.imshow(im, cmap="gray"); ax.set_title(titre); ax.axis("off")
plt.show()
# Suivi de ligne : centre de la ligne dans la rangée du bas → erreur pour le PID (séance 29)
rangee = seuil_ligne[H - 5]; cols = np.where(rangee)[0]
print("centre de la ligne en bas :", cols.mean() if len(cols) else None, "| centre image :", W / 2, "→ erreur =", round(cols.mean() - W / 2, 1), "px")

Le vocabulaire

Lissage gaussien : réduit le bruit avant de dériver (dériver amplifie le bruit). Gradient (Sobel) : où l’intensité change vite = contours. Seuillage : le plus simple des segmentations ; Otsu choisit le seuil automatiquement par l’histogramme. Morphologie (érosion, dilatation) nettoie les masques. Composantes connexes (module L05 : BFS sur les pixels) isolent les objets. Tout cela existe dans OpenCV (cv2.GaussianBlur, cv2.Sobel, cv2.threshold, cv2.findContours) en C++ optimisé, y compris sur Raspberry Pi à 30 images/s.

04 / Vision

Détecter des droites (Hough) et comprendre la caméra (sténopé, homographie)

import numpy as np
# Transformée de Hough : chaque pixel de contour vote pour toutes les droites (ρ, θ) qui passent par lui
ys, xs = np.where(contours[:, :70])                                   # partie gauche : la ligne courbe, localement droite
thetas = np.deg2rad(np.arange(-90, 90, 2)); rho_max = int(np.hypot(H, W)); acc = np.zeros((2 * rho_max, len(thetas)), int)
for xi, yi in zip(xs, ys):
    rhos = (xi * np.cos(thetas) + yi * np.sin(thetas)).astype(int) + rho_max
    acc[rhos, np.arange(len(thetas))] += 1
r, c = np.unravel_index(acc.argmax(), acc.shape)
print(f"droite dominante : ρ = {r - rho_max}, θ = {np.degrees(thetas[c]):.0f}° ({acc.max()} votes)")

# Modèle sténopé : un point 3D (X, Y, Z) dans le repère caméra se projette en pixel (u, v) = (fx X/Z + cx, fy Y/Z + cy)
K = np.array([[500, 0, 320], [0, 500, 240], [0, 0, 1]])              # matrice intrinsèque (f = 500 px, centre 320×240)
def projeter(P): p = K @ P; return p[:2] / p[2]
print("point à 2 m devant, 0.5 m à droite :", projeter(np.array([0.5, 0.0, 2.0])).round(1), "px")
print("le même à 4 m :", projeter(np.array([0.5, 0.0, 4.0])).round(1), "px  → la profondeur est perdue : un pixel = un rayon, pas un point")

# Homographie : si tous les points sont sur un plan (le sol !), pixel ↔ position au sol par une matrice 3×3.
# On l'estime à partir de 4 correspondances connues (un damier posé au sol) : c'est la « vue d'oiseau » des robots suiveurs.
sol = np.array([[0, 0], [1, 0], [1, 1], [0, 1]], float)              # 4 coins d'un carré de 1 m au sol
pix = np.array([[300, 400], [340, 400], [335, 300], [305, 300]], float)   # leurs pixels (perspective : le haut est plus étroit)
A = []
for (X, Y), (u, v) in zip(sol, pix):
    A += [[X, Y, 1, 0, 0, 0, -u * X, -u * Y, -u], [0, 0, 0, X, Y, 1, -v * X, -v * Y, -v]]
_, _, Vt = np.linalg.svd(np.array(A)); Hm = Vt[-1].reshape(3, 3)     # solution du système homogène : dernier vecteur singulier (L10)
Hinv = np.linalg.inv(Hm)
def pixel_vers_sol(u, v): p = Hinv @ [u, v, 1]; return (p[:2] / p[2]).round(3)
print("pixel (320, 350) → sol", pixel_vers_sol(320, 350), "m ; pixel (320, 400) →", pixel_vers_sol(320, 400))

De la calibration à la 3D

La calibration (Zhang, 2000) estime K et la distorsion de l’objectif à partir de photos d’un damier : cv2.calibrateCamera. Avec deux caméras (stéréo) ou une caméra en mouvement (structure from motion), on retrouve la profondeur par triangulation ; avec un motif projeté ou un temps de vol, les caméras 3D (RealSense, Kinect) la mesurent directement. Les réseaux convolutifs (module L14) ont remplacé les détecteurs à la main pour reconnaître des objets (YOLO en temps réel sur Jetson), mais géométrie et filtrage restent indispensables pour passer des détections aux positions et aux commandes.

Cours

Cours 1 — Caractériser un capteur : les mots exacts et comment les mesurer

Caractéristique	Définition	Comment la mesurer	Comment la traiter
Bruit (précision)	Dispersion des lectures à entrée constante ; écart-type σ	1000 lectures immobiles ; histogramme ; σ	Moyenne, Kalman (R = σ²)
Biais (justesse)	Écart entre la moyenne et la vraie valeur	Comparer à une référence connue	Calibration (soustraire), ou estimer le biais dans le filtre
Dérive	Biais qui évolue (température, temps)	Lectures sur 1 h ; tendance ; test à plusieurs températures	Modèle de biais dans l’état (IMU), recalibration, fusion avec un capteur sans dérive
Non-linéarité	Réponse ≠ droite (capteur IR : ∝ 1/d)	Courbe d’étalonnage sur toute la plage	Modèle inverse (L10, moindres carrés), table de correspondance
Résolution	Plus petit changement détectable (LSB de l’ADC)	Fiche technique ; bits de l’ADC	Sur-échantillonnage (+1 bit pour ×4 échantillons)
Latence / bande passante	Retard et fréquence max suivie	Échelon (choc, mouvement brusque) ; réponse	Compenser dans le filtre (retard connu) ; ne pas filtrer plus que nécessaire
Aberrations	Valeurs impossibles occasionnelles (échos, multi-trajets)	Fréquence et amplitude sur un long enregistrement	Médiane, rejet par Mahalanobis (L19 §02)

Loi d’Allan (pour gyros et horloges). La variance d’Allan σ²(τ) mesure la stabilité en fonction du temps de moyennage τ : pente −1/2 (bruit blanc, la moyenne aide), plateau (instabilité du biais : moyenner plus ne sert à rien), pente +1/2 (marche aléatoire). Elle dit exactement combien de temps il est utile de moyenner un gyro — et donne les paramètres Q du Kalman.

Cours

Cours 2 — Exemple travaillé : dériver le gain de Kalman 1D et comprendre chaque terme

Situation. Avant la mesure, on croit x ∼ N(μ₀, σ₀²) (prédiction). Le capteur donne z = x + bruit, bruit ∼ N(0, r²). Quelle est la meilleure estimation après ?

Bayes : p(x | z) ∝ p(z | x) p(x) ∝ exp(−(z − x)²/2r²) · exp(−(x − μ₀)²/2σ₀²). Le produit de deux gaussiennes est une gaussienne ; en développant l’exposant et en identifiant : 1/σ² = 1/σ₀² + 1/r² (les précisions s’ajoutent) et μ = σ²(μ₀/σ₀² + z/r²) (moyenne pondérée par les précisions).

Forme « gain » : posons K = σ₀²/(σ₀² + r²). Alors μ = μ₀ + K(z − μ₀) et σ² = (1 − K)σ₀². Trois lectures : (1) K ∈ [0, 1] pondère l’innovation z − μ₀ ; (2) si r → 0 (capteur parfait) K → 1, on prend la mesure ; si σ₀ → 0 (prédiction sûre) K → 0, on l’ignore ; (3) la variance après est toujours plus petite qu’avant : une mesure, même mauvaise, informe. En dimension n, K = PHᵀ(HPHᵀ + R)⁻¹ est exactement cette formule avec des matrices.

Cours

Cours 3 — Vision : de la géométrie de la caméra aux caractéristiques

Étape	Ce qu’elle fait	Outils
Modèle de caméra	Point 3D → pixel : K [R \| t] X (intrinsèques K, pose extrinsèque)	Calibration par damier (`cv2.calibrateCamera`) ; distorsion radiale k₁, k₂ à corriger (`undistort`)
Prétraitement	Niveaux de gris, flou gaussien, égalisation d’histogramme, seuils adaptatifs	`cvtColor, GaussianBlur, equalizeHist, adaptiveThreshold`
Caractéristiques locales	Points d’intérêt répétables (coins, blobs) + descripteurs invariants (rotation, échelle)	Harris, FAST ; ORB (rapide, libre), SIFT ; appariement par distance de Hamming + ratio test
Géométrie à deux vues	Homographie (plan), matrice essentielle (mouvement de caméra), triangulation	`findHomography` avec RANSAC (rejette les faux appariements), `findEssentialMat`, `recoverPose`
Détection d’objets	Boîtes + classes en temps réel	YOLO (réseau convolutif, L14) ; sur Jetson/Coral ; ArUco pour des marqueurs (pose 6D en quelques ms)
Segmentation	Classe par pixel (route, obstacle)	U-Net, SAM ; ou couleur + morphologie pour les cas simples

import numpy as np
rng = np.random.default_rng(1)
# RANSAC pour ajuster une droite parmi 40 % de points aberrants : l'idée derrière findHomography(…, RANSAC)
n = 200; x = rng.uniform(0, 10, n); y = 2 * x + 1 + rng.normal(0, 0.2, n)
aber = rng.random(n) < 0.4; y[aber] = rng.uniform(-5, 30, aber.sum())
def ransac_droite(x, y, iters=200, seuil=0.5):
    meilleur, n_in = None, 0
    for _ in range(iters):
        i, j = rng.choice(len(x), 2, replace=False)
        if x[i] == x[j]: continue
        a = (y[j] - y[i]) / (x[j] - x[i]); b = y[i] - a * x[i]
        inliers = np.abs(y - (a * x + b)) < seuil
        if inliers.sum() > n_in: n_in, meilleur = inliers.sum(), inliers
    a, b = np.polyfit(x[meilleur], y[meilleur], 1)            # raffiner sur les inliers
    return a, b, n_in
a_mc, b_mc = np.polyfit(x, y, 1); a_r, b_r, n_in = ransac_droite(x, y)
print(f"moindres carrés (tirés par les aberrants) : y = {a_mc:.2f} x + {b_mc:.2f}")
print(f"RANSAC : y = {a_r:.2f} x + {b_r:.2f} avec {n_in} inliers (vrai : 2 x + 1)")

RANSAC (« RANdom SAmple Consensus », 1981) est l’algorithme le plus utilisé de la vision géométrique : tirer le minimum de points pour un modèle, compter ceux qui sont d’accord, garder le meilleur, raffiner. Il tolère 50 % d’aberrations et plus — ce que les moindres carrés ne supportent pas du tout.

TP guidé

TP — Fusion IMU réelle et vision avec OpenCV (sur PC + carte, 3 h)

Acquisition. Un MPU-6050 (ou l’IMU d’un téléphone via une appli d’export) : enregistrez 5 minutes à 100 Hz, immobile puis en mouvement, en CSV (t, ax, ay, az, gx, gy, gz). Caractérisez (cours 1) : σ de chaque axe immobile, biais gyro, variance d’Allan (au moins 10 min immobile pour voir le plateau).
Calibration accéléro. Six positions (chaque axe vers le haut/bas) : ajustez gain et offset par axe par moindres carrés (L10) pour que la norme au repos soit 9,81 m/s² ; vérifiez sur un enregistrement à 45°.
Attitude. Filtre complémentaire puis Kalman (angle + biais) hors ligne sur l’enregistrement ; réglez R par σ mesuré et Q par le NIS ; comparez les deux sur les secousses. Puis portez le filtre en C sur la carte (virgule flottante suffit sur Cortex-M4 ; virgule fixe sur AVR) et affichez l’angle à 100 Hz sur le port série : latence ? gigue ?
Vision. pip install opencv-python. Webcam : (a) calibration avec un damier imprimé (20 images) ; affichez K et la distorsion ; (b) détection de marqueurs ArUco (cv2.aruco) et pose 6D en temps réel ; (c) suivi de ligne : seuil adaptatif, morphologie, centre de la ligne, homographie vers le sol ; (d) ORB + matching + homographie RANSAC entre deux images du bureau : tracez les correspondances et les inliers.
Fusion vision + IMU (bonus). La caméra donne l’orientation d’un marqueur à 15 Hz avec du bruit ; l’IMU à 100 Hz avec de la dérive : Kalman qui prédit avec le gyro et corrige avec le marqueur. Tracez les trois courbes.
Livrable. Dépôt : données brutes, scripts de caractérisation (tableau σ/biais/Allan), calibration, filtres avec courbes, firmware C, scripts OpenCV avec captures ; RESULTATS.md.

Exercices

Exercices auto-corrigés — filtres

Exercice 1 — Filtre de Kalman générique et test de cohérence

Écrivez une classe Kalman(F, B, H, Q, R, x0, P0) avec predire(u), corriger(z) (renvoie l’innovation normalisée y²/S) et une fonction nis_moyen(kf, mesures, commandes). Vérifiez sur un système 1D vitesse constante que : la variance P converge (régime établi), le NIS moyen est ≈ 1 quand Q et R sont justes, et > 1 si R est déclaré 10× trop petit.

Correction

class Kalman:
    def __init__(self, F, B, H, Q, R, x0, P0): self.F, self.B, self.H, self.Q, self.R, self.x, self.P = F, B, H, Q, R, x0.astype(float), P0.astype(float)
    def predire(self, u): self.x = self.F @ self.x + (self.B @ u).ravel(); self.P = self.F @ self.P @ self.F.T + self.Q
    def corriger(self, z):
        y = z - self.H @ self.x; S = self.H @ self.P @ self.H.T + self.R; K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y; self.P = (np.eye(len(self.x)) - K @ self.H) @ self.P
        return float(y @ np.linalg.solve(S, y))
def nis_moyen(kf, mesures, commandes):
    return float(np.mean([(kf.predire(u), kf.corriger(z))[1] for z, u in zip(mesures, commandes)][50:]))

Exercice 2 — Filtre médian rapide et détecteur de sauts

a) median_glissant(x, k) en O(n·k) simple, puis vérifiez contre une version naïve. b) detecter_sauts(x, k, seuil) renvoie les indices où |x[i] − médiane locale| > seuil × MAD local (MAD = médiane des écarts absolus, un σ robuste : σ ≈ 1,4826 MAD).

Correction

def median_glissant(x, k):
    h = k // 2; xp = np.pad(x, h, mode="edge"); return np.array([np.median(xp[i:i + k]) for i in range(len(x))])
def detecter_sauts(x, k=15, seuil=5.0):
    m = median_glissant(x, k); e = np.abs(x - m); mad = np.median(e) * 1.4826 + 1e-9      # MAD global : écart-type robuste
    return [int(i) for i in np.where(e > seuil * mad)[0]]

Exercices

Exercices auto-corrigés — capteurs et images

Exercice 3 — Calibration d’un capteur non linéaire et propagation d’incertitude

Un capteur IR donne une tension v ≈ a/(d + b) + c. À partir de mesures (d, v) bruitées : a) ajustez (a, b, c) par Gauss-Newton ou scipy.optimize.least_squares si disponible, sinon par une grille sur b + moindres carrés linéaires sur (a, c) ; b) écrivez distance(v) (modèle inverse) ; c) propagez l’incertitude : si v a un écart-type σ_v, quel est l’écart-type de d à 10 cm et à 60 cm ? (dérivée numérique du modèle inverse). Concluez sur la plage utile.

Correction

def ajuster(d, v):
    best = None
    for b in np.linspace(0, 10, 201):
        X = np.column_stack([1 / (d + b), np.ones_like(d)]); (a, c), res, *_ = np.linalg.lstsq(X, v, rcond=None)
        err = ((X @ [a, c] - v)**2).sum()
        if best is None or err < best[0]: best = (err, a, b, c)
    return best[1:]
def distance(v, a, b, c): return a / (v - c) - b
def incertitude_distance(v, a, b, c, sigma_v=0.02, h=1e-4):
    return abs(distance(v + h, a, b, c) - distance(v - h, a, b, c)) / (2 * h) * sigma_v

Exercice 4 — Composantes connexes et centroïdes d’une image binaire

composantes(masque) étiquette les composantes 4-connexes d’une image booléenne (BFS, L05) et renvoie une liste de (aire, centroïde (ligne, colonne)) triée par aire décroissante ; plus_gros_objet(masque, aire_min) renvoie le centroïde du plus gros ou None. C’est le détecteur d’obstacle/balle le plus simple.

Correction

from collections import deque
def composantes(masque):
    H, W = masque.shape; vu = np.zeros_like(masque); out = []
    for i in range(H):
        for j in range(W):
            if masque[i, j] and not vu[i, j]:
                vu[i, j] = True; file = deque([(i, j)]); pts = []
                while file:
                    a, b = file.popleft(); pts.append((a, b))
                    for c, d in ((a+1, b), (a-1, b), (a, b+1), (a, b-1)):
                        if 0 <= c < H and 0 <= d < W and masque[c, d] and not vu[c, d]: vu[c, d] = True; file.append((c, d))
                out.append((len(pts), (float(np.mean([p[0] for p in pts])), float(np.mean([p[1] for p in pts])))))
    return sorted(out, key=lambda c: -c[0])
def plus_gros_objet(masque, aire_min=5):
    c = composantes(masque); return c[0][1] if c and c[0][0] >= aire_min else None

05 / Défis

Défi ★ — Régler un filtre de Kalman comme un ingénieur

Consigne

1) Simulez un capteur de température (vraie valeur : rampe puis palier) lu à 1 Hz avec bruit σ = 0,5 et un saut aberrant de +8 °C à t = 40 s. 2) Kalman 1D avec état (température, pente) ; mesurez R sur 30 s de palier ; réglez Q pour NIS ≈ 1 ; ajoutez le rejet d’aberration par Mahalanobis. 3) Comparez au passe-bas et au médian : tracez, et donnez pour chacun le retard (temps pour atteindre 90 % d’un échelon) et le RMS. 4) Rédigez en 10 lignes votre procédure de réglage, réutilisable pour n’importe quel capteur.

Piste

Procédure : (1) capteur immobile → R = variance mesurée ; (2) choisir le modèle (constant, pente, accélération) selon la physique ; (3) Q petit → filtre lent mais lisse ; augmenter jusqu’à NIS ≈ 1 ; (4) seuil de rejet χ² à 3σ ; (5) vérifier sur un échelon réel que le retard est acceptable ; (6) enregistrer les données brutes et rejouer hors ligne à chaque changement de réglage.

05 / Défis

Défi ★★ — Suiveur de ligne par vision, simulé de bout en bout

Consigne

Construisez une boucle complète : 1) un « monde » : une ligne courbe sur un plan (fonction y(x)) ; 2) une caméra virtuelle sur le robot (position, cap) qui produit une image 40×60 du sol devant lui (utilisez l’homographie inverse pour échantillonner le plan) ; 3) le détecteur de ligne du module (seuil, centre de la rangée du bas, et aussi celle du milieu pour anticiper) ; 4) un PID (séance 29) qui commande la vitesse angulaire ; 5) le modèle de mouvement différentiel (EKF slide) ; 6) 500 pas de simulation, tracé de la trajectoire par-dessus la ligne. Ajoutez du bruit d’image et un flou de mouvement : jusqu’à quel bruit le suiveur tient-il ?

Piste (caméra virtuelle)

def image_sol(pose, ligne, h=40, w=60, portee=(0.1, 0.6), largeur=0.4):
    """Chaque pixel (i, j) regarde un point du sol devant le robot ; on lit la couleur du monde à cet endroit."""
    x, y, th = pose; img = np.full((h, w), 0.6)
    for i in range(h):
        d = portee[1] - (portee[1] - portee[0]) * i / h                  # rangée du haut = loin
        for j in range(w):
            lat = (j - w / 2) / w * largeur * (d / portee[0])              # perspective : plus loin = plus large
            X = x + d * np.cos(th) - lat * np.sin(th); Y = y + d * np.sin(th) + lat * np.cos(th)
            if abs(Y - ligne(X)) < 0.02: img[i, j] = 0.1
    return img

05 / Défis

Défi ★★★ — Esprit prépa : dériver Kalman et implémenter un UKF

Consigne

1) Démontrez la formule du gain de Kalman en 1D : deux estimations gaussiennes indépendantes (μ₁, σ₁²) et (μ₂, σ₂²) de la même quantité ; montrez que la fusion optimale (au sens du maximum de vraisemblance ou de Bayes) est μ = (σ₂² μ₁ + σ₁² μ₂)/(σ₁² + σ₂²) avec 1/σ² = 1/σ₁² + 1/σ₂², et identifiez K. 2) Généralisez : montrez que K = PHᵀ(HPHᵀ + R)⁻¹ minimise la trace de la covariance a posteriori. 3) Implémentez le filtre de Kalman sans parfum (UKF) : 2n + 1 points sigma propagés par f et h, sans jacobienne ; comparez à l’EKF sur le robot différentiel avec un bruit fort (l’EKF diverge, l’UKF non ?). 4) Lisez le résumé de l’article original de Kalman (1960) et expliquez pourquoi il a été refusé par une revue avant d’être publié.

Piste (UKF)

def points_sigma(x, P, kappa=0.0):
    n = len(x); L = np.linalg.cholesky((n + kappa) * P)
    pts = [x] + [x + L[:, i] for i in range(n)] + [x - L[:, i] for i in range(n)]
    w = np.full(2 * n + 1, 1 / (2 * (n + kappa))); w[0] = kappa / (n + kappa)
    return np.array(pts), w
# Prédiction : propager chaque point par f, moyenne et covariance pondérées + Q.
# Correction : propager par h, covariance croisée Pxz, K = Pxz Pzz⁻¹. Attention aux angles dans les moyennes (moyenne circulaire).

06 / Vérification

Dans un filtre de Kalman, si on augmente R (bruit de mesure) :

Deux questions supplémentaires

1. Pourquoi intégrer deux fois un accéléromètre est-il désespéré à long terme ? Un biais constant b donne une erreur de position ½bt² : quadratique en temps.

2. Pourquoi une caméra seule ne mesure-t-elle pas la distance ? La projection perd la profondeur : tous les points d’un rayon donnent le même pixel. Il faut une seconde vue, une hypothèse (le sol est plan) ou un capteur actif.

Référence

Les mots à retenir

Mot	Définition
Bruit / biais / dérive	Aléa à chaque mesure / décalage constant / décalage qui évolue.
Passe-bas / médian	Lisse (retarde) / supprime les aberrations.
Filtre complémentaire	Haute fréquence d’un capteur + basse fréquence d’un autre, gains fixes.
Kalman	Prédiction (F, Q) puis correction (H, R) ; gain optimal pour gaussiennes linéaires.
Innovation / NIS	Mesure − prédiction / test de cohérence.
Mahalanobis	Distance normalisée par la covariance ; rejet d’aberrations.
EKF / UKF	Kalman linéarisé par jacobiennes / par points sigma.
IMU / attitude	Gyro + accéléro (+ magnéto) → orientation ; quaternions en 3D.
Convolution / gradient / seuil	Outils de base du traitement d’images.
Hough	Détection de droites par votes.
Sténopé / intrinsèques	Modèle de caméra ; matrice K.
Homographie	Plan ↔ image par une matrice 3×3.

Pour continuer

Vous estimez l’état du monde

Module suivant : localisation et cartographie — filtre particulaire, cartes d’occupation, SLAM, et l’architecture logicielle d’un robot (ROS 2).

À faire chez soi

Lire « Kalman and Bayesian Filters in Python » chapitres 1-8 (notebooks gratuits) et refaire chaque exemple.
Sur robot : enregistrer 60 s d’IMU brute (CSV par port série, séance 26), rejouer le filtre complémentaire et le Kalman hors ligne, comparer.
Sur PC : pip install opencv-python, refaire la détection de ligne sur la webcam en temps réel.

← L18 Sommaire L20 : Localisation, cartographie, ROS →