Filtre de Kalman pour la position et la vitesse: introduction d'estimations de vitesse

24

Merci à tous ceux qui ont posté hier des commentaires / réponses à ma requête ( Implémentation d'un filtre de Kalman pour la position, la vitesse, l'accélération ). J'ai regardé ce qui était recommandé, et en particulier à la fois (a) l'exemple de Wikipédia sur la position et la vitesse à une dimension et également un autre site Web qui considère une chose similaire .

Mise à jour du 26 avril 2013 : la question d'origine contenait ici des erreurs, liées au fait que je n'avais pas bien compris l' exemple wikipedia sur la position et la vitesse unidimensionnelles . Avec ma meilleure compréhension de ce qui se passe, j'ai maintenant reformulé la question et je l'ai concentrée plus étroitement.

Les deux exemples auxquels je me réfère dans le paragraphe d'introduction ci-dessus supposent que seule la position est mesurée. Cependant, aucun des deux exemples n'a de calcul (xkxk1)/dt pour la vitesse. Par exemple, l'exemple Wikipedia spécifie la matrice H comme H=[1   0] , ce qui signifie que seule la position est entrée. En se concentrant sur l'exemple de Wikipedia, le vecteur d'état xk du filtre de Kalman contient la position xk et la vitesse x˙k , c'est-à-dire

xk=(xkx˙k)

Supposons que la mesure de la position à l' instant k est - x k . Ensuite , si la position et la vitesse à l' instant k - 1 étaient x k - 1 et ˙ x k - 1 , et si une est une accélération constante qui applique dans l'intervalle de temps k - 1 à k , à partir de la mesure de x , il est possible déduire une valeur pour un en utilisant la formulex^kk1xk1x˙k1ak1kx^a

x^k=xk1+x˙k1dt+12adt2

Cela implique que au moment , une mesure ˙ x k de la vitesse est donnée parkx˙^k

x˙^k=x˙k1+adt=2x^kxk1dtx˙k1

Toutes les quantités sur le côté droit de l'équation , x k - 1 et ˙ x k - 1 ) sont normalement distribués variables aléatoires avec des moyens connus et les écarts types, de sorte que le R de matrice pour le vecteur de mesurex^kxk1x˙k1R

x^k=(x^kx˙^k)

peut être calculé. Est-ce une façon valable d'introduire des estimations de vitesse dans le processus?

Stochastiquement
la source
2
Je n'ai pas parcouru tous vos calculs. Cependant, en parlant de l'exemple de Wikipedia, vous semblez être un peu confus sur sa structure. Vous avez raison, seule la position est mesurée. Cependant, un modèle dit "à vitesse constante" est utilisé. Cela signifie que la vitesse est considérée comme constante dans la matrice de transition d'état.
Jason R
3
Les changements de vitesse sont modélisés à l'aide de la matrice de bruit de processus. Par conséquent, vous supposez intrinsèquement que la vitesse va changer de façon aléatoire avec une covariance spécifiée. Étonnamment, cela fonctionne souvent bien. De cette manière, il est courant d'utiliser le dérivé du bruit de processus une dérivation au-dessus de votre dérivée à variable d'état la plus élevée. Par exemple, si vous avez inclus l'accélération dans votre modèle, vous pourriez avoir un composant de secousse aléatoire inclus dans le bruit de votre processus.
Jason R
@JasonR avec le modèle wikipedia (en supposant une covariance initiale nulle entre la position et la vitesse), l'estimation de la vitesse est toujours sa valeur initiale (comme vous le dites, un modèle "à vitesse constante"). Cependant, la variance de la vitesse augmente de façon monotone via le bruit du processus, et aucune mesure ne peut la réduire. Quel est l'avantage de cela sur un modèle qui ne positionne que les modèles et suppose une vitesse constante?
Stochastiquement le
2
La variance de l'estimation de la vitesse ne devrait pas augmenter de façon monotone. Le bruit de processus introduit simplement une composante stochastique dans l'équation de transition d'état, vous permettant d'exprimer une certaine incertitude quant à la façon exacte dont l'état du système évoluera d'un pas de temps à l'autre. Si vous n'incluez pas de bruit de processus, votre filtre produirait vraiment une vitesse constante. Ce n'est probablement pas ce que vous voulez.
Jason R
Eh bien @JasonR, si vous regardez le modèle wikipedia, vous verrez que la variance de vitesse monotiniquement croissante est ce qu'elle vous donne!
Stochastiquement le

Réponses:

24

Est-ce une façon valable d'introduire des estimations de vitesse dans le processus?

Si vous choisissez votre état de manière appropriée, les estimations de vitesse sont "gratuites". Voir la dérivation du modèle de signal ci-dessous (pour le cas 1-D simple que nous avons examiné).

Modèle de signal, prise 2

xk

xk+1=xk+x˙kΔt+12a(Δt)2x˙k+1=x˙k+aΔt

xk=(xkx˙k)
then the state update equation is just:
xk+1=(1  Δt0  1)xk+((Δt)22Δt)ak
where now our ak is the normally distributed acceleration.

That gives different G matrix from the previous version, but the F and H matrices should be the same.


If I implement this in scilab (sorry, no access to matlab), it looks like:

// Signal Model
DeltaT = 0.1;
F = [1 DeltaT; 0 1];
G = [DeltaT^2/2; DeltaT];
H = [1 0];

x0 = [0;0];
sigma_a = 0.1;

Q = sigma_a^2;
R = 0.1;

N = 1000;

a = rand(1,N,"normal")*sigma_a;

x_truth(:,1) = x0;
for t=1:N,
    x_truth(:,t+1) = F*x_truth(:,t) + G*a(t);
    y(t) = H*x_truth(:,t) + rand(1,1,"normal")*sqrt(R);
end

Then, I can apply the Kalman filter equations to this y (the noisy measurements).

// Kalman Filter
p0 = 100*eye(2,2);

xx(:,1) = x0;
pp = p0;
pp_norm(1) = norm(pp);
for t=1:N,
    [x1,p1,x,p] = kalm(y(t),xx(:,t),pp,F,G,H,Q,R);
    xx(:,t+1) = x1;
    pp = p1;
    pp_norm(t+1) = norm(pp);
end

So we have our noisy measurements y, and we've applied the Kalman filter to them and used the same signal model to generate y as we do to apply the Kalman filter (a pretty big assumption, sometimes!).

Then following plots show the result.

Plot 1: y and xk versus time.

enter image description here

Plot 2: A zoomed view of the first few samples:

enter image description here

Plot 3: Something you never get in real life, the true position vs the state estimate of the position.

enter image description here

Plot 4: Something you also never get in real life, the true velocity vs the state estimate of the velocity.

enter image description here

Plot 5: The norm of the state covariance matrix (something you should always monitor in real life!). Note that it very quickly goes from its initial very large value to something very small, so I've only shown the first few samples.

enter image description here

Plot 6: Plots of the error between the true position and velocity and their estimates.

enter image description here

If you study the case where the position measurements are exact, then you find that the Kalman udpate equations produce exact results for BOTH position and speed. Mathematically it's straightforward to see why. Using the same notation as the wikipedia article, exact measurements mean that zk+1=xk+1. If you assume that the initial position and speed are known so that Pk=0, then Pk+1=Q and the Kalman gain matrix Kk+1 is given by

Kk+1=(12/dt)

This means that the Kalman update procedure produces

x^k+1=Fk+1xk+Kk+1(zk+1Hk+1Fk+1xk)=(xk+x˙kdtx˙k)+(12/dt)(xk+1(xk+x˙kdt))=(xk+12(xk+1xk)/dtx˙k)

As you can see, the value for the speed is given by exactly the formula you were proposing to use for the speed estimate. So although you couldn't see any kind of calculation (xkxk1)/dt for speed, in fact it is hidden in there after all.

Peter K.
la source
Thanks for all your help so far. My original question did contain some misunderstandings, so I've tried to refocus it and with an attempt to answer your question about z_k.
Stochastically
1
thanks vm for all your efforts :-). Your work spurred me on to do a bit of maths which I've tacked on to your answer, hope you don't mind. Anyway, I'm now 100% convinced that the standard method is good, because I can see formulas in there for speed after all. Thanks again
Stochastically
Glad to be able to help! No problem with adding to the answer.
Peter K.