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

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 , dh $(x_k-x_{k-1})/dt$ ${\bf H}$ ${\bf H} = [1\ \ \ 0]$ ${\bf x}_k$ $x_k$ $\dot{x}_{k}$

\begin{aligned} x_{k} & = (\begin{matrix} x_{k} \\ {\dot{x}}_{k} \end{matrix}) \end{aligned}

$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$

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 werden $k$ $\hat{x}_k$ $k-1$ $x_{k-1}$ $\dot{x}_{k-1}$ $a$ $k-1$ $k$ $\hat{x}$ $a$

{\hat{x}}_{k} = x_{k - 1} + {\dot{x}}_{k - 1} d t + \frac{1}{2} a d t^{2}

$\hat{x}_k = x_{k-1} + \dot{x}_{k-1} dt + \frac{1}{2} a dt^2$

Dies impliziert, dass zum Zeitpunkt $k$ ein Maß $\hat{\dot{x}}_k$ der Geschwindigkeit gegeben ist durch

{\hat{\dot{x}}}_{k} = {\dot{x}}_{k - 1} + a d t = 2 \frac{{\hat{x}}_{k} - x_{k - 1}}{d t} - {\dot{x}}_{k - 1}

$\hat{\dot{x}}_k = \dot{x}_{k-1} + a dt = 2 \frac{\hat{x}_k - {x}_{k-1}}{dt} - \dot{x}_{k-1}$

Alle Größen auf der rechten Seite dieser Gleichung (dh $\hat{x}_k$ , $x_{k-1}$ und $\dot{x}_{k-1}$ ) sind normalverteilte Zufallsvariablen mit bekannten Mitteln und Standardabweichungen , also die $\bf R$ Matrix für den Messvektor

\begin{aligned} {\hat{x}}_{k} & = (\begin{matrix} {\hat{x}}_{k} \\ {\hat{\dot{x}}}_{k} \end{matrix}) \end{aligned}

$\begin{align*} \mathbf{\hat{x}}_{k} & =\left(\begin{array}[c]{c}\hat{x}_{k}\\ \hat{\dot{x}}_{k}\end{array} \right) \end{align*}$

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

kalman-filters

— Stochastisch
quelle

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

Ä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

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

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 : $x_k$

\begin{matrix} x_{k + 1} & = & x_{k} + {\dot{x}}_{k} Δ t + \frac{1}{2} ein (Δ t)^{2} \\ {\dot{x}}_{k + 1} & = & {\dot{x}}_{k} + ein Δ t \end{matrix}

$\begin{array} xx_{k+1} &=& x_{k} + \dot{x}_{k} \Delta t + \frac{1}{2} a (\Delta t)^2\\ \dot{x}_{k+1} &=& \dot{x}_{k} + a \Delta t \end{array}$

Wenn unser Zustand wie zuvor ist: dann lautet die Zustandsaktualisierungsgleichung: wobei nun unser die normalverteilte Beschleunigung ist.

\begin{aligned} x_{k} & = (\begin{matrix} x_{k} \\ {\dot{x}}_{k} \end{matrix}) \end{aligned}

$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$

x_{k + 1} = (\begin{matrix} 1 Δ t \\ 0 1 \end{matrix}) x_{k} + (\begin{matrix} \frac{(Δ t)^{2}}{2} \\ Δ t \end{matrix}) {ein}_{k}

$\mathbf{x}_{k+1} = \left(\begin{array}[c]{c} 1\ \ \Delta t\\ 0\ \ 1\end{array} \right) \mathbf{x}_{k} + \left(\begin{array}[c]{c} \frac{(\Delta t)^2}{2} \\ \Delta t \end{array} \right) a_k$

a_{k}

$a_k$

Das gibt eine andere -Matrix als die vorherige Version, aber die Matrizen und sollten gleich sein. $\mathbf{G}$ $\mathbf{F}$ $\mathbf{H}$

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!). $y$ $y$

Die folgenden Darstellungen zeigen das Ergebnis.

Diagramm 1 : und gegen die Zeit. $y$ $x_k$