Bonjour à tous,

Je dois m'appuyer sur les données d'un capteur à ultrason (permettant de mesurer des distances) pour effectuer un asservissement. Cependant, les données du capteur sont loin d'être fiable. J'aimerais donc appliquer un filtre de Kalman sur les valeurs discrètes que je reçois. Sur toutes les applications que j'ai pu trouver, il y avait des vecteurs et des matrices, et je ne sais pas du tout comment les remplir.

J'ai trouvé une version simplifiée du filtre que j'ai implémenté de la manière suivante :
Code:
typedef struct {
    double Q;                 // Process Noise Covariance
    double R;                 // Measurement Noise Covariance
    double estimateValue;     // Estimates Value
    double covariance;        // Estimation Error Covariance
    double K;                 // Kalman ain
} Kalman;

Kalman Kalman_Init(double Q, double R, double initialCovariance, double intialValue) {
    Kalman result;

    result.Q = Q;
    result.R = R;
    result.covariance = initialCovariance;
    result.estimateValue = intialValue;

    return result;
}

double Kalman_Filter(Kalman* filter, double measurement) {
    // Prediction Update
    filter->covariance = filter->covariance + filter->Q;

    // Measurement Update
    filter->K = filter->covariance / (filter->covariance + filter->R);
    filter->estimateValue = filter->estimateValue + filter->K * (measurement - filter->estimateValue);
    filter->covariance = (1 - filter->K) * filter->covariance;

    return filter->estimateValue;
}
Mais les valeurs que j'obtiens sont encore moins cohérantes (ex : -30040, 400, -5000 etc.).

Est-ce que quelqu'un pourrait m'aider en identifiant ce qui pourrait ne pas fonctionner, en me donnant un algarithme à implémenter ou tout renseignemant utile à mon problème ?

Merci d'avance,
Anthony.