Le filtre de Kalman (FC) est un algorithme linéaire optimal pour filtrer les paramètres d'un système linéaire dynamique en présence d'observations incomplètes et bruyantes. Ce filtre est largement utilisé dans les systèmes de contrôle technique pour évaluer la dynamique des changements dans les situations macroéconomiques ou l'opinion publique.
Cet article vise à familiariser le lecteur avec une approche standard de la transition d'un modèle continu d'un système dynamique décrit par un système d'équations différentielles linéaires arbitraires à un modèle discret.
Texte masquéainsi que gagner du temps au lecteur en éliminant les tentatives d'inventer un vélo et de s'exposer à ses collègues sous un jour laid. Ne soyez pas comme un auteur
Cet article vise également à encourager le lecteur à utiliser le FC dans les problèmes où, à première vue, il semble que le FC linéaire ne soit pas applicable, mais en fait il se peut qu'il ne le soit pas.
Pour écrire un article de l'auteur, le fait que malgré la simplicité des éléments suivants dans les résultats de recherche pour Google en russe et en anglais (au moins sur la première page), l'auteur n'a pas pu les trouver.
Modèle dynamique pour un filtre de Kalman discret
Texte masquéFondamentalement, cette section est nécessaire pour initier le lecteur au système de notation accepté, qui est très différent d'un livre à l'autre et d'un article à un autre. Une explication de la signification de toutes les quantités incluses dans les équations dépasse le cadre de cet article, alors qu'il est entendu que la personne qui vient à la lumière en a une idée. Sinon, bienvenue
ici ,
ici et
ici .
La FC peut être réalisée à la fois sous forme discrète et continue. Du point de vue de la mise en œuvre pratique sur les ordinateurs numériques modernes est particulièrement intéressant le FC discret qui sera souligné dans cet article.
Le FC discret linéaire est décrit par les expressions suivantes. Que le modèle de système soit représenté comme suit:
$$ afficher $$ \ mathbf {x} _ {k} = F \ mathbf {x} _ {k-1} + \ Psi \ mathbf {u} _k + \ Gamma \ mathbf {w} _k $$ afficher $$
où $ en ligne $ f $ en ligne $ - matrice de transition, $ inline $ \ Psi $ inline $ - matrice de contrôle de transition, $ inline $ \ Gamma $ inline $ - matrice de perturbation de transition, $ inline $ \ mathbf {x} _k $ inline $ , $ inline $ \ mathbf {u} _k $ inline $ , $ inline $ \ mathbf {w} _k $ inline $ - vecteurs d'état, contrôle et bruit (perturbation) du système sur $ en ligne $ k $ en ligne $ cette étape. Modèle d'observation:$$ afficher $$ \ mathbf {z} _k = H \ mathbf {x} _k + \ mathbf {n} _k $$ afficher $$
où $ inline $ \ mathbf {z} _k $ inline $ , $ inline $ \ mathbf {n} _k $ inline $ - vecteurs d'observation et bruit d'observation sur $ en ligne $ k $ en ligne $ cette étape. 5 équations de travail de FC dans cet article ne sont pas intéressantes, donc si quelqu'un en a besoin, elles sont données sous un spoiler.
Texte masquéLa première étape, l'extrapolation:
$$ afficher $$ \ mathbf {x} _ {k | k-1} = F \ hat {\ mathbf {x}} _ {k-1} + \ mathbf {w} _k $$ afficher $$
$$ afficher $$ P_ {k | k-1} = FP_ {k-1} F ^ T + Q_k $$ afficher $$
Cette étape est appelée extrapolation. L'étape suivante, appelée correction:
$$ afficher $$ K = PH ^ T (HP_ {k | k-1} H ^ T + R) ^ {- 1} $$ afficher $$
l'évaluation elle-même
$$ afficher $$ \ chapeau {\ mathbf {x}} _ {k} = x_ {k | k-1} + K (H \ mathbf {z} _k- \ mathbf {x} _ {k | k-1 }) $$ afficher $$
$$ afficher $$ P_k = (E-KH) P_ {k | k-1} $$ afficher $$
Nous parlons ci-après de systèmes stationnaires (à coefficients constants) pour lesquels les matrices $ en ligne $ f $ en ligne $ , $ inline $ \ Psi $ inline $ et $ inline $ \ Gamma $ inline $ indépendant du nombre $ en ligne $ k $ en ligne $ .
Modèle de système dynamique continu. Espace d'état.
Dans la grande majorité des applications pratiques, le FC filtre les paramètres des systèmes dynamiques continus décrits par des équations différentielles pour le temps continu. Le calcul du FC dans ce cas se fait sur un ordinateur numérique, ce qui rend automatiquement le FC discret et le modèle doit donc être discret. Pour obtenir un modèle discret de ces systèmes continus, il faut d'abord composer le vecteur d'état lui-même (vecteur de phase), le système d'équation d'état, puis les discrétiser, obtenant ainsi des matrices $ en ligne $ f $ en ligne $ , $ inline $ \ Psi $ inline $ et $ inline $ \ Gamma $ inline $ .
Laissez le comportement du système être décrit par un ensemble de $ en ligne $ n $ en ligne $ équations différentielles du premier ordre:$$ affichage $$ \ dot {\ mathbf {x}} (t) = A \ mathbf {x} (t) + B \ mathbf {u} (t) + G \ mathbf {w} (t) $$ display $$
ici $ inline $ \ mathbf {x} $ inline $ - $ en ligne $ n $ en ligne $ vectoriel d'état tridimensionnel du système. Le vecteur d'état (ou vecteur de phase) est un vecteur qui contient des variables qui décrivent le système et leurs dérivés jusqu'à l'ordre requis. $ inline $ \ mathbf {u} $ inline $ - $ en ligne $ r $ en ligne $ -Vecteur de contrôle dimensionnel du système qui décrit l'effet contrôlé exercé sur le système.
$ inline $ \ mathbf {w} $ inline $ $ inline $ p $ inline $ -vecteur dimensionnel contenant un effet aléatoire non contrôlé sur le système, ou bruit. $ inline $ a $ inline $ - matrice de la taille de l'état du système $ en ligne $ n \ fois n $ en ligne $ . $ en ligne $ B $ en ligne $ - matrice de contrôle de taille $ en ligne $ n \ fois r $ en ligne $ . $ en ligne $ g $ en ligne $ - matrice de perturbation de taille $ en ligne $ n \ fois p $ en ligne $ . Dans cette expression, tous les produits sont calculés selon les règles de multiplication matricielle. Dans le cas général, les éléments de toutes les matrices sont des fonctions du temps, cependant, seuls les systèmes stationnaires sont considérés dans l'article, où les éléments sont indépendants du temps.
Un exemple de transition d'une description de système utilisant une équation différentielle d'ordre supérieur à une description à travers un espace d'états est donné ci-dessous.
ExempleLaissez le mouvement d'un point le long d'un axe
$ inline $ ox $ inline $ est décrite par une équation différentielle du second ordre:
$$ afficher $$ \ ddot {x} = - \ omega ^ 2 x $$ afficher $$
Si on ne s'en souvient pas, un mouvement oscillatoire est ainsi représenté. On passe d'une équation du second ordre à un système de deux équations en introduisant une nouvelle variable
$ inline $ x_1 = \ dot {x} $ inline $ . Maintenant, nous avons:
$$ afficher $$ \ begin {aligné} \ dot {x} & = x_1 \\ \ dot {x} _1 & = - \ omega ^ 2 x \ end {aligné} $$ afficher $$
Ce système d'équations peut être écrit sous forme matricielle, tandis que le vecteur d'état
$ inline $ \ mathbf {x} = [x \, x_1] ^ T $ inline $ , la matrice d'état sera
$$ display $$ A = \ begin {bmatrix} 0 & 1 \\ - \ omega ^ 2 & 0 \ end {bmatrix} $$ display $$
Variable saisie
$ en ligne $ x_1 $ en ligne $ joue le rôle de la vitesse. Matrices
$ en ligne $ B $ en ligne $ et
$ en ligne $ g $ en ligne $ dans cet exemple, ils sont nuls, car il n'y a pas de contrôle et d'influences perturbatrices.
Transition discrète
Pour la transition correcte vers la région discrète (en d'autres termes, la discrétisation du modèle), nous devons introduire le concept d' exposant matriciel . Un exposant matriciel est une fonction matricielle obtenue par analogie avec l'expansion de la fonction exponentielle dans une série de Taylor en fait de Maclaurin :
$$ afficher $$ e ^ {At} = E + At + \, ... \, \ dfrac {A ^ nt ^ n} {n!} + \, ... \, = \ sum_ {k = 0 } ^ {\ infty} \ dfrac {A ^ nt ^ n} {n!} $$ display $$
où sous $ en ligne $ E $ en ligne $ implique une matrice unitaire.
La transition exacte d'un modèle continu dans l'espace d'état à un modèle discret nécessite la recherche d'une solution à un système homogène $ inline $ \ dot {\ mathbf {x}} (t) = A (t) \ mathbf {x} (t) $ inline $ , puis la transition vers le système d'origine, trouver une solution commune et l'intégration à partir du moment initial $ inline $ t_0 $ inline $ à certains $ en ligne $ t $ en ligne $ . Une conclusion stricte peut être trouvée dans [1], ici, un résultat final est présenté.
Dans le cas de la stationnarité d'un modèle dynamique continu (indépendant des matrices $ inline $ a $ inline $ , $ en ligne $ B $ en ligne $ , $ en ligne $ g $ en ligne $ du temps) pour obtenir un modèle discret, on peut introduire une matrice de transition auxiliaire du système $ inline $ \ Phi (t, \ tau) $ inline $ à partir du moment $ inline $ \ tau $ inline $ en ce moment $ en ligne $ t $ en ligne $ où $ inline $ t> \ tau $ inline $ :$$ afficher $$ \ Phi (t, \ tau) = e ^ {A (t- \ tau)} = \ sum_ {k = 0} ^ {\ infty} \ dfrac {A ^ nt ^ n} {n! } $$ afficher $$
De plus, en utilisant cette matrice auxiliaire, les matrices requises pour un modèle discret peuvent être obtenues:$$ afficher $$ F = \ Phi (t + T, t) = e ^ {AT} = E + AT + \ dfrac {A ^ 2T ^ 2} {2!} + \ dfrac {A ^ 3T ^ 3} {3!} + ... $$ afficher $$
$$ afficher $$ \ Gamma = \ int_ {kT} ^ {(k + 1) T} \ Phi (t_ {k + 1}, \ tau) G (\ tau) d \ tau $$ afficher $$
$$ afficher $$ \ Psi = \ int_ {kT} ^ {(k + 1) T} \ Phi (t_ {k + 1}, \ tau) B (\ tau) d \ tau $$ afficher $$
Ici sous $ inline $ B (\ tau) $ inline $ et $ inline $ G (\ tau) $ inline $ nous entendons des matrices d'équations continues, par $ inline $ \ Psi $ inline $ et $ inline $ \ Gamma $ inline $ matrices requises d'un modèle discret.
Exemples pratiques
Texte masquéMalheureusement, les exemples n'auront que des perversions avec la matrice $ en ligne $ f $ en ligne $ , puisque l'auteur est trop paresseux pour inventer des exemples avec des actions de contrôle, et en général, dans le cadre de sa thèse, il traite des problèmes de navigation où il n'y a pas d'actions de contrôle. De plus, avec la connaissance rudimentaire de l'analyse mathématique, après analyse des exemples, ces actions ne devraient pas poser de problèmes. Pour des exemples différents de zéro $ inline $ \ Gamma $ inline $ et $ inline $ \ Psi $ inline $ peut aller à [2].
Pour illustrer les mathématiques ci-dessus, considérons deux exemples. Dont l'un est l'échauffement, et le second est illustratif, pour démontrer les capacités de la méthode décrite.
Trivial
Laisser l'objet se déplacer le long de l'axe $ inline $ ox $ inline $ avec vitesse initiale $ inline $ v_0 $ inline $ et accélération constante $ inline $ a $ inline $ . Ensuite, son modèle peut être représenté comme:
$$ display $$ \ ddot {x} = a $$ display $$
Nous représentons ce modèle sous la forme d'un système d'équations différentielles homogènes. Pour ce faire, nous divisons l'équation en un système de trois télécommandes:$$ affichage $$ \ begin {aligné} \ dot {x} & = v_x \\ \ dot {v} _x & = a_x \\ \ dot {a} _x & = 0 \ end {aligné} $$ display $$
Lors de l'écriture de systèmes d'équations, les dérivées suivantes y sont ajoutées jusqu'à présent, les éléments suivants sont nécessaires pour calculer l'actuelle. Ensuite, dans le système actuel, vous ne pouvez pas vous arrêter à $ inline $ v_x $ inline $ , car le calcul nécessite $ inline $ a_x $ inline $ . En même temps pour calculer $ inline $ a_x $ inline $ dérivé $ inline $ \ dot {a} _x $ inline $ n'est donc pas tenu d'introduire des dérivés de l'ordre ci-dessus $ inline $ a_x $ inline $ dans le vecteur d'état n'a pas beaucoup de sens.
Combinez trois variables dans un vecteur d'état $ inline $ \ mathbf {x} = [x \, v_x \, a_x] ^ T $ inline $ et écrire le système d'équations sous forme matricielle pour la transition vers la forme d'espace d'état:$$ display $$ \ dot {\ mathbf {x}} = A \ mathbf {x} $$ display $$
où est la matrice
$$ display $$ A = \ begin {bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \ end {bmatrix} $$ display $$
Nous pouvons maintenant calculer la matrice de transition du système dynamique discret correspondant au continu considéré:
$ inline $ \ begin {aligné} F = E + A \ cdot T + A \ times 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 $
Le lecteur peut vérifier que $ en ligne $ A ^ 3 $ en ligne $ et au-dessus est une matrice nulle.
De cette façon, une matrice de transition connue de tous a été obtenue, qui a été déduite sans appliquer d'hypothèses.
Exemple non trivial
Nous supposons que notre objet se déplace dans un espace tridimensionnel avec une certaine vitesse linéaire constante (modulo) et avec une vitesse angulaire représentée par un pseudovecteur:$$ display $$ \ omega = [\ omega_x \, \ omega_y \, \ omega_z] ^ T $$ display $$
Vous devez d'abord créer les équations de l'espace d'état. Nous écrivons l'accélération lors du déplacement autour du cercle. Du cours de physique d'un semestre, on sait que l'accélération centripète est un produit vectoriel des vitesses angulaires et linéaires:$$ afficher $$ \ dot {v} = \ oméga \ fois v $$ afficher $$
Ici, le vecteur vitesse est $ inline $ v = [v_x \, v_y \, v_z] ^ T $ inline $ .
Nous écrirons le produit vectoriel plus en détail:
$$ afficher $$ \ 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} $$ afficher $$
Maintenant, nous écrivons le système d'équations
$$ afficher $$ \ 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é} $$ afficher $$
Lors du passage à la forme matricielle, la matrice $ inline $ a $ inline $ sera:
$$ afficher $$ A = \ commencer {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} $$ afficher $$
Ensuite, nous procédons à la matrice $ en ligne $ f $ en ligne $ par l'expression correspondante. Depuis multiplier oralement les matrices par taille $ en ligne $ 6 \ fois 6 $ en ligne $ trois fois est assez difficile, la probabilité d'erreur est élevée, et ce n'est pas une affaire royale, alors nous allons écrire un script en utilisant la bibliothèque sympy Python:
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))
Et en l'exécutant, nous obtenons quelque chose comme ceci:
Texte masqué \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]
Cela après avoir été encadré avec des balises appropriées et collé dans le code source de l'article se transforme en:$$ afficher $$ 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} \ droite)} {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} \ gauche (- w_ {x} ^ {2} - w_ {y} ^ {2} \ droite)} {2} + 1 \ end {matrice} \ droite] $$ afficher $$
Ainsi, la matrice de transition du filtre de Kalman pour le mouvement circulaire peut être dérivée.
Contrairement au cas précédent, le résultat de la construction $ inline $ a $ inline $ à une puissance supérieure à 3 n'est pas une matrice nulle.
par exemple <math> $ inline $ A ^ 3 $ inline $ </math>$$ afficher $$ \ gauche [\ commencer {matrice} 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} \ gauche (- w_ {y} ^ {2} - w_ {z} ^ {2} \ droite) & - w_ {x} ^ {2} w_ {y} + w_ {y} \ gauche (- w_ {y} ^ {2} - w_ {z} ^ {2} \ droite) \\ 0 & 0 & 0 & - w_ {y} ^ {2} w_ {z} + w_ {z} \ gauche (- w_ {x} ^ {2} - w_ {z} ^ {2} \ droite) & 0 & w_ {x} w_ {y} ^ { 2} - w_ {x} \ gauche (- w_ {x} ^ {2} - w_ {z} ^ {2} \ droite) \\ 0 & 0 & 0 & w_ {y} w_ {z} ^ {2 } - w_ {y} \ gauche (- w_ {x} ^ {2} - w_ {y} ^ {2} \ droite) & - w_ {x} w_ {z} ^ {2} + w_ {x} \ gauche (- w_ {x} ^ {2} - w_ {y} ^ {2} \ droite) & 0 \ end {matrice} \ droite] $$ display $$
ou <math> $ inline $ A ^ 4 $ inline $ </math>$$ afficher $$ \ gauche [\ commencer {matrice} 0 & 0 & 0 & 0 & w_ {x} ^ {2} w_ {z} - w_ {z} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ droite) & - w_ {x} ^ {2} w_ {y} + w_ {y} \ gauche (- w_ {y} ^ {2} - w_ {z} ^ { 2} \ droite) \\ 0 & 0 & 0 & - w_ {y} ^ {2} w_ {z} + w_ {z} \ left (- w_ {x} ^ {2} - w_ {z} ^ { 2} \ droite) & 0 & w_ {x} w_ {y} ^ {2} - w_ {x} \ gauche (- w_ {x} ^ {2} - w_ {z} ^ {2} \ droite) \ \ 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} \ gauche (- w_ {x} ^ {2} - w_ {y} ^ {2} \ droite) & 0 \\ 0 & 0 & 0 & - w_ {y} \ left (- w_ {x} ^ {2} w_ {y} + w_ {y} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ right ) \ droite) + w_ {z} \ gauche (w_ {x} ^ {2} w_ {z} - w_ {z} \ gauche (- w_ {y} ^ {2} - w_ {z} ^ {2} \ droite) \ droite) & w_ {x} \ gauche (- w_ {x} ^ {2} w_ {y} + w_ {y} \ gauche (- w_ {y} ^ {2} - w_ {z} ^ {2} \ droite) \ droite) & - w_ {x} \ gauche (w_ {x} ^ {2} w_ {z} - w_ {z} \ gauche (- w_ {y} ^ {2} - w_ { z} ^ {2} \ droite) \ droite) \\ 0 & 0 & 0 & - w_ {y} \ left (w_ {x} w_ {y} ^ {2} - w_ {x} \ left (- w_ {x} ^ {2} - w_ {z} ^ {2} \ droite) \ droite) & w_ {x} \ gauche (w_ {x} w_ {y} ^ {2} - w_ {x} \ gauche ( - w_ {x} ^ {2} - w_ {z} ^ {2} \ droite) \ droite) - w_ {z} \ gauche (- w _ {y} ^ {2} w_ {z} + w_ {z} \ gauche (- w_ {x} ^ {2} - w_ {z} ^ {2} \ droite) \ right) & w_ {y} \ gauche (- w_ {y} ^ {2} w_ {z} + w_ {z} \ gauche (- w_ {x} ^ {2} - w_ {z} ^ {2} \ droite) \ droite) \\ 0 & 0 & 0 & w_ {z} \ left (- w_ {x} w_ {z} ^ {2} + w_ {x} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2 } \ droite) \ droite) & - w_ {z} \ gauche (w_ {y} w_ {z} ^ {2} - w_ {y} \ gauche (- w_ {x} ^ {2} - w_ {y} ^ {2} \ droite) \ droite) & - w_ {x} \ gauche (- w_ {x} w_ {z} ^ {2} + w_ {x} \ gauche (- w_ {x} ^ {2} - w_ {y} ^ {2} \ droite) \ droite) + w_ {y} \ gauche (w_ {y} w_ {z} ^ {2} - w_ {y} \ gauche (- w_ {x} ^ {2 } - w_ {y} ^ {2} \ right) \ right) \ end {matrix} \ right] $$ display $$
Par conséquent, la représentation d'une telle matrice est possible avec une précision finie. Cependant, lorsque $ inline $ \ omega T \ ll 1 $ inline $ série obtenue en éléments matriciels $ en ligne $ f $ en ligne $ convergent assez rapidement. Pour une utilisation pratique, suffisamment de membres au deuxième degré, rarement au troisième et encore plus au quatrième.
De plus, nous illustrons le fonctionnement de la matrice $ en ligne $ f $ en ligne $ vecteur de demande $ inline $ \ omega $ inline $ , $ inline $ \ bf {x} _0 $ inline $ , $ inline $ \ bf {v} _0 $ inline $ et une séquence de récurrence du formulaire:$$ afficher $$ \ mathbf {x} _k = F \ mathbf {x} _ {k-1} $$ afficher $$
Nous calculons cette séquence de récurrence pour $ inline $ \ omega T \ approx \ frac {1} {100} $ inline $
Code Python 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()
Permettez-moi de vous rappeler que pour le type np.array, le symbole "@" indique une multiplication matricielle. Les distances et les vitesses sont mesurées en perroquets, la vitesse angulaire en rad / s. Il faut aussi se rappeler que pour obtenir un cercle, il faut que les vecteurs vitesse et vitesse angulaire soient perpendiculaires, sinon une spirale sera obtenue à la place d'un cercle.
En conséquence, en fixant une certaine position initiale, vitesse et vitesse angulaire, on peut obtenir une telle trajectoire
La précision de la coïncidence des premier et dernier points peut être obtenue comme
>>> print(X[:3, 0] - X[:3,-1]) [-0.00051924 -0.0072984 0.0072984 ]
Avec un rayon de braquage de l'ordre de 150 unités, l'erreur relative ne dépasse pas les valeurs de l'ordre $ inline $ 5 \ cdot 10 ^ {- 5} $ inline $ . Cette précision est tout à fait suffisante pour le modèle FC, qui surveille la cible de virage.
Conclusion
Si auparavant le FC était utilisé principalement pour résoudre des problèmes de navigation, où l'utilisation de modèles de mouvement linéaire a donné de bons résultats, alors avec le développement d'applications modernes telles que la robotique, la vision par ordinateur, etc., le besoin de modèles plus complexes de mouvement des objets a augmenté. De plus, l'application de l'approche ci-dessus permet de synthétiser un modèle FC discret sans coût particulier, ce qui facilitera la tâche des développeurs. La seule limite de cette approche est qu'un modèle continu d'un système dynamique doit être décrit par un ensemble d'équations linéaires, ou du moins linéarisables, dans l'espace d'état.
En résumant ce qui précède, nous pouvons donner un algorithme pour la synthèse de la matrice de transition FC:
- Écriture de l'équation différentielle du système
- Transition vers un vecteur d'état et un espace d'état
- Linéarisation si nécessaire
- Représentation de la matrice de transition sous forme d'exposant matriciel et troncature de la série si nécessaire
- Calcul des matrices restantes en tenant compte de la matrice de transition
L'auteur accueille favorablement les critiques constructives concernant les erreurs commises, les inexactitudes, les formulations incorrectes, les méthodes non mentionnées, etc. Merci de votre attention!
Littérature utilisée
[1] Medic J. Estimations linéaires et contrôle statistiquement optimaux. Per. de l'anglais Ed. A.S. Shatalova Moscou. Maison d'édition "Energy", 1973, 440 p.
[2] Matveev V.V. Basics of the construction of strapdown inertial systems of St. Petersburg: State Research Center of the Russian Federation OJSC Concern TsNII Elektribribor, 2009. - 280s. ISBN 978-5-900180-73-3