Das Kalman-Filter (FC) ist ein optimaler linearer Algorithmus zum Filtern der Parameter eines dynamischen linearen Systems bei unvollständigen und verrauschten Beobachtungen. Dieser Filter wird häufig in technischen Steuerungssystemen verwendet, um die Dynamik von Änderungen in makroökonomischen Situationen oder in der öffentlichen Meinung zu bewerten.
Dieser Artikel soll den Leser mit einem Standardansatz für den Übergang von einem kontinuierlichen Modell eines dynamischen Systems, das durch ein System beliebiger linearer Differentialgleichungen beschrieben wird, zu einem diskreten Modell vertraut machen.
Versteckter TextSie sparen dem Leser auch Zeit, indem Sie Versuche vermeiden, ein Fahrrad zu erfinden und sich Kollegen in einem hässlichen Licht auszusetzen. Sei nicht wie ein Autor
Dieser Artikel soll den Leser auch dazu ermutigen, FC bei Problemen zu verwenden, bei denen auf den ersten Blick die lineare FC nicht anwendbar zu sein scheint, dies jedoch möglicherweise nicht der Fall ist.
Um einen Artikel des Autors zu schreiben, konnte der Autor trotz der Einfachheit der folgenden Dinge in den Suchergebnissen für Google sowohl auf Russisch als auch auf Englisch (zumindest auf der ersten Seite) diese nicht finden.
Dynamisches Modell für einen diskreten Kalman-Filter
Versteckter TextGrundsätzlich wird dieser Abschnitt benötigt, um den Leser in das System der akzeptierten Notation einzuführen, das sich von Buch zu Buch und von Artikel zu Artikel stark unterscheidet. Eine Erklärung der Bedeutung aller in den Gleichungen enthaltenen Größen würde den Rahmen dieses Artikels sprengen, während die Person, die ans Licht kommt, eine Vorstellung davon hat. Wenn nicht, willkommen
hier ,
hier und
hier .
FC kann sowohl in diskreter als auch in kontinuierlicher Form durchgeführt werden. Von besonderem Interesse im Hinblick auf die praktische Implementierung auf modernen Digitalcomputern ist die diskrete FC, die in diesem Artikel hervorgehoben wird.
Die lineare diskrete FC wird durch die folgenden Ausdrücke beschrieben. Das Systemmodell sei wie folgt dargestellt:
$$ display $$ \ mathbf {x} _ {k} = F \ mathbf {x} _ {k-1} + \ Psi \ mathbf {u} _k + \ Gamma \ mathbf {w} _k $$ display $$
wo $ inline $ f $ inline $ - Übergangsmatrix, $ inline $ \ Psi $ inline $ - Übergangskontrollmatrix, $ inline $ \ Gamma $ inline $ - Übergangsstörungsmatrix, $ inline $ \ mathbf {x} _k $ inline $ , $ inline $ \ mathbf {u} _k $ inline $ , $ inline $ \ mathbf {w} _k $ inline $ - Zustandsvektoren, Steuerung und Rauschen (Störung) des Systems an $ inline $ k $ inline $ dieser Schritt. Beobachtungsmodell:$$ display $$ \ mathbf {z} _k = H \ mathbf {x} _k + \ mathbf {n} _k $$ display $$
wo $ inline $ \ mathbf {z} _k $ inline $ , $ inline $ \ mathbf {n} _k $ inline $ - Beobachtungsvektoren und Beobachtungsrauschen an $ inline $ k $ inline $ dieser Schritt. 5 Arbeitsgleichungen von FC in diesem Artikel sind nicht von Interesse, daher werden sie, falls jemand sie benötigt, unter einem Spoiler angegeben.
Versteckter TextDie erste Stufe, Extrapolation:
$$ display $$ \ mathbf {x} _ {k | k-1} = F \ hat {\ mathbf {x}} _ {k-1} + \ mathbf {w} _k $$ display $$
$$ Anzeige $$ P_ {k | k-1} = FP_ {k-1} F ^ T + Q_k $$ Anzeige $$
Diese Stufe wird als Extrapolation bezeichnet. Der nächste Schritt, Korrektur genannt:
$$ Anzeige $$ K = PH ^ T (HP_ {k | k-1} H ^ T + R) ^ {- 1} $$ Anzeige $$
die Bewertung selbst
$$ display $$ \ hat {\ mathbf {x}} _ {k} = x_ {k | k-1} + K (H \ mathbf {z} _k- \ mathbf {x} _ {k | k-1 }) $$ display $$
$$ Anzeige $$ P_k = (E-KH) P_ {k | k-1} $$ Anzeige $$
Im Folgenden sprechen wir über stationäre (mit konstanten Koeffizienten) Systeme, für die die Matrizen $ inline $ f $ inline $ , $ inline $ \ Psi $ inline $ und $ inline $ \ Gamma $ inline $ unabhängig von der Anzahl $ inline $ k $ inline $ .
Kontinuierliches dynamisches Systemmodell. Zustandsraum.
In der überwiegenden Mehrheit der praktischen Anwendungen filtert FC die Parameter kontinuierlicher dynamischer Systeme, die durch Differentialgleichungen für die kontinuierliche Zeit beschrieben werden. Die Berechnung der FC erfolgt in diesem Fall auf einem digitalen Computer, wodurch die FC automatisch diskret wird und das Modell dementsprechend diskret sein sollte. Um ein diskretes Modell dieser kontinuierlichen Systeme zu erhalten, ist es zunächst erforderlich, den Zustandsvektor selbst (Phasenvektor), das Zustandsgleichungssystem, zusammenzusetzen und diese dann zu diskretisieren, wodurch Matrizen erhalten werden $ inline $ f $ inline $ , $ inline $ \ Psi $ inline $ und $ inline $ \ Gamma $ inline $ .
Lassen Sie das Systemverhalten durch eine Reihe von beschreiben $ inline $ n $ inline $ Differentialgleichungen erster Ordnung:$$ Anzeige $$ \ Punkt {\ mathbf {x}} (t) = A \ mathbf {x} (t) + B \ mathbf {u} (t) + G \ mathbf {w} (t) $$ Anzeige $$
hier $ inline $ \ mathbf {x} $ inline $ - $ inline $ n $ inline $ -dimensionaler Zustandsvektor des Systems. Der Zustandsvektor (auch Phasenvektor genannt) ist ein Vektor, der Variablen enthält, die das System und seine Ableitungen bis zur erforderlichen Reihenfolge beschreiben. $ inline $ \ mathbf {u} $ inline $ - $ inline $ r $ inline $ -dimensionaler Kontrollvektor des Systems, der den kontrollierten Effekt beschreibt, der auf das System ausgeübt wird.
$ inline $ \ mathbf {w} $ inline $ $ inline $ p $ inline $ -dimensionaler Vektor, der einen zufälligen unkontrollierten Effekt auf das System oder Rauschen enthält. $ inline $ a $ inline $ - Matrix der Systemstatusgröße $ inline $ n \ times n $ inline $ . $ inline $ B $ inline $ - Größenkontrollmatrix $ inline $ n \ times r $ inline $ . $ inline $ g $ inline $ - Störungsmatrix der Größe $ inline $ n \ times p $ inline $ . In diesem Ausdruck werden alle Produkte nach den Regeln der Matrixmultiplikation berechnet. Im allgemeinen Fall sind die Elemente aller Matrizen Funktionen der Zeit, jedoch werden im Artikel nur stationäre Systeme berücksichtigt, bei denen die Elemente zeitunabhängig sind.
Ein Beispiel für einen Übergang von einer Systembeschreibung unter Verwendung einer Differentialgleichung höherer Ordnung zu einer Beschreibung durch einen Zustandsraum ist unten angegeben.
BeispielLassen Sie die Bewegung eines Punktes entlang einer Achse
$ inline $ ox $ inline $ wird durch eine Differentialgleichung zweiter Ordnung beschrieben:
$$ display $$ \ ddot {x} = - \ omega ^ 2 x $$ display $$
Wenn man sich nicht erinnert, wird somit eine oszillierende Bewegung dargestellt. Wir bewegen uns von einer Gleichung zweiter Ordnung zu einem System aus zwei Gleichungen, indem wir eine neue Variable einführen
$ inline $ x_1 = \ dot {x} $ inline $ . Jetzt haben wir:
$$ display $$ \ begin {align} \ dot {x} & = x_1 \\ \ dot {x} _1 & = - \ omega ^ 2 x \ end {align} $$ display $$
Dieses Gleichungssystem kann in Matrixform geschrieben werden, während der Zustandsvektor
$ inline $ \ mathbf {x} = [x \, x_1] ^ T $ inline $ wird die Zustandsmatrix sein
$$ display $$ A = \ begin {bmatrix} 0 & 1 \\ - \ omega ^ 2 & 0 \ end {bmatrix} $$ display $$
Variable eingegeben
$ inline $ x_1 $ inline $ spielt die Rolle der Geschwindigkeit. Matrizen
$ inline $ B $ inline $ und
$ inline $ g $ inline $ In diesem Beispiel sind sie Null, da es keine Kontrolle und störenden Einflüsse gibt.
Diskreter Übergang
Für den korrekten Übergang zum diskreten Bereich (mit anderen Worten zur Diskretisierung des Modells) müssen wir das Konzept eines Matrixexponenten einführen. Ein Matrixexponent ist eine Matrixfunktion, die in Analogie zur Erweiterung der Exponentialfunktion in einer Taylorreihe in der Tat von Maclaurin erhalten wird :
$$ display $$ e ^ {At} = E + At + \, ... \, \ dfrac {A ^ nt ^ n} {n!} + \, ... \, = \ sum_ {k = 0 } ^ {\ infty} \ dfrac {A ^ nt ^ n} {n!} $$ display $$
wo unter $ inline $ E $ inline $ impliziert eine Einheitsmatrix.
Der genaue Übergang von einem kontinuierlichen Modell im Zustandsraum zu einem diskreten Modell erfordert die Suche nach einer Lösung für ein homogenes System $ inline $ \ dot {\ mathbf {x}} (t) = A (t) \ mathbf {x} (t) $ inline $ , dann der Übergang zum ursprünglichen System, eine gemeinsame Lösung und Integration vom ersten Moment an zu finden $ inline $ t_0 $ inline $ zu einigen $ inline $ t $ inline $ . Eine strikte Schlussfolgerung findet sich in [1], hier wird ein fertiges Ergebnis präsentiert.
Bei Stationarität eines kontinuierlichen dynamischen Modells (unabhängig von Matrizen) $ inline $ a $ inline $ , $ inline $ B $ inline $ , $ inline $ g $ inline $ Um ein diskretes Modell zu erhalten, können wir eine Hilfsübergangsmatrix des Systems einführen $ inline $ \ Phi (t, \ tau) $ inline $ von dem Moment an $ inline $ \ tau $ inline $ im Moment $ inline $ t $ inline $ wo $ inline $ t> \ tau $ inline $ :$$ display $$ \ Phi (t, \ tau) = e ^ {A (t- \ tau)} = \ sum_ {k = 0} ^ {\ infty} \ dfrac {A ^ nt ^ n} {n! } $$ display $$
Ferner können unter Verwendung dieser Hilfsmatrix die für ein diskretes Modell erforderlichen Matrizen erhalten werden:$$ Anzeige $$ F = \ Phi (t + T, t) = e ^ {AT} = E + AT + \ dfrac {A ^ 2T ^ 2} {2!} + \ dfrac {A ^ 3T ^ 3} {3!} + ... $$ display $$
$$ Anzeige $$ \ Gamma = \ int_ {kT} ^ {(k + 1) T} \ Phi (t_ {k + 1}, \ tau) G (\ tau) d \ tau $$ Anzeige $$
$$ Anzeige $$ \ Psi = \ int_ {kT} ^ {(k + 1) T} \ Phi (t_ {k + 1}, \ tau) B (\ tau) d \ tau $$ Anzeige $$
Hier unter $ inline $ B (\ tau) $ inline $ und $ inline $ G (\ tau) $ inline $ wir meinen Matrizen aus stetigen Gleichungen mit $ inline $ \ Psi $ inline $ und $ inline $ \ Gamma $ inline $ erforderliche Matrizen eines diskreten Modells.
Praktische Beispiele
Versteckter TextLeider haben die Beispiele nur Perversionen mit der Matrix $ inline $ f $ inline $ Da der Autor zu faul ist, um Beispiele mit Kontrollaktionen zu erfinden, befasst er sich im Rahmen seiner Dissertation im Allgemeinen mit der Navigation, bei der es keine Kontrollaktionen gibt. Darüber hinaus sollten diese Aktionen mit dem rudimentären Wissen der mathematischen Analyse nach dem Parsen von Beispielen keine Probleme verursachen. Für Beispiele ungleich Null $ inline $ \ Gamma $ inline $ und $ inline $ \ Psi $ inline $ kann zu [2] gehen.
Betrachten Sie zur Veranschaulichung der obigen Mathematik zwei Beispiele. Eines davon ist das Aufwärmen und das zweite ist veranschaulichend, um die Fähigkeiten des beschriebenen Verfahrens zu demonstrieren.
Trivial
Lassen Sie das Objekt entlang der Achse bewegen $ inline $ ox $ inline $ mit Anfangsgeschwindigkeit $ inline $ v_0 $ inline $ und konstante Beschleunigung $ inline $ a $ inline $ . Dann kann sein Modell dargestellt werden als:
$$ display $$ \ ddot {x} = ein $$ display $$
Wir stellen dieses Modell in Form eines Systems homogener Differentialgleichungen dar. Dazu teilen wir die Gleichung in ein System von drei Fernbedienungen:$$ display $$ \ begin {align} \ dot {x} & = v_x \\ \ dot {v} _x & = a_x \\ \ dot {a} _x & = 0 \ end {align} $$ display $$
Beim Schreiben von Gleichungssystemen werden dort die folgenden Ableitungen hinzugefügt, soweit die folgenden erforderlich sind, um die aktuelle zu berechnen. Dann können Sie im aktuellen System nicht aufhören $ inline $ v_x $ inline $ , da die Berechnung erfordert $ inline $ a_x $ inline $ . Zur gleichen Zeit zu berechnen $ inline $ a_x $ inline $ Derivat $ inline $ \ dot {a} _x $ inline $ Es ist daher nicht erforderlich, Derivate der oben genannten Reihenfolge einzuführen $ inline $ a_x $ inline $ im Zustand macht Vektor nicht viel Sinn.
Kombinieren Sie drei Variablen zu einem Zustandsvektor $ inline $ \ mathbf {x} = [x \, v_x \, a_x] ^ T $ inline $ und schreibe das Gleichungssystem in Matrixform für den Übergang in die Zustandsraumform:$$ display $$ \ dot {\ mathbf {x}} = A \ mathbf {x} $$ display $$
Wo ist die Matrix?
$$ display $$ A = \ begin {bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \ end {bmatrix} $$ display $$
Nun können wir die Übergangsmatrix des diskreten dynamischen Systems berechnen, die der betrachteten kontinuierlichen entspricht:
$ inline $ \ begin {ausgerichtet} F = E + A \ cdot T + A \ mal A \ cdot \ dfrac {T ^ 2} {2} = \ begin {bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \ end {bmatrix} + \ begin {bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \ end {bmatrix} \ cdot T + \\ \ begin {bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \ end {bmatrix} \ cdot \ dfrac {T ^ 2} {2} = \ begin {bmatrix} 1 & T & T ^ 2/2 \\ 0 & 1 & T \\ 0 & 0 & 1 \ end {bmatrix} \ end {align} $ inline $
Der Leser kann dies überprüfen $ inline $ A ^ 3 $ inline $ und oben ist eine Nullmatrix.
Auf diese Weise wurde eine allen bekannte Übergangsmatrix erhalten, die ohne Anwendung von Annahmen abgeleitet wurde.
Nicht triviales Beispiel
Wir nehmen an, dass sich unser Objekt im dreidimensionalen Raum mit einer bestimmten konstanten (Modulo) Lineargeschwindigkeit und mit einer Winkelgeschwindigkeit bewegt, die durch einen Pseudovektor dargestellt wird:$$ display $$ \ omega = [\ omega_x \, \ omega_y \, \ omega_z] ^ T $$ display $$
Zuerst müssen Sie die Gleichungen des Zustandsraums erstellen. Wir schreiben die Beschleunigung, wenn wir uns um den Kreis bewegen. Aus dem Physikkurs für 1 Semester ist bekannt, dass die Zentripetalbeschleunigung ein Vektorprodukt von Winkel- und Lineargeschwindigkeiten ist:$$ display $$ \ dot {v} = \ omega \ times v $$ display $$
Hier ist der Geschwindigkeitsvektor $ inline $ v = [v_x \, v_y \, v_z] ^ T $ inline $ .
Wir werden das Vektorprodukt genauer schreiben:
$$ display $$ \ omega \ times v = \ begin {bmatrix} \ omega_x \\ \ omega_y \\ \ omega_z \ end {bmatrix} \ times \ begin {bmatrix} v_x \\ v_y \\ v_z \ end {bmatrix} = \ begin {bmatrix} \ omega_yz- \ omega_zy \\ \ omega_zx- \ omega_xz \\ \ omega_xy- \ omega_yx \ end {bmatrix} $$ display $$
Nun schreiben wir das Gleichungssystem
$$ display $$ \ begin {align} \ dot {x} & = v_x \\ \ dot {y} & = v_y \\ \ dot {z} & = v_z \\ \ dot {v} _x & = \ omega_yz - \ omega_zy \\ \ dot {v} _y & = \ omega_zx- \ omega_xz \\ \ dot {v} _z & = \ omega_xy- \ omega_yx \ end {align} $$ display $$
Beim Übergang zur Matrixform wird die Matrix $ inline $ a $ inline $ wird sein:
$$ display $$ A = \ begin {bmatrix} 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & \ 0 & 0 & 0 & 0 & - \ omega_z & \ omega_y \\ 0 & 0 & 0 & \ omega_z & 0 & - \ omega_x \\ 0 & 0 & 0 & - \ omega_y & \ omega_x & 0 \ end { bmatrix} $$ display $$
Als nächstes fahren wir mit der Matrix fort $ inline $ f $ inline $ durch den entsprechenden Ausdruck. Da oral Matrizen mit Größe multiplizieren $ inline $ 6 \ times 6 $ inline $ dreimal ist ziemlich schwierig, die Fehlerwahrscheinlichkeit ist hoch, und dies ist keine königliche Angelegenheit, dann werden wir ein Skript unter Verwendung der Python-Sympy-Bibliothek schreiben:
from sympy import symbols, Matrix, eye x, y, z, T = symbols('xyz T') vx, vy, vz = symbols('v_x v_y v_z') wx, wy, wz = symbols('w_x w_y w_z') A = Matrix([ [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1], [0, 0, 0, 0, -wz, wy], [0, 0, 0, wz, 0, -wx], [0, 0, 0, -wy, wx, 0] ]) F = eye(6) + A*T + A*A*T**2/2 from sympy import latex print(latex(F))
Und wenn wir es laufen lassen, bekommen wir so etwas:
Versteckter Text \left[\begin{matrix}1 & 0 & 0 & T & - \frac{T^{2} w_{z}}{2} & \frac{T^{2} w_{y}}{2}\\0 & 1 & 0 & \frac{T^{2} w_{z}}{2} & T & - \frac{T^{2} w_{x}}{2}\\0 & 0 & 1 & - \frac{T^{2} w_{y}}{2} & \frac{T^{2} w_{x}}{2} & T\\0 & 0 & 0 & \frac{T^{2} \left(- w_{y}^{2} - w_{z}^{2}\right)}{2} + 1 & \frac{T^{2} w_{x} w_{y}}{2} - T w_{z} & \frac{T^{2} w_{x} w_{z}}{2} + T w_{y}\\0 & 0 & 0 & \frac{T^{2} w_{x} w_{y}}{2} + T w_{z} & \frac{T^{2} \left(- w_{x}^{2} - w_{z}^{2}\right)}{2} + 1 & \frac{T^{2} w_{y} w_{z}}{2} - T w_{x}\\0 & 0 & 0 & \frac{T^{2} w_{x} w_{z}}{2} - T w_{y} & \frac{T^{2} w_{y} w_{z}}{2} + T w_{x} & \frac{T^{2} \left(- w_{x}^{2} - w_{y}^{2}\right)}{2} + 1\end{matrix}\right]
Das nach dem Einrahmen mit geeigneten Tags und dem Einfügen in den Quellcode des Artikels wird zu:$$ display $$ F = \ left [\ begin {matrix} 1 & 0 & 0 & T & - \ frac {T ^ {2} w_ {z}} {2} & \ frac {T ^ {2} w_ {y}} {2} \\ 0 & 1 & 0 & \ frac {T ^ {2} w_ {z}} {2} & T & - \ frac {T ^ {2} w_ {x}} {2 } \\ 0 & 0 & 1 & - \ frac {T ^ {2} w_ {y}} {2} & \ frac {T ^ {2} w_ {x}} {2} & T \\ 0 & 0 & 0 & \ frac {T ^ {2} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ right)} {2} + 1 & \ frac {T ^ {2} w_ {x} w_ {y}} {2} - T w_ {z} & \ frac {T ^ {2} w_ {x} w_ {z}} {2} + T w_ {y} \\ 0 & 0 & 0 & \ frac {T ^ {2} w_ {x} w_ {y}} {2} + T w_ {z} & \ frac {T ^ {2} \ left (- w_ {x} ^ {2} - w_ {z} ^ {2} \ right)} {2} + 1 & \ frac {T ^ {2} w_ {y} w_ {z}} {2} - T w_ {x} \\ 0 & 0 & 0 & \ frac {T ^ {2} w_ {x} w_ {z}} {2} - T w_ {y} & \ frac {T ^ {2} w_ {y} w_ {z}} {2} + T w_ {x} & \ frac {T ^ {2} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2} \ right)} {2} + 1 \ end {matrix} \ right] $$ display $$
Somit kann die Kalman-Filterübergangsmatrix für Kreisbewegungen abgeleitet werden.
Im Gegensatz zum vorherigen Fall ist das Ergebnis der Konstruktion $ inline $ a $ inline $ zu einer Potenz höher als 3 ist keine Nullmatrix.
B. <math> $ inline $ A ^ 3 $ inline $ </ mat>$$ display $$ \ left [\ begin {matrix} 0 & 0 & 0 & - w_ {y} ^ {2} - w_ {z} ^ {2} & w_ {x} w_ {y} & w_ {x } w_ {z} \\ 0 & 0 & 0 & w_ {x} w_ {y} & - w_ {x} ^ {2} - w_ {z} ^ {2} & w_ {y} w_ {z} \ \ 0 & 0 & 0 & w_ {x} w_ {z} & w_ {y} w_ {z} & - w_ {x} ^ {2} - w_ {y} ^ {2} \\ 0 & 0 & 0 & 0 & w_ {x} ^ {2} w_ {z} - w_ {z} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ right) & - w_ {x} ^ {2} w_ {y} + w_ {y} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ right) \\ 0 & 0 & 0 & - w_ {y} ^ {2} w_ {z} + w_ {z} \ left (- w_ {x} ^ {2} - w_ {z} ^ {2} \ right) & 0 & w_ {x} w_ {y} ^ { 2} - w_ {x} \ left (- w_ {x} ^ {2} - w_ {z} ^ {2} \ right) \\ 0 & 0 & 0 & w_ {y} w_ {z} ^ {2 } - w_ {y} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2} \ right) & - w_ {x} w_ {z} ^ {2} + w_ {x} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2} \ right) & 0 \ end {matrix} \ right] $$ display $$
oder <math> $ inline $ A ^ 4 $ inline $ </ mat>$$ display $$ \ left [\ begin {matrix} 0 & 0 & 0 & 0 & w_ {x} ^ {2} w_ {z} - w_ {z} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ rechts) & - w_ {x} ^ {2} w_ {y} + w_ {y} \ links (- w_ {y} ^ {2} - w_ {z} ^ { 2} \ rechts) \\ 0 & 0 & 0 & - w_ {y} ^ {2} w_ {z} + w_ {z} \ links (- w_ {x} ^ {2} - w_ {z} ^ { 2} \ rechts) & 0 & w_ {x} w_ {y} ^ {2} - w_ {x} \ links (- w_ {x} ^ {2} - w_ {z} ^ {2} \ rechts) \ \ 0 & 0 & 0 & w_ {y} w_ {z} ^ {2} - w_ {y} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2} \ right) & - w_ {x} w_ {z} ^ {2} + w_ {x} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2} \ right) & 0 \\ 0 & 0 & 0 & - w_ {y} \ left (- w_ {x} ^ {2} w_ {y} + w_ {y} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ right ) \ rechts) + w_ {z} \ links (w_ {x} ^ {2} w_ {z} - w_ {z} \ links (- w_ {y} ^ {2} - w_ {z} ^ {2} \ rechts) \ rechts) & w_ {x} \ links (- w_ {x} ^ {2} w_ {y} + w_ {y} \ links (- w_ {y} ^ {2} - w_ {z} ^ {2} \ rechts) \ rechts) & - w_ {x} \ links (w_ {x} ^ {2} w_ {z} - w_ {z} \ links (- w_ {y} ^ {2} - w_ { z} ^ {2} \ rechts) \ rechts) \\ 0 & 0 & 0 & - w_ {y} \ links (w_ {x} w_ {y} ^ {2} - w_ {x} \ links (- w_ {x} ^ {2} - w_ {z} ^ {2} \ rechts) \ rechts) & w_ {x} \ links (w_ {x} w_ {y} ^ {2} - w_ {x} \ links ( - w_ {x} ^ {2} - w_ {z} ^ {2} \ rechts) \ rechts) - w_ {z} \ links (- w _ {y} ^ {2} w_ {z} + w_ {z} \ left (- w_ {x} ^ {2} - w_ {z} ^ {2} \ right) \ right) & w_ {y} \ links (- w_ {y} ^ {2} w_ {z} + w_ {z} \ links (- w_ {x} ^ {2} - w_ {z} ^ {2} \ rechts) \ rechts) \\ 0 & 0 & 0 & w_ {z} \ left (- w_ {x} w_ {z} ^ {2} + w_ {x} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2 } \ rechts) \ rechts) & - w_ {z} \ links (w_ {y} w_ {z} ^ {2} - w_ {y} \ links (- w_ {x} ^ {2} - w_ {y} ^ {2} \ rechts) \ rechts) & - w_ {x} \ links (- w_ {x} w_ {z} ^ {2} + w_ {x} \ links (- w_ {x} ^ {2} - w_ {y} ^ {2} \ rechts) \ rechts) + w_ {y} \ links (w_ {y} w_ {z} ^ {2} - w_ {y} \ links (- w_ {x} ^ {2 } - w_ {y} ^ {2} \ right) \ right) \ end {matrix} \ right] $$ display $$
Daher ist die Darstellung einer solchen Matrix mit endlicher Genauigkeit möglich. Wann jedoch $ inline $ \ omega T \ ll 1 $ inline $ Reihen in Matrixelementen erhalten $ inline $ f $ inline $ ziemlich schnell konvergieren. Für den praktischen Gebrauch genug Mitglieder bis zum zweiten Grad, selten bis zum dritten und noch mehr bis zum vierten.
Zusätzlich veranschaulichen wir die Funktionsweise der Matrix $ inline $ f $ inline $ fragender Vektor $ inline $ \ omega $ inline $ , $ inline $ \ bf {x} _0 $ inline $ , $ inline $ \ bf {v} _0 $ inline $ und eine Wiederholungssequenz des Formulars:$$ display $$ \ mathbf {x} _k = F \ mathbf {x} _ {k-1} $$ display $$
Wir berechnen diese Wiederholungssequenz für $ inline $ \ omega T \ approx \ frac {1} {100} $ inline $
Python-Code import numpy as np from numpy import pi T = 1 wx, wy, wz = 0, 2*pi/100/2**.5, 2*pi/100/2**.5 vx0 = 10 A = np.array([ [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1], [0, 0, 0, 0, -wz, wy], [0, 0, 0, wz, 0, -wx], [0, 0, 0, -wy, wx, 0] ]) F = np.eye(6) + A * T + A @ A * T**2/2 + A @ A @ A * T**3/6 X = np.zeros((6, 101)) X[:, 0] = np.array([0, 0, 0, vx0, 0, 0]) for k in range(X.shape[1] - 1): X[:, k + 1] = F @ X[:, k] import matplotlib as mpl import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D fig = plt.figure() ax = fig.gca(projection='3d') ax.plot(X[0, :], X[1, :], X[2, :]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') plt.show()
Ich möchte Sie daran erinnern, dass für den Typ np.array das Symbol "@" die Matrixmultiplikation bezeichnet. Entfernungen und Geschwindigkeiten werden in Papageien gemessen, Winkelgeschwindigkeit in rad / s. Es ist auch zu beachten, dass zum Erhalten eines Kreises die Geschwindigkeits- und Winkelgeschwindigkeitsvektoren senkrecht sind, da sonst eine Spirale anstelle eines Kreises erhalten wird.
Infolgedessen kann man durch Einstellen einer bestimmten Anfangsposition, Geschwindigkeit und Winkelgeschwindigkeit eine solche Flugbahn erhalten
Die Genauigkeit der Übereinstimmung des ersten und des letzten Punktes kann als erhalten werden
>>> print(X[:3, 0] - X[:3,-1]) [-0.00051924 -0.0072984 0.0072984 ]
Bei einem Wenderadius in der Größenordnung von 150 Einheiten überschreitet der relative Fehler nicht die Auftragswerte $ inline $ 5 \ cdot 10 ^ {- 5} $ inline $ . Diese Genauigkeit reicht für das FC-Modell, das das Wendeziel überwacht, völlig aus.
Fazit
Wenn früher FC hauptsächlich zur Lösung von Navigationsproblemen verwendet wurde, bei denen die Verwendung von linearen Bewegungsmodellen zu einem guten Ergebnis führte, hat mit der Entwicklung moderner Anwendungen wie Robotik, Computer Vision usw. der Bedarf an komplexeren Modellen für die Bewegung von Objekten zugenommen. Darüber hinaus ermöglicht die Anwendung des obigen Ansatzes die Synthese eines diskreten FC-Modells ohne besondere Kosten, was die Aufgabe für Entwickler erleichtert. Die einzige Einschränkung dieses Ansatzes besteht darin, dass das kontinuierliche Modell eines dynamischen Systems durch einen Satz linearer oder zumindest linearisierbarer Gleichungen im Zustandsraum beschrieben werden muss.
Zusammenfassend können wir einen Algorithmus für die Synthese der Übergangsmatrix FC angeben:
- Schreiben der Differentialgleichung des Systems
- Übergang zum Zustandsvektor und Zustandsraum
- Linearisierung bei Bedarf
- Darstellung der Übergangsmatrix in Form eines Matrixexponenten und ggf. Abschneiden der Reihe
- Berechnung der verbleibenden Matrizen unter Berücksichtigung der Übergangsmatrix
Der Autor begrüßt konstruktive Kritik an gemachten Fehlern, Ungenauigkeiten, falschen Formulierungen, nicht erwähnten Methoden und vielem mehr. Vielen Dank für Ihre Aufmerksamkeit!
Gebrauchte Literatur
[1] Medic J. Statistisch optimale lineare Schätzungen und Kontrolle. Per. aus dem Englischen Ed. A.S. Shatalova Moskau. Verlag "Energy", 1973, 440 S.
[2] Matveev V.V. Grundlagen des Aufbaus von Strapdown-Trägheitssystemen in St. Petersburg: Staatliches Forschungszentrum der Russischen Föderation OJSC Concern TsNII Elektropribor, 2009 - 280er Jahre. ISBN 978-5-900180-73-3