Estimation de la vitesse à partir d'une position et d'une accélération connues

Je suis coincé à modéliser un modèle de système, c'est-à-dire à obtenir mon vecteur d'état et mon vecteur d'entrée. Je suppose que la position et la vitesse sont des vecteurs d'état et que l'accélération est un vecteur d'entrée. Ma deuxième supposition est que les trois quantités sont dans le vecteur d'état et aucune dans le vecteur d'entrée.

Alors ... qu'est-ce qu'un vecteur d'état et quel est le vecteur d'entrée dans mon cas?

Information additionnelle:

J'obtiens des mesures du capteur de position et du capteur d'accélération. Tout se passe en 1D, par exemple en ligne droite. Je veux fusionner ces lectures (et supprimer le bruit) pour obtenir une estimation de la vitesse pour chaque pas de temps.

Ces équations décrivent le système; Je ne sais pas si elles sont bien modélisées. Si je comprends bien, il est prudent de prédire que l'accélération est constante (même si en réalité elle change) - parce que la matrice de covariance du processus corrige cette hypothèse (non?). entrez la description de l'image ici

J'ai également quelques exemples de données avec lesquelles travailler (les valeurs d'entrée ne sont pas bruyantes ici pour plus de simplicité):

 time    pos     acc      what I should get as output (velocity)
[0.0s]  0.000, -0.000  | 18.850
[0.1s]  1.885, -0.113  | 18.850
[0.2s]  3.768, -0.227  | 18.839
[0.3s]  5.650, -0.340  | 18.816
[0.4s]  7.528, -0.452  | 18.782
[0.5s]  9.401, -0.565  | 18.737

ADDITION 2:

Pour une meilleure communication, je crée une nouvelle réponse mais doit être traitée comme un commentaire à la première réponse. Jason, vous m'avez déjà énormément aidé et je vous suis vraiment reconnaissant de votre temps. J'ai toujours des problèmes avec cela - les résultats de Kalman Filter ne sont pas comme prévu. Puissiez-vous trouver le temps, veuillez lire ce qui suit, merci. Je vous dois déjà une bière ou deux (ou des cafés si vous voulez) - si vous avez paypal contactez moi sur primoz [at] codehunter.eu :)

J'ai implémenté le modèle que Jason avait proposé en première réponse. J'ai ajouté la secousse comme quatrième variable d'état. Après des heures de révision, j'ai décidé de revenir ici pour de l'aide. Les valeurs que je retire de KF ne sont pas celles attendues. Le tableau ci-dessous représente les données des 10 premières itérations de l'algorithme. Remarquez comment la secousse augmente à chaque pas de temps, ce qui rend les autres estimations erronées. Après une seconde, la différence entre l'accélération réelle et estimée est supérieure à 1 m / s² (voir tableau, dernière ligne)!

           real           measured                   estimated                    real 
time   pos     acc       pos    acc        pos      acc    jerk    vel[!]       velocity 
0.0   0.000  -0.000    -0.040  0.030  |  -0.300   -0.060   0.000   18.850  <-->  18.850
0.1   1.885  -0.113     1.965 -0.153  |   1.585   -0.061  -0.006   18.844  <-->  18.844
0.2   3.768  -0.227     3.778 -0.247  |   3.469   -0.066  -0.035   18.835  <-->  18.827
0.3   5.650  -0.340     5.750 -0.370  |   5.351   -0.090  -0.122   18.815  <-->  18.799
0.4   7.528  -0.452     7.358 -0.452  |   7.228   -0.152  -0.291   18.769  <-->  18.759
0.5   9.401  -0.565     9.251 -0.555  |   9.094   -0.282  -0.574   18.673  <-->  18.708
0.6   11.269 -0.677    11.309 -0.717  |   10.938  -0.518  -1.006   18.494  <-->  18.646
0.7   13.130 -0.788    13.260 -0.758  |   12.752  -0.840  -1.490   18.233  <-->  18.573
0.8   14.983 -0.899    15.043 -0.949  |   14.520  -1.286  -2.096   17.854  <-->  18.488
0.9   16.827 -1.009    16.977 -1.089  |   16.235  -1.838  -2.770   17.362  <-->  18.393
1.0   18.661 -1.118    18.831 -1.168  |   17.890  -2.477  -3.476   16.762  <-->  18.287

Mes matrices sont ici:

Qu'est-ce qui cause cet ajout à chaque pas de temps pour jerk ? L'une de mes matrices est-elle incorrecte?

Il en va de même avec la première solution (modèle à seulement 3 états) - l'accélération ne change pas comme il se doit.

DERNIÈRE ÉDITION:

J'ai finalement réussi à le faire fonctionner. Je ne sais pas s'il y a eu une erreur de mise en œuvre ou de mauvaises matrices P&Q.

kalman-filters

— Primož Kralj
source

Pour obtenir une réponse utile, vous devez fournir plus d'informations. Décrivez le système. Montrez-nous quelles équations vous avez rassemblées.

— Jim Clay

Merci pour votre réponse. J'ai fourni des informations supplémentaires.

— Primož Kralj

S'il vous plaît, quelqu'un m'aide avec ça - j'ai l'impression de perdre la tête.

— Primož Kralj

Je ne vois aucun problème évident. Il est probable que vous ayez une erreur de mise en œuvre.

— Jason R

@ Phonon, pas de rancune. Je me sens comme si j'étais sur une montagne russe :)

— Primož Kralj

Réponses:

Vous ne feriez pas d'hypothèse d'accélération constante dans ce cas. Vous feriez généralement cela si vous n'aviez aucun moyen de mesurer l'accélération du système, mais vous dites qu'elle est observable pour votre cas. La façon la plus évidente de modéliser ce système serait d'utiliser le vecteur d'état

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \end{matrix}]

$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \end{array} \right ]$

où $x_k$ est la position, $v_k$ est la vitesse, et $a_k$ est l'accélération du système à l'instant $k$ . Puisque vous dites que vous pouvez mesurer la position et l'accélération du système, le vecteur de mesure $\mathbf{z_k}$ serait:

z_{k} = H x_{k} + v_{k}

$\mathbf{z_k} = \mathbf{Hx_k} + \mathbf{v_k}$

avec

H = [\begin{array}{cc} 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}]

$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 \\ 0 && 0 && 1 \end{array} \right]$

résultant dans le modèle de mesure:

z_{k} = [\begin{matrix} x_{k} \\ a_{k} \end{matrix}] + v_{k}

$\mathbf{z_k} = \left[ \begin{array}{c} x_k \\ a_k \end{array} \right] + \mathbf{v_k}$

où $\mathbf{v_k}$ est le bruit (gaussien) inhérent à la mesure. Maintenant, sauf si vous ajoutez une force connue au système qui affecterait sa dynamique, le vecteur d'entrée $\mathbf{u_k} = \mathbf{0}$ . Voir cette question récente pour une discussion plus détaillée des différents termes du modèle de Kalman, que vous devez connaître et que vous n'avez pas .

Edit: Concernant le commentaire ci-dessous sur la façon de gérer la matrice de transition d'état pour le terme d'accélération: il existe deux façons différentes de gérer cela. Vous pourriez supposer un modèle à secousse nulle (la secousse est la dérivée temporelle de l'accélération); cela revient à supposer que $a_{k+1}=a_k$ . Ou, vous pouvez ajouter un quatrième élément à votre vecteur d'état:

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \\ \overset{⃛}{x_{k}} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \\ j_{k} \end{matrix}]

$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \\ \dddot{x_k} \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \\ j_k \end{array} \right ]$

où $j_k$ est la secousse du système, ou taux de variation de l'accélération, à l'instant $k$ . Puisque vous ne mesurez pas le jerk directement, vous auriez une matrice de mesure de:

H = [\begin{array}{cc} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}]

$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 && 0\\ 0 && 0 && 1 && 0 \end{array} \right]$

Et la matrice de transition d'état deviendrait:

F = [\begin{array}{cccc} 1 & d t & \frac{1}{2} d t^{2} & \frac{1}{6} d t^{3} \\ 0 & 1 & d t & \frac{1}{2} d t^{2} \\ 0 & 0 & 1 & d t \\ 0 & 0 & 0 & 1 \end{array}]

$\mathbf{F} = \left[ \begin{array}{cccc} 1 && dt && \frac{1}{2}dt^2 && \frac{1}{6} dt^3 \\ 0 && 1 && dt && \frac{1}{2}dt^2 \\ 0 && 0 && 1 && dt \\ 0 && 0 && 0 && 1 \end{array} \right]$

Il s'agit d'un modèle à «secousse constante» (similaire aux modèles à vitesse constante ou à accélération constante que vous pourriez voir dans des exemples simples); votre matrice de transition d'état suppose implicitement que la secousse est constante pour toutes les valeurs de k. Ce n'est probablement pas vrai. Pour gérer cet aspect du problème, vous introduiriez un terme de bruit de processus dans le composant jerk de l'équation de transition d'état.

Qualitativement, cela vous permet d'exprimer que vous n'êtes pas sûr de la façon dont le terme de secousse va changer d'un pas de temps à l'autre, mais que vous vous attendez à ce que les changements soient aléatoires avec une distribution gaussienne. Il s'agit d'un outil souvent utilisé pour "régler" des modèles de filtres adaptatifs comme celui-ci jusqu'à ce que vous trouviez un ensemble de paramètres qui fonctionne bien pour votre application.

— Jason R
source

Merci, vous avez vraiment allumé une lumière dans ma tête. Mais je ne sais toujours pas quelle est mon accélération qui devrait être à l'étape suivante (sinon constante) - c'est important pour construire la matrice de transition A, la dernière ligne pour être précise. Que suggérerais-tu?

— Primož Kralj

Il y a tout un désordre avec la migration des questions. Une partie de votre réponse a disparu. J'espère que les mods réparent ça. Regardez également l'addition 2 dans ma question d'origine.

— Primož Kralj

J'ai pu récupérer le texte édité et l'ai appliqué à nouveau à ma réponse.

— Jason R

Pourriez-vous expliquer comment obtenir la covariance de processus pour la première solution (3 variables d'état)? Je ne trouve pas de lien décent en ligne qui l'expliquerait dans la pratique.

— Primož Kralj

Je suppose que vos variables d'état sont conformes à la réponse de Jason. Si tel est le cas, je pense que votre problème pourrait être votre $Q$ matrice (bruit de processus): parce qu'elle est nulle partout sauf pour la mise à jour de la secousse, les seuls termes mettant à jour les valeurs de la position, de la vitesse et de l'accélération sont les parties déterministes des équations de mise à jour (sauf là où la nature stochastique de la secousse se répercute) .

Essayez d'utiliser un $Q$ matrice comme:

Q = [\begin{array}{cccc} 0,00001 & 0 & 0 & 0 \\ 0 & 0,00001 & 0 & 0 \\ 0 & 0 & 0,00001 & 0 \\ 0 & 0 & 0 & 0,001 \end{array}]

$Q = \left [ \begin{array}{cccc} 0.00001 & 0& 0& 0\\ 0 & 0.00001 & 0 & 0\\ 0 & 0 & 0.00001 & 0 \\ 0 & 0& 0& 0.001 \end{array} \right ]$

Si ces éléments diagonaux sont nuls, le modèle dit "Je ne m'attends pas à ce que la position, la vitesse ou l'accélération changent (autrement que par les équations de mise à jour)" --- ce qui n'est clairement pas le cas.

— Peter K.
source

Belle explication - mais pourquoi choisir 0,001, pourquoi pas 2,84 par exemple?

— Primož Kralj

Merci! La raison du choix de 0,00001 sur 2,84: Eh bien, 2,84 est TRÈS important pour une variance. J'ai supposé que vous aviez un raisonnement valide derrière le numéro de secousse (0,001), et que les autres chiffres devraient être nettement plus petits que cela. S'il n'y a aucune raison réelle de choisir le numéro de secousse, essayez la même valeur dans tous les éléments diagonaux.

— Peter K.

Mais qu'est-ce que cela signifie dans la pratique? Si je dis que mon capteur de position a une variance de 0,00001 (premier élément de diagnostic dans la matrice Q), cela signifie-t-il que son erreur est de + - 0,00001 mètre par exemple (je suppose que non)? Je suppose que vous choisissez une variance plus petite pour un capteur plus précis (position) et une variance plus grande pour un capteur avec moins de précision (accélération).

— Primož Kralj

Si vous dites que la variance d'une mesure est

0.00001

$0.00001$ , puis dans le cadre du filtre de Kalman, vous dites qu'il est distribué gaussien avec un écart-type de

\sqrt{0.00001}

$\sqrt{0.00001}$ . Ainsi, ~ 68% du temps, l'amplitude de l'erreur de mesure sera inférieure à

\sqrt{0.00001}

$\sqrt{0.00001}$ , ~ 95% du temps, ce sera moins de

2 \sqrt{0.00001}

$2\sqrt{0.00001}$ , etc.

— Jason R

Merci à vous deux, j'espère que ce fil aidera aussi les futurs visiteurs.

— Primož Kralj

Je pense que le problème est en Q. Q facturent K avec leurs valeurs. J'obtiens de bons résultats lorsque Q est préchargé avec des pouvoirs de delta_t.

Voir tout article sur le modèle wiener. Il a une bonne matrice Q.

A00 doit être multiple de delta_t puissance de 5 A11 delta_t puissance de 3 A22 delta_t puissance de 1 A01 multiple de delta_t puissance de 4

Etc

Regarder

https://www.google.com/url?sa=t&source=web&rct=j&url=https://pdfs.semanticscholar.org/9c1b/db6b5a25c1231e775c654f731e1ca1fef31a.pdf&ved=2ahUKEwj4LJAQVJAQA

— Jm Rams
source