El filtro Kalman (FC) es un algoritmo lineal óptimo para filtrar los parámetros de un sistema lineal dinámico en presencia de observaciones incompletas y ruidosas. Este filtro se usa ampliamente en sistemas de control técnico para evaluar la dinámica de los cambios en situaciones macroeconómicas o de opinión pública.
Este artículo tiene como objetivo familiarizar al lector con un enfoque estándar para la transición de un modelo continuo de un sistema dinámico descrito por un sistema de ecuaciones diferenciales lineales arbitrarias a un modelo discreto.
Texto ocultoademás de ahorrarle tiempo al lector al eliminar los intentos de inventar una bicicleta y exponerse a sus colegas bajo una luz fea. No seas como un autor
Este artículo también tiene la intención de alentar al lector a usar FC en aquellos problemas en los que a primera vista parece que el FC lineal no es aplicable, pero de hecho puede no serlo.
Para escribir un artículo del autor, el hecho de que a pesar de la simplicidad de las siguientes cosas en los resultados de búsqueda de Google en ruso e inglés (al menos en la primera página), el autor no pudo encontrarlas.
Modelo dinámico para un filtro discreto de Kalman
Texto ocultoBásicamente, esta sección es necesaria para presentar al lector el sistema de notación aceptado, que es muy diferente de un libro a otro y de un artículo a otro. Una explicación del significado de todas las cantidades incluidas en las ecuaciones está más allá del alcance de este artículo, mientras que se entiende que la persona que sale a la luz tiene alguna idea al respecto. Si no, bienvenido
aquí ,
aquí y
aquí .
El FC puede realizarse tanto en forma discreta como continua. De particular interés desde el punto de vista de la implementación práctica en las computadoras digitales modernas es el FC discreto que se enfatizará en este artículo.
El FC discreto lineal se describe mediante las siguientes expresiones. Deje que el modelo del sistema se represente de la siguiente manera:
$$ display $$ \ mathbf {x} _ {k} = F \ mathbf {x} _ {k-1} + \ Psi \ mathbf {u} _k + \ Gamma \ mathbf {w} _k $$ display $$
donde $ en línea $ f $ en línea $ - matriz de transición, $ en línea $ \ Psi $ en línea $ - matriz de control de transición, $ en línea $ \ Gamma $ en línea $ - matriz de perturbación de transición, $ en línea $ \ mathbf {x} _k $ en línea $ , $ en línea $ \ mathbf {u} _k $ en línea $ , $ en línea $ \ mathbf {w} _k $ en línea $ - vectores de estado, control y ruido (perturbación) del sistema en $ en línea $ k $ en línea $ ese paso Modelo de observación:$$ display $$ \ mathbf {z} _k = H \ mathbf {x} _k + \ mathbf {n} _k $$ display $$
donde $ en línea $ \ mathbf {z} _k $ en línea $ , $ en línea $ \ mathbf {n} _k $ en línea $ - vectores de observación y ruido de observación en $ en línea $ k $ en línea $ ese paso 5 ecuaciones de trabajo de FC en este artículo no son de interés, por lo tanto, en caso de que alguien las necesite, se dan bajo un spoiler.
Texto ocultoLa primera etapa, extrapolación:
$$ display $$ \ mathbf {x} _ {k | k-1} = F \ hat {\ mathbf {x}} _ {k-1} + \ mathbf {w} _k $$ display $$
$$ display $$ P_ {k | k-1} = FP_ {k-1} F ^ T + Q_k $$ display $$
Esta etapa se llama extrapolación. El siguiente paso, llamado corrección:
$$ display $$ K = PH ^ T (HP_ {k | k-1} H ^ T + R) ^ {- 1} $$ display $$
la evaluación en sí
$$ display $$ \ hat {\ mathbf {x}} _ {k} = x_ {k | k-1} + K (H \ mathbf {z} _k- \ mathbf {x} _ {k | k-1 }) $$ display $$
$$ display $$ P_k = (E-KH) P_ {k | k-1} $$ display $$
De aquí en adelante, estamos hablando de sistemas estacionarios (con coeficientes constantes) para los cuales las matrices $ en línea $ f $ en línea $ , $ en línea $ \ Psi $ en línea $ y $ en línea $ \ Gamma $ en línea $ independiente del número $ en línea $ k $ en línea $ .
Modelo de sistema dinámico continuo. Espacio de estado.
En la gran mayoría de las aplicaciones prácticas, FC filtra los parámetros de los sistemas dinámicos continuos descritos por ecuaciones diferenciales para tiempo continuo. El cálculo de la FC en este caso ocurre en una computadora digital, lo que automáticamente hace que la FC sea discreta y el modelo debe ser discreto en consecuencia. Para obtener un modelo discreto de estos sistemas continuos, primero es necesario componer el vector de estado mismo (vector de fase), el sistema de ecuaciones de estado, luego discretizarlos, obteniendo así matrices $ en línea $ f $ en línea $ , $ en línea $ \ Psi $ en línea $ y $ en línea $ \ Gamma $ en línea $ .
Deje que el comportamiento del sistema sea descrito por un conjunto de $ en línea $ n $ en línea $ ecuaciones diferenciales de primer orden:$$ display $$ \ dot {\ mathbf {x}} (t) = A \ mathbf {x} (t) + B \ mathbf {u} (t) + G \ mathbf {w} (t) $$ display $$
aqui $ en línea $ \ mathbf {x} $ en línea $ - $ en línea $ n $ en línea $ -dimensional del vector de estado del sistema. El vector de estado (también conocido como vector de fase) es un vector que contiene variables que describen el sistema y sus derivados hasta el orden requerido. $ en línea $ \ mathbf {u} $ en línea $ - $ en línea $ r $ en línea $ -dimensional del vector de control del sistema que describe el efecto controlado ejercido sobre el sistema.
$ en línea $ \ mathbf {w} $ en línea $ $ en línea $ p $ en línea $ -dimensional vectorial que contiene un efecto aleatorio no controlado en el sistema, o ruido. $ en línea $ a $ en línea $ - matriz del tamaño del estado del sistema $ en línea $ n \ veces n $ en línea $ . $ en línea $ B $ en línea $ - matriz de control de tamaño $ en línea $ n \ veces r $ en línea $ . $ en línea $ g $ en línea $ - matriz de perturbación de tamaño $ en línea $ n \ veces p $ en línea $ . En esta expresión, todos los productos se calculan de acuerdo con las reglas de multiplicación de matrices. En el caso general, los elementos de todas las matrices son funciones del tiempo, sin embargo, solo los sistemas estacionarios se consideran en el artículo, donde los elementos son independientes del tiempo.
A continuación se muestra un ejemplo de una transición de una descripción del sistema utilizando una ecuación diferencial de orden superior a una descripción a través de un espacio de estado.
EjemploDeje que el movimiento de un punto a lo largo de algún eje
$ en línea $ ox $ en línea $ se describe mediante una ecuación diferencial de segundo orden:
$$ display $$ \ ddot {x} = - \ omega ^ 2 x $$ display $$
Si no se recuerda, se representa un movimiento oscilatorio. Pasamos de una ecuación de segundo orden a un sistema de dos ecuaciones introduciendo una nueva variable
$ en línea $ x_1 = \ dot {x} $ en línea $ . Ahora tenemos:
$$ display $$ \ begin {alineado} \ dot {x} & = x_1 \\ \ dot {x} _1 & = - \ omega ^ 2 x \ end {alineado} $$ display $$
Este sistema de ecuaciones se puede escribir en forma de matriz, mientras que el vector de estado
$ en línea $ \ mathbf {x} = [x \, x_1] ^ T $ en línea $ , la matriz de estado será
$$ display $$ A = \ begin {bmatrix} 0 & 1 \\ - \ omega ^ 2 & 0 \ end {bmatrix} $$ display $$
Variable ingresada
$ en línea $ x_1 $ en línea $ juega el papel de la velocidad. Matrices
$ en línea $ B $ en línea $ y
$ en línea $ g $ en línea $ en este ejemplo, son cero, ya que no hay control ni influencias perturbadoras.
Transición discreta
Para la transición correcta a la región discreta (en otras palabras, la discretización del modelo), necesitamos introducir el concepto de un exponente matricial . Un exponente matricial es una función matricial obtenida por analogía con la expansión de la función exponencial en una serie de Taylor, de hecho, de Maclaurin :
$$ display $$ e ^ {At} = E + At + \, ... \, \ dfrac {A ^ nt ^ n} {n!} + \, ... \, = \ sum_ {k = 0 } ^ {\ infty} \ dfrac {A ^ nt ^ n} {n!} $$ display $$
donde debajo $ en línea $ E $ en línea $ implica una unidad de matriz.
La transición exacta de un modelo continuo en el espacio de estado a un modelo discreto requiere la búsqueda de una solución para un sistema homogéneo. $ en línea $ \ dot {\ mathbf {x}} (t) = A (t) \ mathbf {x} (t) $ en línea $ , luego la transición al sistema original, encontrando una solución común e integración desde el momento inicial $ en línea $ t_0 $ en línea $ a algunos $ en línea $ t $ en línea $ . Se puede encontrar una conclusión estricta en [1], aquí se presenta un resultado final.
En el caso de la estacionariedad de un modelo dinámico continuo (independiente de las matrices $ en línea $ a $ en línea $ , $ en línea $ B $ en línea $ , $ en línea $ g $ en línea $ de tiempo) para obtener un modelo discreto, podemos introducir una matriz de transición auxiliar del sistema $ en línea $ \ Phi (t, \ tau) $ en línea $ desde el momento $ en línea $ \ tau $ en línea $ por el momento $ en línea $ t $ en línea $ donde $ en línea $ t> \ tau $ en línea $ :$$ display $$ \ Phi (t, \ tau) = e ^ {A (t- \ tau)} = \ sum_ {k = 0} ^ {\ infty} \ dfrac {A ^ nt ^ n} {n! } $$ display $$
Además, utilizando esta matriz auxiliar, se pueden obtener las matrices requeridas para un modelo discreto:$$ display $$ F = \ Phi (t + T, t) = e ^ {AT} = E + AT + \ dfrac {A ^ 2T ^ 2} {2!} + \ dfrac {A ^ 3T ^ 3} {3!} + ... $$ display $$
$$ display $$ \ Gamma = \ int_ {kT} ^ {(k + 1) T} \ Phi (t_ {k + 1}, \ tau) G (\ tau) d \ tau $$ display $$
$$ display $$ \ Psi = \ int_ {kT} ^ {(k + 1) T} \ Phi (t_ {k + 1}, \ tau) B (\ tau) d \ tau $$ display $$
Aquí debajo $ en línea $ B (\ tau) $ en línea $ y $ en línea $ G (\ tau) $ en línea $ nos referimos a matrices de ecuaciones continuas, por $ en línea $ \ Psi $ en línea $ y $ en línea $ \ Gamma $ en línea $ matrices requeridas de un modelo discreto.
Ejemplos prácticos
Texto ocultoDesafortunadamente, los ejemplos solo tendrán perversiones con la matriz $ en línea $ f $ en línea $ , dado que el autor es demasiado vago para inventar ejemplos con acciones de control y, en general, como parte de su disertación, se ocupa de los problemas de navegación donde no hay acciones de control. Además, con el conocimiento rudimentario del análisis matemático, después de analizar ejemplos, estas acciones no deberían causar problemas. Para ejemplos distintos de cero $ en línea $ \ Gamma $ en línea $ y $ en línea $ \ Psi $ en línea $ puede ir a [2].
Para ilustrar las matemáticas anteriores, considere dos ejemplos. Uno de los cuales es el calentamiento, y el segundo es ilustrativo, para demostrar las capacidades del método descrito.
Trivial
Deje que el objeto se mueva a lo largo del eje. $ en línea $ ox $ en línea $ con velocidad inicial $ en línea $ v_0 $ en línea $ y aceleración constante $ en línea $ a $ en línea $ . Entonces su modelo se puede representar como:
$$ display $$ \ ddot {x} = a $$ display $$
Representamos este modelo en forma de un sistema de ecuaciones diferenciales homogéneas. Para hacer esto, dividimos la ecuación en un sistema de tres controles remotos:$$ display $$ \ begin {alineado} \ dot {x} & = v_x \\ \ dot {v} _x & = a_x \\ \ dot {a} _x & = 0 \ end {alineado} $$ display $$
Al escribir sistemas de ecuaciones, las siguientes derivadas se agregan allí hasta ahora, se requiere lo siguiente para calcular la actual. Entonces, en el sistema actual no puedes parar en $ en línea $ v_x $ en línea $ , ya que el cálculo requiere $ en línea $ a_x $ en línea $ . Al mismo tiempo para calcular $ en línea $ a_x $ en línea $ derivada $ en línea $ \ dot {a} _x $ en línea $ por lo tanto, no es obligatorio introducir derivados del orden anterior $ en línea $ a_x $ en línea $ en el estado el vector no tiene mucho sentido.
Combina tres variables en un vector de estado $ en línea $ \ mathbf {x} = [x \, v_x \, a_x] ^ T $ en línea $ y escribe el sistema de ecuaciones en forma de matriz para la transición a la forma de espacio de estado:$$ display $$ \ dot {\ mathbf {x}} = A \ mathbf {x} $$ display $$
donde esta la matriz
$$ display $$ A = \ begin {bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \ end {bmatrix} $$ display $$
Ahora podemos calcular la matriz de transición del sistema dinámico discreto correspondiente al continuo bajo consideración:
$ en línea $ \ begin {alineado} 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 {alineado} $ en línea $
El lector puede verificar que $ en línea $ A ^ 3 $ en línea $ y arriba es una matriz cero.
De esta forma, se obtuvo una matriz de transición conocida por todos, que se dedujo sin aplicar ningún supuesto.
Ejemplo no trivial
Suponemos que nuestro objeto se mueve en un espacio tridimensional con una cierta velocidad lineal constante (módulo) y con una velocidad angular representada por un pseudovector:$$ display $$ \ omega = [\ omega_x \, \ omega_y \, \ omega_z] ^ T $$ display $$
Primero necesitas inventar las ecuaciones del espacio de estados. Escribimos la aceleración cuando nos movemos alrededor del círculo. Del curso de física durante 1 semestre, se sabe que la aceleración centrípeta es un producto vectorial de velocidades angulares y lineales:$$ display $$ \ dot {v} = \ omega \ times v $$ display $$
Aquí el vector de velocidad es $ en línea $ v = [v_x \, v_y \, v_z] ^ T $ en línea $ .
Escribiremos el producto vectorial con más detalle:
$$ 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 $$
Ahora escribimos el sistema de ecuaciones.
$$ display $$ \ begin {alineado} \ 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 {alineado} $$ display $$
Tras la transición a la forma matricial, la matriz $ en línea $ a $ en línea $ será:
$$ display $$ A = \ begin {bmatrix} 0 y 0 y 0 y 1 y 0 y 0 \\ 0 y 0 y 0 y 0 y 1 y 0 \\ 0 y 0 y 0 y 0 y 0 y 1 y \ 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 $$
A continuación, procedemos a la matriz. $ en línea $ f $ en línea $ por la expresión correspondiente. Dado que oralmente multiplicar matrices por tamaño $ en línea $ 6 \ veces 6 $ en línea $ tres veces es bastante difícil, la probabilidad de error es alta, y esto no es un asunto real, entonces escribiremos un script usando la biblioteca simulada de 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))
Y ejecutándolo, obtenemos algo como esto:
Texto oculto \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]
Que después de enmarcar con etiquetas apropiadas y pegar en el código fuente del artículo se convierte en:$$ 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} y 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 {matriz} \ right] $$ display $$
Por lo tanto, se puede derivar la matriz de transición del filtro de Kalman para movimiento circular.
A diferencia del caso anterior, el resultado de la construcción $ en línea $ a $ en línea $ a una potencia superior a 3 no es una matriz cero.
por ejemplo, <math> $ inline $ A ^ 3 $ inline $ </math>$$ 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 $$
o <math> $ inline $ A ^ 4 $ inline $ </math>$$ display $$ \ left [\ begin {matrix} 0 & 0 & 0 & 0 & w_ {x} ^ {2} w_ {z} - w_ {z} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ right) y - 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 \\ 0 & 0 & 0 & - w_ {y} \ left (- w_ {x} ^ {2} w_ {y} + w_ {y} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ right ) \ right) + w_ {z} \ left (w_ {x} ^ {2} w_ {z} - w_ {z} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ right) \ right) & w_ {x} \ left (- w_ {x} ^ {2} w_ {y} + w_ {y} \ left (- w_ {y} ^ {2} - w_ {z} ^ {2} \ right) \ right) & - w_ {x} \ left (w_ {x} ^ {2} w_ {z} - w_ {z} \ left (- w_ {y} ^ {2} - w_ { z} ^ {2} \ right) \ right) \\ 0 & 0 & 0 & - w_ {y} \ left (w_ {x} w_ {y} ^ {2} - w_ {x} \ left (- w_ {x} ^ {2} - w_ {z} ^ {2} \ right) \ right) & w_ {x} \ left (w_ {x} w_ {y} ^ {2} - w_ {x} \ left ( - w_ {x} ^ {2} - w_ {z} ^ {2} \ right) \ right) - w_ {z} \ left (- w _ {y} ^ {2} w_ {z} + w_ {z} \ left (- w_ {x} ^ {2} - w_ {z} ^ {2} \ right) \ right) & w_ {y} \ left (- w_ {y} ^ {2} w_ {z} + w_ {z} \ left (- w_ {x} ^ {2} - w_ {z} ^ {2} \ right) \ right) \\ 0 & 0 & 0 & w_ {z} \ left (- w_ {x} w_ {z} ^ {2} + w_ {x} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2 } \ right) \ right) & - w_ {z} \ left (w_ {y} w_ {z} ^ {2} - w_ {y} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2} \ right) \ right) & - w_ {x} \ left (- w_ {x} w_ {z} ^ {2} + w_ {x} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2} \ right) \ right) + w_ {y} \ left (w_ {y} w_ {z} ^ {2} - w_ {y} \ left (- w_ {x} ^ {2 } - w_ {y} ^ {2} \ right) \ right) \ end {matrix} \ right] $$ display $$
Por lo tanto, la representación de dicha matriz es posible con precisión finita. Sin embargo, cuando $ en línea $ \ omega T \ ll 1 $ en línea $ series obtenidas en elementos matriciales $ en línea $ f $ en línea $ convergen bastante rápido. Para uso práctico, suficientes miembros para el segundo grado, rara vez para el tercero y aún más para el cuarto.
Además, ilustramos el funcionamiento de la matriz. $ en línea $ f $ en línea $ vector preguntando $ en línea $ \ omega $ en línea $ , $ en línea $ \ bf {x} _0 $ en línea $ , $ en línea $ \ bf {v} _0 $ en línea $ , y una secuencia de recurrencia de la forma:$$ display $$ \ mathbf {x} _k = F \ mathbf {x} _ {k-1} $$ display $$
Calculamos esta secuencia de recurrencia para $ en línea $ \ omega T \ aprox \ frac {1} {100} $ en línea $
Código de 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()
Permítame recordarle que para el tipo np.array, el símbolo "@" denota la multiplicación de matrices. Las distancias y las velocidades se miden en loros, la velocidad angular en rad / s. También es necesario recordar que para obtener un círculo, es necesario que los vectores de velocidad y velocidad angular sean perpendiculares, de lo contrario se obtendrá una espiral en lugar de un círculo.
Como resultado, al establecer una determinada posición inicial, velocidad y velocidad angular, se puede obtener dicha trayectoria
La precisión de la coincidencia del primer y último punto se puede obtener como
>>> print(X[:3, 0] - X[:3,-1]) [-0.00051924 -0.0072984 0.0072984 ]
Con un radio de giro del orden de 150 unidades, el error relativo no excede los valores del orden $ en línea $ 5 \ cdot 10 ^ {- 5} $ en línea $ . Esta precisión es suficiente para el modelo FC, que monitorea el objetivo giratorio.
Conclusión
Si el FC anterior se usaba principalmente para resolver problemas de navegación, donde el uso de modelos de movimiento lineal producía un buen resultado, entonces con el desarrollo de aplicaciones modernas como la robótica, la visión por computadora, etc., la necesidad de modelos más complejos de movimiento de objetos ha aumentado. Además, la aplicación del enfoque anterior hace posible sintetizar un modelo FC discreto sin costo particular, lo que facilitará la tarea a los desarrolladores. La única limitación de este enfoque es que el modelo continuo de un sistema dinámico debe describirse mediante un conjunto de ecuaciones lineales, o al menos linealizables, en el espacio de estados.
Resumiendo lo anterior, podemos dar un algoritmo para la síntesis de la matriz de transición FC:
- Escribir la ecuación diferencial del sistema.
- Transición al vector de estado y espacio de estado
- Linealización si es necesario
- Representación de la matriz de transición en forma de exponente de matriz y truncamiento de la serie si es necesario.
- Cálculo de las matrices restantes teniendo en cuenta la matriz de transición.
El autor agradece las críticas constructivas sobre los errores cometidos, las inexactitudes, las formulaciones incorrectas, los métodos no mencionados y más. Gracias por su atencion!
Literatura usada
[1] Medic J. Estimaciones lineales y control estadísticamente óptimos. Por. del ingles Ed. A.S. Shatalova Moscú. Editorial "Energía", 1973, 440 p.
[2] Matveev V.V. Conceptos básicos de la construcción de sistemas inerciales de amarre de San Petersburgo: Centro de Investigación del Estado de la Federación de Rusia Preocupación de OJSC TsNII Elektribribor, 2009 - 280s. ISBN 978-5-900180-73-3