Kalman Filtre de hauteur et d'accélération

Question

Je travaille sur un processeur de bras stm32f417ve et j'essaie de mettre en œuvre un filtre kalman pour fusionner des données d'accéléromètre avec des données de hauteur (capteur de pression). Je veux connaître la vitesse et la position verticales estimées. Les lectures d'accéléromètre sont tournées du cadre de corps à la terre, ce n'est pas le problème. J'ai déjà beaucoup cherché sur internet et j'ai aussi trouvé des choses intéressantes, mais je ne sais pas si ma situation correspond aux autres que j'ai trouvées, donc je suis là :) Cet article (Using Kalman filter with acceleration and position inputs) est très similaire à celui-ci, mais j'ai besoin d'un peu plus d'aide. J'ai aussi un MPU6000 comme 6DOF imu et un MS5611 baro. Je pense que la meilleure façon de combiner ces données est d'utiliser l'accélération comme entrée de contrôle, n'est-ce pas? Peut-être que quelqu'un pourrait regarder mes matrices et formules pour me dire, si c'est vrai ou non.

Formules:

//PREDICT 

x = A*x + B*u 

p = A*p*AT + Q 

//UPDATE 

Innovation = (H*p*HT + R)^-1 

K = p*HT*Innovation 

x = x + K*(y-H*x) 

p = (I-K*H)*p 

Matrizes:

#define NumState 3 
#define NumInput 1 
#define NumOutput 1 

static float32_t xAr[NumState][1]; 

static float32_t uAr[NumInput][1]; 

static float32_t yAr[NumOutput][1]; 

static float32_t AAr[NumState][NumState]; 

static float32_t BAr[NumState][NumInput]; 

static float32_t HAr[NumOutput][NumState]; 

static float32_t QAr[NumState][NumState]; 

static float32_t RAr[NumOutput][NumOutput]; 

static float32_t PAr[NumState][NumState]; 

static float32_t kAr[NumState][NumOutput]; 

static float32_t IAr[NumState][NumState]; 

I mettent l'accélération dans le vecteur u et la hauteur en y. La matrice IAR est juste une matrice d'identité, de sorte que ses éléments sont en diagonale 1.

RAr[0][0] = 0.1f; 

    QAr[0][0] = 1.0f; 
    QAr[0][1] = 1.0f; 
    QAr[0][2] = 0.0f; 
    QAr[1][0] = 1.0f; 
    QAr[1][1] = 1.0f; 
    QAr[1][2] = 0.0f; 
    QAr[2][0] = 0.0f; 
    QAr[2][1] = 0.0f; 
    QAr[2][2] = 0.0f; 

    uAr[0][0] = AccZEarth; 
    yAr[0][0] = Height; 

    HAr[0][0] = 1.0f; 
    HAr[0][1] = 0.0f; 
    HAr[0][2] = 0.0f; 

    BAr[0][0] = (dt*dt)/2; 
    BAr[1][0] = dt; 
    BAr[2][0] = 0.0f; 

    AAr[0][0] = 1.0f; 
    AAr[0][1] = dt; 
    AAr[0][2] = 0.0f - ((dt*dt)/2.0f); 
    AAr[1][0] = 0.0f; 
    AAr[1][1] = 1.0f; 
    AAr[1][2] = 0.0f - dt; 
    AAr[2][0] = 0.0f; 
    AAr[2][1] = 0.0f; 
    AAr[2][2] = 1.0f; 

    IAr[0][0] = 1.0f; 
    IAr[0][1] = 0.0f; 
    IAr[0][2] = 0.0f; 
    IAr[1][0] = 0.0f; 
    IAr[1][1] = 1.0f; 
    IAr[1][2] = 0.0f; 
    IAr[2][0] = 0.0f; 
    IAr[2][1] = 0.0f; 
    IAr[2][2] = 1.0f; 

    PAr[0][0] = 100.0f; 
    PAr[0][1] = 0.0f; 
    PAr[0][2] = 0.0f; 
    PAr[1][0] = 0.0f; 
    PAr[1][1] = 100.0f; 
    PAr[1][2] = 0.0f; 
    PAr[2][0] = 0.0f; 
    PAr[2][1] = 0.0f; 
    PAr[2][2] = 100.0f; 

Ce serait vraiment super si certains d'entre vous pouvez jeter un oeil et me dire wheter im bon ou mauvais!

Merci, Chris

Answer 1

La première chose à déterminer est de savoir si les deux capteurs vous avez l'intention d'utiliser ensemble un bon complément. La position MEMS IMU divergera rapidement à mesure que s'accumulent les erreurs d'intégration double. Pour l'utiliser avec succès dans cette application, vous devrez calibrer son biais et son échelle. Ceux-ci seront différents sur chaque axe, ce qui, compte tenu de votre état unidimensionnel, devra être appliqué en dehors du filtre. Puisque vous serez probablement à l'extérieur (où un altimètre est intéressant), votre étalonnage de biais/échelle devrait également être compensé en température.

Vous pouvez facilement tester l'IMU en effectuant la boucle x = A*x + B*u pendant que l'IMU se trouve sur votre bureau pour voir à quelle vitesse x[0] devient grand. Compte tenu de ce que je sais sur les IMU et les altimètres (pas autant que les IMU), je suppose que votre position dérivée de l'IMU sera pire que votre lecture altimétrique brute en quelques secondes. Beaucoup plus rapidement si le biais et l'échelle ne sont pas correctement calibrés. Le filtre de Kalman ne vaut que «fusionner» ces deux capteurs si vous pouvez atteindre un point où la précision à court terme de l'IMU est significativement meilleure que la précision à court terme de l'altimètre.

Si vous continuez avec le KF, votre structure semble généralement bonne. Voici quelques commentaires spécifiques:

• Vous modélisez l'accélération en tant que -x[2]. (Le signe moins est dû à votre matrice A Je ne sais pas pourquoi vous avez choisi d'annuler l'accélération.) Je ne pense pas que l'accélération de votre état vous rende bien. L'un des avantages de la méthode ... + B*u d'utiliser l'IMU est que vous n'avez pas besoin de conserver l'accélération (comme le montre votre matrice B). Si l'accélération est une mesure vous devez l'avoir dans votre vecteur d'état parce que H=[0 0 1].
• Vous n'avez fait aucun effort pour choisir P, Q ou R.Ce sont les matrices les plus importantes dans le KF. Une autre réponse ici pourrait aider: https://electronics.stackexchange.com/questions/86102/kalman-filter-on-linear-acceleration-for-distance/134975#134975

Answer 2

merci pour votre réponse! Jusqu'à présent, j'utilise un filtre complémentaire pour fusionner les données acc avec les données baro. Les trois axes de l'acc sont déjà compensés. En ce moment, j'ai un filtre 1D-kalman qui réduit le bruit de la sortie baro tout en gardant le retard de phase assez faible, c'est la raison pour laquelle je n'utilise pas de filtre passe-bas. Je calcule la dérivée des données baro pour obtenir la vitesse basée sur le baro, qui a environ 100ms de retard. Cette vitesse est ensuite introduite dans le premier filtre complémentaire avec la vitesse calculée acc en l'intégrant. Le second filtre complémentaire utilise cette vitesse fusionnée (qui est sans dérive &) et l'intègre pour le fusionner avec les données d'altitude baro. Cela fonctionne plutôt bien, mais je voulais essayer le filtre Kalman pour voir s'il était possible d'obtenir des données plus précises. Sur Internet, si trouvé ce document: http://www.actawm.pb.edu.pl/volume/vol8no2/06_2014_004_ROMANIUK_GOSIEWSKI.pdf Il semble correspondre à mon "problème" très bon, j'ai donc décidé de l'utiliser comme point de départ. Le signe négatif dans ma matrice A vient de ceci, peut-être en raison de leur direction de montage. Je vais vérifier que;)

Meilleures salutations Chris