Kalman-Filter für Position und Geschwindigkeit: Einführung von Geschwindigkeitsschätzungen

24

Vielen Dank an alle, die gestern Kommentare / Antworten zu meiner Anfrage gepostet haben ( Implementierung eines Kalman-Filters für Position, Geschwindigkeit, Beschleunigung ). Ich habe mir angesehen, was empfohlen wurde, und insbesondere sowohl (a) das Wikipedia-Beispiel für eine eindimensionale Position und Geschwindigkeit als auch eine andere Website, die ähnliche Aspekte berücksichtigt .

Update 26. April 2013 : Die ursprüngliche Frage enthielt einige Fehler, die damit zusammenhängen, dass ich das Wikipedia-Beispiel für eindimensionale Position und Geschwindigkeit nicht richtig verstanden hatte . Mit meinem verbesserten Verständnis der Vorgänge habe ich die Frage jetzt neu formuliert und enger fokussiert.

Bei beiden Beispielen, auf die ich im einleitenden Abschnitt verweise, wird davon ausgegangen, dass nur die Position gemessen wird. Keines der Beispiele hat jedoch irgendeine Berechnung für die Geschwindigkeit. Das Wikipedia-Beispiel gibt beispielsweise die -Matrix als , was bedeutet, dass nur die Position eingegeben wird. Konzentration auf die Wikipedia Beispiel der Zustandsvektor des Kalman - Filters enthält Position und Geschwindigkeit , dhH H = [ 1 0 ] x k x k x k(xkxk1)/dtHH=[1   0]xkxkx˙k

xk=(xkx˙k)

Angenommen, die Messung der Position zum Zeitpunkt ist . Dann , wenn die Position und die Geschwindigkeit zum Zeitpunkt war und , und wenn eine konstante Beschleunigung, die in dem Zeitpunkt gilt Intervall bis , Aus der Messung von kann mit der Formel ein Wert für werdenx k k - 1 x k - 1 ˙ x k - 1 a k - 1 k x akx^kk1xk1x˙k1ak1kx^a

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

Dies impliziert, dass zum Zeitpunkt k ein Maß x˙^k der Geschwindigkeit gegeben ist durch

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

Alle Größen auf der rechten Seite dieser Gleichung (dh x^k , xk1 und x˙k1 ) sind normalverteilte Zufallsvariablen mit bekannten Mitteln und Standardabweichungen , also die R Matrix für den Messvektor

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

berechnet werden kann. Ist dies eine gültige Methode, um Geschwindigkeitsschätzungen in den Prozess einzufügen?

Stochastisch
quelle
2
Ich habe nicht alle Ihre Berechnungen durchgesehen. Wenn Sie jedoch vom Wikipedia-Beispiel sprechen, scheinen Sie in Bezug auf die Struktur etwas verwirrt zu sein. Sie haben Recht, dass nur die Position gemessen wird. Es wird jedoch ein sogenanntes "Konstantgeschwindigkeits" -Modell verwendet. Dies bedeutet, dass die Geschwindigkeit in der Zustandsübergangsmatrix als konstant angesehen wird.
Jason R
3
Änderungen der Geschwindigkeit werden mit Hilfe der Prozessrauschmatrix modelliert. Sie gehen daher von einer zufälligen Änderung der Geschwindigkeit mit einer bestimmten Kovarianz aus. Überraschenderweise funktioniert dies oft gut. Es ist üblich, auf diese Weise Prozessrauschen zu verwenden, das eine Ableitung über Ihrer höchsten zustandsvariablen Ableitung liegt. Wenn Sie beispielsweise die Beschleunigung in Ihr Modell aufgenommen haben, ist möglicherweise eine zufällige Ruckkomponente in Ihrem Prozessrauschen enthalten.
Jason R
@JasonR Mit dem Wikipedia-Modell (unter der Annahme einer anfänglichen Kovarianz von Null zwischen Position und Geschwindigkeit) ist die Geschwindigkeitsschätzung immer der Anfangswert (wie Sie sagen, ein Modell mit konstanter Geschwindigkeit). Die Varianz der Drehzahl wächst jedoch monoton über das Prozessrauschen, und es gibt keine Messungen, die dies reduzieren können. Was ist der Vorteil gegenüber einem Modell, das nur die Position des Modells angibt und eine konstante Geschwindigkeit annimmt?
Stochastisch
2
Die Varianz der Geschwindigkeitsschätzung sollte nicht monoton ansteigen. Prozessrauschen fügt der Zustandsübergangsgleichung lediglich eine stochastische Komponente hinzu, mit der Sie eine gewisse Unsicherheit darüber ausdrücken können, wie sich der Systemzustand von Zeit zu Zeit weiterentwickelt. Wenn Sie das Prozessrauschen nicht einbeziehen, würde Ihr Filter wirklich eine konstante Geschwindigkeit ausgeben. Das ist wahrscheinlich nicht das, was du willst.
Jason R
Nun, @JasonR, wenn Sie sich das Wikipedia-Modell ansehen, werden Sie sehen, dass es Ihnen eine monotinisch zunehmende Varianz der Geschwindigkeit gibt!
Stochastisch

Antworten:

24

Ist dies eine gültige Methode, um Geschwindigkeitsschätzungen in den Prozess einzufügen?

Wenn Sie Ihr Bundesland entsprechend auswählen, sind die Geschwindigkeitsschätzungen "kostenlos". Siehe die Ableitung des Signalmodells unten (für den einfachen 1-D-Fall, den wir uns angesehen haben).

Signalmodell, Nehmen Sie 2

Wir müssen uns also wirklich auf ein Signalmodell einigen, bevor wir dies vorantreiben können. Aus Ihrer Bearbeitung geht hervor, dass Ihr Modell der Position folgt aussieht :xk

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

Wenn unser Zustand wie zuvor ist: dann lautet die Zustandsaktualisierungsgleichung: wobei nun unser die normalverteilte Beschleunigung ist. xk+1=( 1 & Dgr; t 0 1 )xk+( ( & Dgr ; t ) 2

xk=(xkx˙k)
ak
xk+1=(1  Δt0  1)xk+((Δt)22Δt)eink
eink

Das gibt eine andere -Matrix als die vorherige Version, aber die Matrizen und sollten gleich sein.F HGFH


Wenn ich dies einsetze scilab(sorry, kein Zugriff auf matlab), sieht es so aus:

// 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

Dann kann ich die Kalman-Filtergleichungen auf dieses (die verrauschten Messungen) anwenden .y

// 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

Wir haben also unsere verrauschten Messungen , und wir haben den Kalman-Filter auf sie angewendet und dasselbe Signalmodell verwendet, um zu generieren, wie wir es tun, um den Kalman-Filter anzuwenden (eine ziemlich große Annahme, manchmal!).yyy

Die folgenden Darstellungen zeigen das Ergebnis.

Diagramm 1 : und gegen die Zeit.x kyxk

Bildbeschreibung hier eingeben

Diagramm 2 : Eine vergrößerte Ansicht der ersten Beispiele:

Bildbeschreibung hier eingeben

Handlung 3 : Etwas, das man im wirklichen Leben nie bekommt, die wahre Position gegen die Zustandsschätzung der Position.

Bildbeschreibung hier eingeben

Plot 4 : Etwas, das man im wirklichen Leben auch nie bekommt, die wahre Geschwindigkeit gegen die Zustandsschätzung der Geschwindigkeit.

Bildbeschreibung hier eingeben

Diagramm 5 : Die Norm der Zustands-Kovarianz-Matrix (etwas, das Sie im wirklichen Leben immer überwachen sollten!). Beachten Sie, dass es sehr schnell von seinem anfänglichen sehr großen Wert zu einem sehr kleinen Wert übergeht, sodass ich nur die ersten Beispiele gezeigt habe.

Bildbeschreibung hier eingeben

Diagramm 6 : Diagramme des Fehlers zwischen der wahren Position und Geschwindigkeit und deren Schätzungen.

Bildbeschreibung hier eingeben

Wenn Sie den Fall untersuchen, in dem die Positionsmessungen genau sind, stellen Sie fest, dass die Kalman-Udpate-Gleichungen genaue Ergebnisse für BEIDE Positionen und Geschwindigkeiten liefern. Mathematisch ist es einfach zu verstehen, warum. Unter Verwendung der gleichen Notation wie im Wikipedia-Artikel bedeuten genaue Maße, dass . Wenn Sie davon ausgehen, dass die Ausgangsposition und die Geschwindigkeit bekannt sind, sodass , dann und die Kalman-Gewinnmatrix ist gegeben durchP k =0 P - k + 1 = Q K k + 1zk+1=xk+1Pk=0Pk+1-=Q.Kk+1

Kk+1=(12/dt)

Dies bedeutet, dass der Kalman-Aktualisierungsvorgang erzeugt

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

Wie Sie sehen, wird der Wert für die Geschwindigkeit durch genau die Formel angegeben, die Sie für die Geschwindigkeitsschätzung vorgeschlagen haben. Also, obwohl du keine Berechnung für die Geschwindigkeit sehen konntest, ist sie doch darin versteckt.(xk-xk-1)/dt

Peter K.
quelle
Vielen Dank für all Ihre Hilfe. Meine ursprüngliche Frage enthielt einige Missverständnisse, daher habe ich versucht, sie neu zu fokussieren und Ihre Frage zu z_k zu beantworten.
Stochastisch
1
danke vm für all deine bemühungen :-). Ihre Arbeit hat mich zu ein bisschen Mathe angespornt, die ich auf Ihre Antwort geheftet habe, ich hoffe, es macht Ihnen nichts aus. Wie auch immer, ich bin jetzt zu 100% davon überzeugt, dass die Standardmethode gut ist, da ich dort Formeln für die Geschwindigkeit sehen kann.
Nochmals
Freut mich helfen zu können! Kein Problem mit dem Hinzufügen zur Antwort.
Peter K.