Wie zu verwenden Kalman-filter in Python für Standort-Daten?

TL;DR, finden Sie den code und Bild auf der Unterseite.

Ich denke, dass ein Kalman-filter kann ganz gut funktionieren, in Ihrer Anwendung, aber es erfordert ein wenig mehr Gedanken über die Dynamik/Physik der kite.

Ich würde dringend empfehlen, das Lesen diese Webseite. Ich habe keine Verbindung zu oder der Kenntnis des Autors, aber ich habe über einen Tag versucht, meinen Kopf Runde Kalman-Filter, und auf dieser Seite wirklich es Klick gemacht bei mir.

Kurz; für ein system ist linear, und die bekannte Dynamik (d.h. wenn Sie wissen, den Staat und Eingänge, Sie können die Zukunft Vorhersagen-Zustand), es bietet eine optimale Kombination, was Sie wissen über ein system zu schätzen, ist es wahre Staat. Der schlaue (die kümmern sich alle matrix-algebra, die Sie sehen, auf den Seiten, es zu beschreiben) ist, wie es optimal verbindet die beiden Stücke von Informationen, die Sie haben:

1. Messungen (die unter "Messung von Lärm", also sensoren nicht perfekt)

2. Dynamik (d.h. wie Sie glauben, dass Staaten sich entwickeln unterliegen Eingaben, die unter "Prozess " Rauschen", das ist nur eine Art zu sagen, das Modell nicht mit der Realität übereinstimmen perfekt).

Können Sie angeben, wie sicher Sie sind auf jeder dieser (über die co-Varianzen Matrizen R und Q beziehungsweise), und die Kalman-Gain bestimmt, wie viel Sie glauben, Ihr Modell (z.B. Ihre aktuelle Einschätzung Ihres Staates), und wie viel Sie glauben, dass Ihre Messungen.

Kurzerhand bauen wir ein einfaches Modell von Ihrem kite. Was ich Vorschlage, unten ist ein sehr einfaches Modell möglich. Sie vielleicht wissen, mehr über die Drachen und die Dynamik so schaffen kann, eine bessere.

Behandeln wir den kite wie ein Teilchen (natürlich eine Vereinfachung, eine echte kite ist ein erweiterter Körper, so hat eine Ausrichtung in 3 Dimensionen), die vier Staaten, die für die Bequemlichkeit, können wir schreiben in einem Zustand Vektor:

x = [x, x_dot, y, y_dot],

Wobei x und y sind die Positionen, und die _dot sind die Geschwindigkeiten in den einzelnen Richtungen. Ihre Frage ich nehme an, es sind zwei (verrauschte) Messungen, die wir schreiben können in einer Messung Vektor:

z = [x, y],

Können wir schreiben-down-Messung matrix (H diskutiert hier, und observation_matrices im pykalman Bibliothek):

z = Hx => H = [[1, 0, 0, 0], [0, 0, 1, 0]]

Dann müssen wir beschreiben die Dynamik des Systems. Hier gehe ich davon aus, dass keine äußeren Kräfte wirken, und dass es keine Dämpfung auf die Bewegung des Kites (mit mehr wissen können Sie in der Lage sein, besser zu machen, diese effektiv behandelt externe Kräfte und Dämpfung als unbekannt/unmodeled Störung).

In diesem Fall die Dynamik für jeden unserer Staaten in der aktuellen Probe "k" als Funktion von Zuständen in den vorherigen Proben "k-1" sind gegeben als:

x(k) = x(k-1) + dt*x_dot(k-1)

x_dot(k) = x_dot(k-1)

y(k) = y(k-1) + dt*y_dot(k-1)

y_dot(k) = y_dot(k-1)

Wobei "dt" ist die Zeit-Schritt. Wir nehmen an (x, y) - position aktualisiert wird, basierend auf der aktuellen position und Geschwindigkeit, und die Geschwindigkeit bleibt unverändert. Gegeben, dass keine Einheiten gegeben sind, können wir nur sagen, die velocity-Einheiten sind solche, die wir weglassen können "dt" aus den obigen Gleichungen, D. H. in Einheiten von position_units/sample_interval (ich nehme an, Ihr gemessenen Proben sind in einem Konstanten Intervall). Wir können zusammenfassen, dass diese vier Gleichungen in ein Dynamik-matrix als (F hier diskutiert, und transition_matrices im pykalman Bibliothek):

x(k) = Fx(k-1) => F = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]].

Können wir nun mal an mit dem Kalman-filter in python. Geändert von deine-code:

from pykalman import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import time

measurements = np.asarray([(399,293),(403,299),(409,308),(416,315),(418,318),(420,323),(429,326),(423,328),(429,334),(431,337),(433,342),(434,352),(434,349),(433,350),(431,350),(430,349),(428,347),(427,345),(425,341),(429,338),(431,328),(410,313),(406,306),(402,299),(397,291),(391,294),(376,270),(372,272),(351,248),(336,244),(327,236),(307,220)])

initial_state_mean = [measurements[0, 0],
                      0,
                      measurements[0, 1],
                      0]

transition_matrix = [[1, 1, 0, 0],
                     [0, 1, 0, 0],
                     [0, 0, 1, 1],
                     [0, 0, 0, 1]]

observation_matrix = [[1, 0, 0, 0],
                      [0, 0, 1, 0]]

kf1 = KalmanFilter(transition_matrices = transition_matrix,
                  observation_matrices = observation_matrix,
                  initial_state_mean = initial_state_mean)

kf1 = kf1.em(measurements, n_iter=5)
(smoothed_state_means, smoothed_state_covariances) = kf1.smooth(measurements)

plt.figure(1)
times = range(measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
         times, measurements[:, 1], 'ro',
         times, smoothed_state_means[:, 0], 'b--',
         times, smoothed_state_means[:, 2], 'r--',)
plt.show()

Produziert die folgenden zeigen, es macht einen angemessenen job, der Ablehnung der Lärm (blau ist die x-position, rot ist die y-position und x-Achse ist nur Beispiel-Zahl).

Nehme an, Sie schauen auf das Grundstück vor und denke, es sieht holprig. Wie könnte man das beheben? Wie oben erörtert, ein Kalman-filter ist die auf zwei Stücke von Informationen:

1. Messungen (in diesem Fall von zwei unserer Mitgliedstaaten, x und y)
2. System dynamics (und der aktuellen Schätzung des Staates)

Die Dynamik im Modell erfasst, die oben sind sehr einfach. Wörtlich genommen sagen Sie, dass die Positionen aktualisiert werden, indem aktuelle Geschwindigkeiten (in einem offensichtlichen, körperlich angemessenen Art und Weise), und dass die Geschwindigkeiten konstant bleiben (dies ist eindeutig nicht physisch wahr, aber fängt unsere intuition, die Geschwindigkeiten sollte sich langsam ändern).

Wenn wir denken, dass der geschätzte Zustand sein sollte, glatter, ein Weg dies zu erreichen ist, zu sagen, wir haben weniger Vertrauen in unsere Messungen als unsere Dynamik (d.h. wir haben eine höhere observation_covariance, im Vergleich zu unseren state_covariance).

Ab Ende der code oben, fixieren Sie die observation covariance zu 10-fache Wert von schätzungsweise zuvor, Einstellung em_vars wie dargestellt, ist erforderlich, um zu vermeiden re-Schätzung der Beobachtung, die Kovarianz (siehe hier)

kf2 = KalmanFilter(transition_matrices = transition_matrix,
                  observation_matrices = observation_matrix,
                  initial_state_mean = initial_state_mean,
                  observation_covariance = 10*kf1.observation_covariance,
                  em_vars=['transition_covariance', 'initial_state_covariance'])

kf2 = kf2.em(measurements, n_iter=5)
(smoothed_state_means, smoothed_state_covariances)  = kf2.smooth(measurements)

plt.figure(2)
times = range(measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
         times, measurements[:, 1], 'ro',
         times, smoothed_state_means[:, 0], 'b--',
         times, smoothed_state_means[:, 2], 'r--',)
plt.show()

Erzeugt der plot unten (Messdaten als Punkte, Staat schätzt, gepunktete Linie). Der Unterschied ist eher subtil, aber hoffentlich können Sie sehen, dass es glatter.

Schließlich, wenn Sie wollen verwenden Sie diese filter ausgestattet on-line, können Sie dies mit Hilfe der filter_update Methode. Beachten Sie, dass diese verwendet die filter - Methode, anstatt die smooth Methode, da die smooth Methode kann nur angewendet werden, um batch-Messungen. Mehr hier:

time_before = time.time()
n_real_time = 3

kf3 = KalmanFilter(transition_matrices = transition_matrix,
                  observation_matrices = observation_matrix,
                  initial_state_mean = initial_state_mean,
                  observation_covariance = 10*kf1.observation_covariance,
                  em_vars=['transition_covariance', 'initial_state_covariance'])

kf3 = kf3.em(measurements[:-n_real_time, :], n_iter=5)
(filtered_state_means, filtered_state_covariances) = kf3.filter(measurements[:-n_real_time,:])

print("Time to build and train kf3: %s seconds" % (time.time() - time_before))

x_now = filtered_state_means[-1, :]
P_now = filtered_state_covariances[-1, :]
x_new = np.zeros((n_real_time, filtered_state_means.shape[1]))
i = 0

for measurement in measurements[-n_real_time:, :]:
    time_before = time.time()
    (x_now, P_now) = kf3.filter_update(filtered_state_mean = x_now,
                                       filtered_state_covariance = P_now,
                                       observation = measurement)
    print("Time to update kf3: %s seconds" % (time.time() - time_before))
    x_new[i, :] = x_now
    i = i + 1

plt.figure(3)
old_times = range(measurements.shape[0] - n_real_time)
new_times = range(measurements.shape[0]-n_real_time, measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
         times, measurements[:, 1], 'ro',
         old_times, filtered_state_means[:, 0], 'b--',
         old_times, filtered_state_means[:, 2], 'r--',
         new_times, x_new[:, 0], 'b-',
         new_times, x_new[:, 2], 'r-')

plt.show()

Plot unten zeigt die Leistung der filter-Methode, einschließlich 3 Punkte gefunden mit der filter_update Methode. Punkte sind Messungen, gestrichelte Linie sind die staatlichen Schätzungen für die filter-training Periode, durchgezogene Linie sind Staaten die Schätzungen für "on-line" - Periode.

Und die timing-Informationen (auf meinem laptop).

Time to build and train kf3: 0.0677888393402 seconds
Time to update kf3: 0.00038480758667 seconds
Time to update kf3: 0.000465154647827 seconds
Time to update kf3: 0.000463008880615 seconds