Kalman Filter - Implementierung, Parameter und Optimierung

Zunächst einmal versuche ich zum ersten Mal, einen Kalman-Filter zu erstellen.

Ich habe zuvor die folgende Frage gestellt: Herausfiltern von Geräuschen und Abweichungen von Geschwindigkeitswerten auf StackOverflow, die den Hintergrund für diesen Beitrag beschreibt. Dies ist eine typische Stichprobe von Werten, die ich filtern möchte. Sie müssen nicht unbedingt abnehmen, was hier der Fall ist. Die Änderungsrate ist jedoch normalerweise so

X ------- Y
16 --- 233,75
24 --- 234,01
26 --- 234,33
32 --- 234,12
36 --- 233,85
39 --- 233,42
47 --- 233,69
52 --- 233,68
55 --- 233,76
60 --- 232,97
66 --- 233,31
72 --- 233,99

Ich habe meinen Kalman-Filter gemäß diesem Tutorial implementiert: Kalman-Filter für Dummies .

Meine Implementierung sieht so aus (Pseudocode).

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

Ich beginne mit einer vorherigen Schätzung von 0. Dies scheint gut zu funktionieren. Was ich jedoch bemerke, ist, dass der kalmanGain jedes Mal abnimmt, wenn dieses Update ausgeführt wird, was bedeutet, dass ich meinen neuen Werten weniger vertraue, je länger mein Filter ausgeführt wird (?). Das will ich nicht

Ich ging von einem gleitenden Durchschnitt (einfach und exponentiell gewichtet) zu diesem über. Im Moment kann ich nicht einmal so gute Ergebnisse erzielen.

Meine Frage ist, ob dies die richtige Implementierung ist und ob meine vorherige Fehlervarianz und Standardabweichung gemäß den von mir veröffentlichten Beispielwerten gut aussieht. Meine Parameter werden eigentlich nur zufällig ausgewählt, um zu sehen, ob ich gute Ergebnisse erzielen kann. Ich habe verschiedene Bereiche ausprobiert, aber mit schlechten Ergebnissen. Wenn Sie Vorschläge zu Änderungen haben, die ich vornehmen kann, wäre ich Ihnen sehr dankbar. Es tut mir leid, wenn einige offensichtliche Dinge fehlen. Auch hier zum ersten Mal posten.

Kalman-Filter sind nützlich, wenn Ihr Eingangssignal aus verrauschten Beobachtungen des Zustands eines linearen dynamischen Systems besteht. Angesichts einer Reihe von Beobachtungen des Systemzustands zielt der Kalman-Filter darauf ab, rekursiv immer bessere Schätzungen des zugrunde liegenden Systemzustands bereitzustellen. Um es erfolgreich anwenden zu können, benötigen Sie ein Modell für die Dynamik des Systems, dessen Status Sie schätzen. Wie bei Wikipedia ausführlich beschrieben , beschreibt dieses Modell, wie sich das zugrunde liegende System des Systems voraussichtlich in einem Zeitschritt ändern wird, wenn es seinen vorherigen Zustand, etwaige Eingaben in das System und eine Gauß-verteilte stochastische Komponente namens Prozessrauschen aufweist.

Vor diesem Hintergrund ist aus Ihrer Frage nicht ersichtlich, ob Sie ein solches zugrunde liegendes Modell haben. Der verlinkte Beitrag zeigte an, dass Sie Geschwindigkeitswerte von einem Sensor verbrauchen. Diese können als direkte Beobachtungen des Zustands eines Systems (wo der Zustand seine Geschwindigkeit ist) oder indirekte Beobachtungen seines Zustands (wo der Zustand beispielsweise seine Position ist) modelliert werden. Um das Kalman-Framework verwenden zu können, müssen Sie jedoch ein Modell auswählen, wie sich dieser Zustand im Laufe der Zeit voraussichtlich entwickeln wird. Diese zusätzlichen Informationen werden verwendet, um die optimale Schätzung zu generieren. Ein Kalman-Filter ist keine magische Blackbox, die nur ein Signal "bereinigt", das an ihn angelegt wird.

Vor diesem Hintergrund tritt das von Ihnen angedeutete Phänomen in der Praxis in der Praxis auf, bei dem der Kalman-Filter zunehmend mehr Vertrauen in seine eigene Ausgabe erhält, bis Eingangsbeobachtungen zunehmend ignoriert werden. Dies kann durch manuelles Erhöhen der Werte in der Prozessrausch-Kovarianzmatrix gemindert werden. Dann enthält das Modell für den Zustandsübergang des Systems qualitativ eine größere stochastische Komponente, so dass die Fähigkeit des Schätzers, den nächsten Zustand unter Berücksichtigung des aktuellen Zustands genau vorherzusagen, verringert ist. Dies verringert die Abhängigkeit von der aktuellen Schätzung des Systemzustands und erhöht die Abhängigkeit von nachfolgenden Beobachtungen, wodurch der Effekt "Ignorieren der Eingabe" verhindert wird.

Wenn ich es richtig verstanden habe, haben Sie etwas, das sich bewegt, und Sie können die Geschwindigkeit beobachten, und diese Geschwindigkeit ist laut. Bei Ihren Messungen beobachten Sie zwei Arten von Variationen. \

1. Durch das Rauschen verursachte Schwankungen
2. Variationen, weil das Objekt die Geschwindigkeit wirklich ändert (z. B. Drehen)

Der Grund, warum Ihre Kalman-Verstärkung auf Null geht, ist, dass Sie implizit angenommen haben, dass die Geschwindigkeit des Objekts konstant ist, und dass Sie nur diese wahre Geschwindigkeit schätzen müssen.

" Hey, ich habe ein Objekt, das sich mit konstanter Geschwindigkeit bewegt, und ich möchte diese konstante Geschwindigkeit schätzen. "

Ihr Modell ist wie folgt: ist die Geschwindigkeit zum Zeitpunkt und ist die entsprechende Messung.$x_k$$k$$y_k$

$$x_k = x_{k-1}$$ $$y_k = x_k + q_k$$

Aber Ihr Objekt bewegt sich nicht so. Die Geschwindigkeit ändert sich und Sie wissen nicht, wie und wann sie sich ändern wird.

Was Sie stattdessen zu sagen haben, ist:

" Hey, ich habe ein Objekt, das sich mit einer Geschwindigkeit bewegt, aber ich bin mir nicht sicher, wie es seine Geschwindigkeit ändert. "

Es gibt viele Möglichkeiten, wie Sie dies tun können: Die einfachste Möglichkeit besteht darin, Ihrem Zustand Unsicherheit zu verleihen.

$$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$$ $$y_k = x_k + q_k$$ wobei und als weißes Rauschen angenommen werden.$q_k$$v_k$

Ihre Kalman-Filtergleichungen sehen folgendermaßen aus:

$$\hat y_{k|k-1} = \hat x_{k|k-1}$$ $$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$$ $$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$$ $$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$$ $$P_{k+1|k} =P_{k|k} + Q^s$$

In Ihrem Fall ist der 0.05Wert die Kovarianz des Beobachtungsrauschens . Um diese Änderung vorzunehmen, müssen Sie lediglich , die Zustandsrausch-Covarinace (die Ungewissheit in Ihrem Zustand), auf einen konstanten Wert setzen.Q $Q^o$$Q^s$

In Ihrem Code wäre die geringfügige Änderung:

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

Indem Sie die stateVarianceoder in Ihren Code einfügen, haben Sie angenommen, dass es Null ist.$Q^s$

Dieser stateVarianceWert kann beliebig sein. Es basiert auf Ihrem Vertrauen, wie stark sich die Geschwindigkeit tatsächlich ändern wird. Wenn Sie glauben, dass die Geschwindigkeit ziemlich konstant bleibt, stellen Sie diese auf eine kleine Zahl ein.

Auf diese Weise geht Ihr Kalman-Gewinn nicht auf Null.

Sie benötigen ein dynamisches System, um einen Kalman-Filter verwenden zu können.

ich würde vorschlagen

$$y = \sum\limits_{i =0}^n a_i\, x^i$$

c o v ( w ) = Q z = n ∑ i = 0 a

$$a[k+1] = a[k] + w$$ $$cov(w) = Q$$ Messung: $$z = \sum\limits_{i =0}^n a_i\, x^i = y$$

Anstatt als Zustände zu verwenden, führen Sie den Koeffizienten ( ) als Zustände ein$x$$a$

Ich denke, Sie könnten einige Ideen aus der klassischen Regelungstheorie verwenden, z . B. PID-Regler .

Ihr Signal Y kann der Sollwert des Reglers u (t) sein. Die verfahrenstechnische Anlage ist nur 1 und y (t) wird gefiltert ausgegeben. Alles was Sie tun müssen, ist die Parameter (Melodie) P, I und D einzustellen, um das zu bekommen, was Sie wollen.

Der Ausgang y (t) versucht, dem Eingang u (t) zu "folgen", aber die Parameter steuern, wie diese Verfolgung sein wird.

Die Differenzverstärkung D macht Ihre Reaktion empfindlich gegenüber schnellen Fehleränderungen. In Ihrem Fall denke ich, dass D klein sein sollte. Sie möchten nicht, dass sich y (t) ändert, wenn sich u (t) abrupt ändert.

Die integrale Verstärkung 'I' macht Ihre Antwort empfindlich gegenüber akkumulierten Fehlern. Sie sollten dort einen hohen Wert legen. Wenn u (t) den Pegel ändert und dort bleibt, baut sich der Fehler auf und Sie möchten, dass y (t) dasselbe tut.

Die P-Verstärkung kann eine Feinabstimmung ergeben. Versuchen Sie auf jeden Fall, mit den Parametern zu spielen und zu sehen, was Sie bekommen.

Es gibt zwar komplizierte Abstimmungsmethoden, aber ich glaube nicht, dass Sie sie brauchen werden.

Viel Glück.

Hier ist eine einfache und übersichtliche Implementierung von Kalman Filter, die den Notationen wie auf der Wikipedia-Seite folgt. https://github.com/zziz/kalman-filter