Qu'est-ce qu'un filtre de Kalman ?
Un filtre de Kalman est un algorithme de traitement du signal qui permet d'estimer l'état d'un système à partir des données mesurées.
Développé par Rudolf Kalman en 1960, ce filtre optimal combine une série de mesures imprécises pour produire une estimation plus précise de l'état du système. Il est largement utilisé en navigation, robotique, traitement d'images et bien d'autres domaines.
Le savais-tu ?
Le filtre de Kalman a été utilisé dans le programme Apollo de la NASA pour guider les vaisseaux spatiaux vers la Lune !
Principe Général du Filtre de Kalman
Le filtre de Kalman fonctionne en deux étapes principales : prédiction et mise à jour. Il maintient une estimation de l'état du système et de son incertitude (matrice de covariance)
$$\begin{cases} \text{Prédiction:} & \hat{x}_k^- = F_k\hat{x}_{k-1} + B_ku_k \\ & P_k^- = F_kP_{k-1}F_k^T + Q_k \\ \\ \text{Mise à jour:} & K_k = P_k^-H_k^T(H_kP_k^-H_k^T + R_k)^{-1} \\ & \hat{x}_k = \hat{x}_k^- + K_k(z_k - H_k\hat{x}_k^-) \\ & P_k = (I - K_kH_k)P_k^- \end{cases}$$
Où:
- \( \hat{x} \): estimation de l'état
- \( P \): matrice de covariance d'erreur
- \( F \): matrice de transition d'état
- \( Q \): covariance du bruit de processus
- \( R \): covariance du bruit de mesure
- \( K \): gain de Kalman
- \( z \): mesure observée
Concept de base du filtre de Kalman [Wikipédia]
Comment appliquer cette notion au projet ?
Nous nous appuierons principalement sur les travaux de hmartiro, qui propose une implémentation détaillée de l'algorithme de Kalman en C++. Nous examinerons comment adapter ses principes à notre robot bipède.
/**
* Implementation of KalmanFilter class.
*
* @author: Hayk Martirosyan
* @date: 2014.11.15
*/
#include <iostream>
#include <stdexcept>
#include "kalman.hpp"
KalmanFilter::KalmanFilter(
double dt,
const Eigen::MatrixXd& A,
const Eigen::MatrixXd& C,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::MatrixXd& P)
: A(A), C(C), Q(Q), R(R), P0(P),
m(C.rows()), n(A.rows()), dt(dt), initialized(false),
I(n, n), x_hat(n), x_hat_new(n)
{
I.setIdentity();
}
KalmanFilter::KalmanFilter() {}
void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) {
x_hat = x0;
P = P0;
this->t0 = t0;
t = t0;
initialized = true;
}
void KalmanFilter::init() {
x_hat.setZero();
P = P0;
t0 = 0;
t = t0;
initialized = true;
}
void KalmanFilter::update(const Eigen::VectorXd& y) {
if(!initialized)
throw std::runtime_error("Filter is not initialized!");
x_hat_new = A * x_hat;
P = A*P*A.transpose() + Q;
K = P*C.transpose()*(C*P*C.transpose() + R).inverse();
x_hat_new += K * (y - C*x_hat_new);
P = (I - K*C)*P;
x_hat = x_hat_new;
t += dt;
}
void KalmanFilter::update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A) {
this->A = A;
this->dt = dt;
update(y);
}
Dans quelle partie du code peut intervenir le Filtre de Kalman ?
Stabilisation
Le filtre de Kalman peut être utilisé pour estimer l'angle d'inclinaison et la vitesse angulaire du robot à partir des données bruyante de l'IMU (gyroscope+accéléromètre).
Exemple pratique :
En fusionnant les mesures à court terme précises du gyroscope avec les mesures à long terme stables de l'accéléromètre, on obtient une estimation plus précise de l'orientation.
Acquisition des données
Filtrage des mesures des capteurs (codeurs moteurs, FSR, IMU) pour réduire le bruit de mesure et améliorer la qualité des données.
Exemple pratique :
Lors de la marche, les capteurs de force (FSR) subissent des perturbations. Le filtre permet d'estimer la vraie force appliquée.
Surveillance du Système
Estimation de l'état interne du robot (position, vitesse, accélération) à partir de mesures partielles et incomplètes.
Exemple pratique :
Reconstruction de la trajectoire complète du centre de masse à partir de mesures intermittentes.
Synchronisation Capteurs et Moteur
Compensation des délais entre les mesures des capteurs et l'action des moteurs par prédiction de l'état futur.Exemple pratique :
Prédire la position future des jambes pour anticiper les commandes moteurs et réduire la latence.
Références
- Kalman, R. E. (1960). A new approach to linear filtering and prediction problems. Journal of Basic Engineering.
- Welch, G., & Bishop, G. (2006). An Introduction to the Kalman Filter. University of North Carolina at Chapel Hill.
- Martirosyan, H. (2014). Kalman Filter C++ Implementation. GitHub repository.
- NASA. (1969). Apollo Guidance Computer and Kalman Filter. Technical Reports.