Filtre de Kalman - Implémentation, paramètres et réglage

Tout d'abord, c'est la première fois que j'essaye de faire un filtre de Kalman.

J'ai déjà posté la question suivante Filtrer le bruit et les variations des valeurs de vitesse sur StackOverflow qui décrit l'arrière-plan de ce message. Il s'agit d'un échantillon typique de valeurs que j'essaie de filtrer. Ils ne doivent pas nécessairement diminuer, ce qui est le cas ici. Mais le taux de changement est généralement comme ceci

X ------- Y
16 --- 233.75
24 --- 234.01
26 --- 234.33
32 --- 234.12
36 --- 233.85
39 --- 233.42
47 --- 233.69
52 --- 233.68
55 --- 233,76
60 --- 232,97
66 --- 233,31
72 --- 233,99

J'ai implémenté mon filtre Kalman selon ce tutoriel: Filtre Kalman pour les nuls .

Mon implémentation ressemble à ceci (pseudocode).

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

Je commence avec une estimation antérieure de 0. Cela semble bien fonctionner. Mais ce que je remarque, c'est que le kalmanGain diminuera chaque fois que cette mise à jour est exécutée, ce qui signifie que je fais confiance à mes nouvelles valeurs moins la durée de fonctionnement de mon filtre (?). Je n'en veux pas.

Je suis passé de l'utilisation d'une moyenne mobile (simple et pondérée exponentielle) à celle-ci. Pour l'instant, je ne peux même pas obtenir de aussi bons résultats que cela.

Ma question est de savoir si c'est la bonne mise en œuvre et si ma variance d'erreur précédente et l'écart-type semblent bons selon les valeurs d'échantillon que j'ai publiées? Mes paramètres sont en fait juste choisis au hasard pour voir si je pouvais obtenir de bons résultats. J'ai essayé plusieurs gammes différentes mais avec des résultats médiocres. Si vous avez des suggestions de changements que je peux faire, ce serait vraiment apprécié. Je suis désolé s'il manque des éléments évidents. Première publication ici aussi.

Les filtres de Kalman sont utiles lorsque votre signal d'entrée est constitué d'observations bruyantes de l'état d'un système dynamique linéaire. Compte tenu d'une série d'observations de l'état du système, le filtre de Kalman vise à fournir de manière récursive de meilleures estimations de l'état du système sous-jacent. Pour l'appliquer avec succès, vous devez disposer d'un modèle de dynamique du système dont vous estimez l'état. Comme décrit en détail sur Wikipédia , ce modèle décrit comment le système sous-jacent d'état devrait changer sur un pas de temps, compte tenu de son état précédent, de toutes les entrées du système et d'une composante stochastique distribuée gaussienne appelée bruit de processus.

Cela dit, il ne ressort pas clairement de votre question si vous avez un tel modèle sous-jacent. Le message lié a indiqué que vous consommiez des valeurs de vitesse à partir d'un capteur. Ceux-ci peuvent être modélisés comme des observations directes de l'état d'un système (où l'état est sa vitesse), ou des observations indirectes de son état (où l'état est sa position, par exemple). Mais, pour utiliser le cadre de Kalman, vous devez choisir un modèle pour la façon dont cet état devrait évoluer au fil du temps; ces informations supplémentaires sont utilisées afin de générer l'estimation optimale. Un filtre de Kalman n'est pas une boîte noire magique qui "nettoiera" simplement un signal qui lui est appliqué.

Cela dit, le phénomène auquel vous avez fait allusion, où le filtre de Kalman deviendra de plus en plus confiant dans sa propre sortie au point où les observations d'entrée seront progressivement ignorées, se produit dans la pratique. Cela peut être atténué en augmentant manuellement les valeurs dans la matrice de covariance du bruit de processus. Ensuite, qualitativement, le modèle de transition d'état du système contient une composante stochastique plus grande, de sorte que la capacité de l'estimateur à prédire avec précision l'état suivant étant donné l'état actuel est diminuée. Cela réduira sa dépendance à l'égard de son estimation actuelle de l'état du système et augmentera sa dépendance à l'égard des observations ultérieures, empêchant l'effet "ignorer l'entrée".

Si j'ai bien compris, vous avez quelque chose qui bouge et vous pouvez observer la vitesse et cette vitesse est bruyante. D'après vos mesures, vous observez 2 types de variations. \

1. Variations causées par le bruit
2. Variations parce que l'objet change vraiment la vitesse (par exemple en tournant)

La raison pour laquelle votre gain Kalman passe à zéro est que vous avez implicitement supposé que la vitesse de l'objet est constante et que tout ce que vous avez à faire est d'estimer cette vitesse réelle.

" Hé, j'ai un objet qui se déplace à vitesse constante et je veux estimer cette vitesse constante "

$x_k$$k$$y_k$

$$x_k = x_{k-1}$$ $$y_k = x_k + q_k$$

Mais votre objet ne bouge pas de cette façon. Sa vitesse change et vous ne savez pas comment ni quand cela va changer.

Ce que vous avez à dire à la place, c'est:

" Hé, j'ai un objet qui se déplace à une vitesse mais je ne sais pas comment il change sa vitesse "

Vous pouvez procéder de plusieurs manières: La manière la plus simple consiste à ajouter de l'incertitude à votre état.

$$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$$ $$y_k = x_k + q_k$$ $q_k$$v_k$

Vos équations de filtre de Kalman ressembleront à ceci:

$$\hat y_{k|k-1} = \hat x_{k|k-1}$$ $$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$$ $$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$$ $$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$$ $$P_{k+1|k} =P_{k|k} + Q^s$$

0.05$Q^o$$Q^s$

Dans votre code, la légère modification serait:

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

stateVariance$Q^s$

Cette stateVariancevaleur peut être tout ce que vous voulez. Il est basé sur votre confiance quant à la variation réelle de la vitesse. Si vous pensez que la vitesse restera assez constante, réglez-la sur un petit nombre.

De cette façon, votre gain Kalman n'ira pas à zéro.

Vous avez besoin d'un système dynamique pour utiliser un filtre de Kalman.

je voudrais suggerer

$$y = \sum\limits_{i =0}^n a_i\, x^i$$

$$a[k+1] = a[k] + w$$ $$cov(w) = Q$$ $$z = \sum\limits_{i =0}^n a_i\, x^i = y$$

$x$$a$

Je pense que vous pourriez utiliser certaines idées de la théorie classique du contrôle, par exemple le contrôleur PID .

Votre signal Y peut être la consigne du régulateur u (t). L'usine de traitement est à seulement 1 et y (t) sera filtré en sortie. Tout ce que vous aurez à faire est de régler les paramètres (réglage) P, I et D pour obtenir ce que vous voulez.

La sortie y (t) essaiera de "suivre" l'entrée u (t), mais les paramètres contrôlent comment ce suivi sera.

Le gain différentiel D rendra votre réponse sensible aux changements d'erreur rapides. Dans votre cas, je pense que D devrait être petit. Vous ne voulez pas que y (t) change si u (t) change brusquement.

Le gain intégral «I» rendra votre réponse sensible aux erreurs accumulées. Vous devez y mettre une valeur élevée. Si u (t) change de niveau et le maintient, l'erreur s'accumule et vous voulez que y (t) fasse de même.

Le gain P peut donner un réglage fin. Quoi qu'il en soit, essayez de jouer avec les paramètres et voyez ce que vous obtenez.

Il existe cependant des méthodes de réglage compliquées, mais je ne pense pas que vous en aurez besoin.

Bonne chance.

Voici une implémentation simple et propre de Kalman Filter qui suit les notations comme dans la page Wikipedia. https://github.com/zziz/kalman-filter