O filtro Kalman (FC) é um algoritmo linear ideal para filtrar os parâmetros de um sistema linear dinâmico na presença de observações incompletas e ruidosas. Esse filtro é amplamente utilizado em sistemas de controle técnico para avaliar a dinâmica de mudanças em situações macroeconômicas ou na opinião pública.
Este artigo tem como objetivo familiarizar o leitor com uma abordagem padrão para a transição de um modelo contínuo de um sistema dinâmico descrito por um sistema de equações diferenciais lineares arbitrárias para um modelo discreto.
Texto ocultoalém de economizar tempo do leitor, eliminando tentativas de inventar uma bicicleta e se expor aos colegas sob uma luz feia. Não seja como um autor
Este artigo também visa incentivar o leitor a usar o FC nos problemas em que, à primeira vista, parece que o FC linear não é aplicável, mas na verdade pode não ser.
Para escrever um artigo do autor, apesar da simplicidade dos itens a seguir nos resultados de pesquisa do Google em russo e inglês (pelo menos na primeira página), o autor não conseguiu encontrá-los.
Modelo dinâmico para um filtro Kalman discreto
Texto ocultoBasicamente, esta seção é necessária para apresentar ao leitor o sistema de notação aceito, que é muito diferente de livro para livro e de artigo para artigo. Uma explicação do significado de todas as quantidades incluídas nas equações está além do escopo deste artigo, enquanto se entende que a pessoa que vem à luz tem alguma idéia sobre isso. Se não, seja bem-vindo
aqui ,
aqui e
aqui .
O FC pode ser realizado de forma discreta e contínua. De particular interesse do ponto de vista da implementação prática em computadores digitais modernos é o FC discreto que será enfatizado neste artigo.
FC discreto linear é descrito pelas seguintes expressões. Deixe o modelo do sistema ser representado da seguinte maneira:
exibição $$ $$ \ mathbf {x} _ {k} = F \ mathbf {x} _ {k-1} + \ Psi \ mathbf {u} _k + \ Gamma \ mathbf {w} _k $$ exibição $$
onde $ inline $ f $ inline $ - matriz de transição, $ inline $ \ Psi $ inline $ - matriz de controle de transição, $ inline $ \ Gamma $ inline $ - matriz de perturbação de transição, $ inline $ \ mathbf {x} _k $ inline $ , $ inline $ \ mathbf {u} _k $ inline $ , $ inline $ \ mathbf {w} _k $ inline $ - vetores de estado, controle e ruído (perturbação) do sistema em $ inline $ k $ inline $ esse passo. Modelo de Observação:$$ display $$ \ mathbf {z} _k = H \ mathbf {x} _k + \ mathbf {n} _k $$ display $$
onde $ inline $ \ mathbf {z} _k $ inline $ , $ inline $ \ mathbf {n} _k $ inline $ - vetores de observação e ruído de observação em $ inline $ k $ inline $ esse passo. 5 equações de trabalho de FC neste artigo não são de interesse; portanto, caso alguém precise delas, elas serão dadas sob um spoiler.
Texto ocultoA primeira etapa, extrapolação:
exibição $$ $$ \ mathbf {x} _ {k | k-1} = F \ hat {\ mathbf {x}} _ {k-1} + \ mathbf {w} _k $$ exibição $$
$$ display $$ P_ {k | k-1} = FP_ {k-1} F ^ T + Q_k $$ display $$
Este estágio é chamado de extrapolação. O próximo passo, chamado correção:
$$ display $$ K = PH ^ T (HP_ {k | k-1} H ^ T + R) ^ {- 1} $$ display $$
a própria avaliação
exibição $$ $$ \ 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 $$
A seguir, estamos falando de sistemas estacionários (com coeficientes constantes) para os quais as matrizes $ inline $ f $ inline $ , $ inline $ \ Psi $ inline $ e $ inline $ \ Gamma $ inline $ independente do número $ inline $ k $ inline $ .
Modelo de sistema dinâmico contínuo. Espaço de estado.
Na grande maioria das aplicações práticas, o FC filtra os parâmetros de sistemas dinâmicos contínuos descritos por equações diferenciais para tempo contínuo. O cálculo do FC neste caso ocorre em um computador digital, o que automaticamente torna o FC discreto e o modelo deve ser discreto. Para obter um modelo discreto desses sistemas contínuos, primeiro é necessário compor o próprio vetor de estado (vetor de fase), o sistema de equações de estado, depois discretizá-los, obtendo assim matrizes $ inline $ f $ inline $ , $ inline $ \ Psi $ inline $ e $ inline $ \ Gamma $ inline $ .
Deixe o comportamento do sistema ser descrito por um conjunto de $ inline $ n $ inline $ equações diferenciais de primeira ordem:exibição $$ $$ \ ponto {\ mathbf {x}} (t) = A \ mathbf {x} (t) + B \ mathbf {u} (t) + G \ mathbf {w} (t) $$ exibição $$
aqui $ inline $ \ mathbf {x} $ inline $ - $ inline $ n $ inline $ de estado tridimensional do sistema. O vetor de estado (também conhecido como vetor de fase) é um vetor que contém variáveis que descrevem o sistema e suas derivadas até a ordem requerida. $ inline $ \ mathbf {u} $ inline $ - $ inline $ r $ inline $ vetor de controle tridimensional do sistema que descreve o efeito controlado exercido no sistema.
$ inline $ \ mathbf {w} $ inline $ $ inline $ p $ inline $ tridimensional contendo um efeito aleatório não controlado no sistema ou ruído. $ inline $ a $ inline $ - matriz do tamanho do estado do sistema $ inline $ n \ vezes n $ inline $ . $ inline $ B $ inline $ - matriz de controle de tamanho $ inline $ n \ vezes r $ inline $ . $ inline $ g $ inline $ - matriz de perturbação de tamanho $ inline $ n \ vezes p $ inline $ . Nesta expressão, todos os produtos são calculados de acordo com as regras de multiplicação de matrizes. No caso geral, os elementos de todas as matrizes são funções do tempo, no entanto, apenas sistemas estacionários são considerados no artigo, onde os elementos são independentes do tempo.
Um exemplo de transição de uma descrição do sistema usando uma equação diferencial de ordem superior para uma descrição através de um espaço de estados é dado abaixo.
ExemploDeixe o movimento de um ponto ao longo de algum eixo
$ inline $ ox $ inline $ é descrito por uma equação diferencial de segunda ordem:
$$ display $$ \ ddot {x} = - \ omega ^ 2 x $$ display $$
Se não se lembra, um movimento oscilatório é representado. Passamos de uma equação de segunda ordem para um sistema de duas equações, introduzindo uma nova variável
$ inline $ x_1 = \ ponto {x} $ inline $ . Agora temos:
exibição $$ $$ \ begin {alinhado} \ ponto {x} & = x_1 \\ \ ponto {x} _1 & = - \ omega ^ 2 x \ end {alinhado} $$ exibição $$
Este sistema de equações pode ser escrito em forma de matriz, enquanto o vetor de estado
$ inline $ \ mathbf {x} = [x \, x_1] ^ T $ inline $ , a matriz de estados será
$$ display $$ A = \ begin {bmatrix} 0 e 1 \\ - \ omega ^ 2 & 0 \ end {bmatrix} $$ display $$
Variável inserida
$ inline $ x_1 $ inline $ desempenha o papel de velocidade. Matrizes
$ inline $ B $ inline $ e
$ inline $ g $ inline $ neste exemplo, eles são zero, uma vez que não há controle e influências perturbadoras.
Transição discreta
Para a transição correta para a região discreta (em outras palavras, a discretização do modelo), precisamos introduzir o conceito de um expoente da matriz . Um expoente de matriz é uma função de matriz obtida por analogia com a expansão da função exponencial em uma série de Taylor de fato de Maclaurin :
$$ display $$ e ^ {At} = E + Em + \, ... \, \ dfrac {A ^ nt ^ n} {n!} + \, ... \, = \ sum_ {k = 0 } ^ {\ infty} \ dfrac {A ^ nt ^ n} {n!} $$ exibem $$
onde sob $ inline $ E $ inline $ implica uma matriz unitária.
A transição exata de um modelo contínuo no espaço de estados para um modelo discreto exige uma busca por uma solução para um sistema homogêneo $ inline $ \ ponto {\ mathbf {x}} (t) = A (t) \ mathbf {x} (t) $ inline $ , depois a transição para o sistema original, encontrando uma solução e integração comuns desde o momento inicial $ inline $ t_0 $ inline $ para alguns $ inline $ t $ inline $ . Uma conclusão estrita pode ser encontrada em [1]; aqui, um resultado final é apresentado.
No caso de estacionariedade de um modelo dinâmico contínuo (independente de matrizes $ inline $ a $ inline $ , $ inline $ B $ inline $ , $ inline $ g $ inline $ do tempo) para obter um modelo discreto, podemos introduzir uma matriz de transição auxiliar do sistema $ inline $ \ Phi (t, \ tau) $ inline $ a partir do momento $ inline $ \ tau $ inline $ no momento $ inline $ t $ inline $ onde $ inline $ t> \ tau $ inline $ :$$ display $$ \ Phi (t, \ tau) = e ^ {A (t- \ tau)} = \ sum_ {k = 0} ^ {\ infty} \ dfrac {A ^ nt ^ n} {n! } $$ exibir $$
Além disso, usando essa matriz auxiliar, as matrizes necessárias para um modelo discreto podem ser obtidas:$$ display $$ F = \ Phi (t + T, t) = e ^ {AT} = E + AT + \ dfrac {A ^ 2T ^ 2} {2!} + \ dfrac {A ^ 3T ^ 3} {3!} + ... $$ exibir $$
$$ 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 $$
Aqui em baixo $ inline $ B (\ tau) $ inline $ e $ inline $ G (\ tau) $ inline $ queremos dizer matrizes de equações contínuas, por $ inline $ \ Psi $ inline $ e $ inline $ \ Gamma $ inline $ matrizes necessárias de um modelo discreto.
Exemplos práticos
Texto ocultoInfelizmente, os exemplos só terão perversões com a matriz $ inline $ f $ inline $ , como o autor tem preguiça de inventar exemplos com ações de controle e, em geral, como parte de sua dissertação, ele lida com questões de navegação onde não há ações de controle. Além disso, com o conhecimento rudimentar da análise matemática, após analisar exemplos, essas ações não devem causar problemas. Para exemplos diferentes de zero $ inline $ \ Gamma $ inline $ e $ inline $ \ Psi $ inline $ pode ir para [2].
Para ilustrar a matemática acima, considere dois exemplos. Um deles é o aquecimento e o segundo é ilustrativo, para demonstrar as capacidades do método descrito.
Trivial
Deixe o objeto se mover ao longo do eixo $ inline $ ox $ inline $ com velocidade inicial $ inline $ v_0 $ inline $ e aceleração constante $ inline $ a $ inline $ . Então, seu modelo pode ser representado como:
$$ display $$ \ ddot {x} = um $$ display $$
Representamos esse modelo na forma de um sistema de equações diferenciais homogêneas. Para fazer isso, dividimos a equação em um sistema de três controles remotos:exibição $$ $$ \ begin {alinhado} \ ponto {x} & = v_x \\ \ ponto {v} _x & = a_x \\ \ ponto {a} _x & = 0 \ end {alinhado} $$ exibição $$
Ao escrever sistemas de equações, as seguintes derivadas são adicionadas lá até agora, o seguinte é necessário para calcular a atual. Então, no sistema atual, você não pode parar em $ inline $ v_x $ inline $ , já que o cálculo requer $ inline $ a_x $ inline $ . Ao mesmo tempo para calcular $ inline $ a_x $ inline $ derivado $ inline $ \ ponto {a} _x $ inline $ portanto, não é necessário introduzir derivativos da ordem acima $ inline $ a_x $ inline $ no vetor estado não faz muito sentido.
Combine três variáveis em um vetor de estado $ inline $ \ mathbf {x} = [x \, v_x \, a_x] ^ T $ inline $ e escreva o sistema de equações na forma de matriz para a transição para a forma de espaço de estado:$$ display $$ \ dot {\ mathbf {x}} = A \ mathbf {x} $$ display $$
onde está a matriz
$$ display $$ A = \ begin {bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \ end {bmatrix} $$ display $$
Agora podemos calcular a matriz de transição do sistema dinâmico discreto correspondente ao contínuo em consideração:
$ inline $ \ begin {alinhado} F = E + A \ cdot T + A \ vezes 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 {alinhado} $ inline $
O leitor pode verificar se $ inline $ A ^ 3 $ inline $ e acima é uma matriz zero.
Dessa maneira, foi obtida uma matriz de transição conhecida por todos, deduzida sem a aplicação de nenhuma premissa.
Exemplo não trivial
Assumimos que nosso objeto se move no espaço tridimensional com uma certa velocidade linear constante (módulo) e com uma velocidade angular representada por um pseudovetor:$$ display $$ \ omega = [\ omega_x \, \ omega_y \, \ omega_z] ^ T $$ display $$
Primeiro você precisa criar as equações do espaço de estados. Escrevemos a aceleração quando nos movemos ao redor do círculo. No curso de física de um semestre, sabe-se que a aceleração centrípeta é um produto vetorial de velocidades angulares e lineares:$$ display $$ \ ponto {v} = \ omega \ times v $$ display $$
Aqui o vetor de velocidade é $ inline $ v = [v_x \, v_y \, v_z] ^ T $ inline $ .
Escreveremos o produto vetorial em mais detalhes:
$$ 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 $$
Agora escrevemos o sistema de equações
exibição $$ $$ \ begin {alinhado} \ ponto {x} & = v_x \\ \ ponto {y} & = v_y \\ \ ponto {z} & = v_z \\ \ ponto {v} _x & = \ omega_yz - \ omega_zy \\ \ ponto {v} _y & = \ omega_zx- \ omega_xz \\ \ ponto {v} _z & = \ omega_xy- \ omega_yx \ end {alinhado} $$ display $$
Após a transição para a forma da matriz, a matriz $ inline $ a $ inline $ será:
$$ display $$ A = \ begin {bmatrix} 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 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 $$
Em seguida, prosseguimos para a matriz $ inline $ f $ inline $ pela expressão correspondente. Como multiplicar oralmente matrizes por tamanho $ inline $ 6 \ vezes 6 $ inline $ três vezes é bastante difícil, a probabilidade de erro é alta e isso não é um problema real; então, escreveremos um script usando a biblioteca sympy do 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))
E, executando-o, temos algo parecido com isto:
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]
Depois de enquadrar com tags apropriadas e colar no código fonte do artigo, ele se transforma em:$$ display $$ F = \ left [\ begin {matrix} 1 & 0 & 0 & T & - \ frac {T ^ {2} w_ {z}} {2} e \ 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} e \ 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} e \ frac {T ^ {2} \ left (- w_ {x} ^ {2} - w_ {y} ^ {2} \ right)} {2} + 1 \ end {matrix} \ right] $$ exibir $$
Assim, a matriz de transição de filtro de Kalman para movimento circular pode ser derivada.
Diferentemente do caso anterior, o resultado da construção $ inline $ a $ inline $ para uma potência maior que 3 não é uma matriz zero.
por exemplo, <math> $ inline $ A ^ 3 $ inline $ </math>exibição $$ $$ \ 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} \ esquerda (- w_ {y} ^ {2} - w_ {z} ^ {2} \ right) \\ 0 & 0 & 0 & - w_ {y} ^ {2} w_ {z} + w_ {z} \ esquerda (- 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} \ esquerda (- w_ {x} ^ {2} - w_ {y} ^ {2} \ direita) & 0 \ end {matriz} \ direita] $$ display $$
ou <math> $ inline $ A ^ 4 $ inline $ </math>exibição $$ $$ \ left [\ begin {matrix} 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} \ esquerda (- w_ {x} ^ {2} - w_ {y} ^ {2} \ direita) & - 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} \ esquerda (- w_ {y} ^ {2} w_ {z} + w_ {z} \ esquerda (- 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 $$
Portanto, a representação dessa matriz é possível com precisão finita. No entanto, quando $ inline $ \ omega T 1 ll $ inline $ séries obtidas em elementos matriciais $ inline $ f $ inline $ convergem muito rapidamente. Para uso prático, membros suficientes para o segundo grau, raramente para o terceiro e ainda mais para o quarto.
Além disso, ilustramos a operação da matriz $ inline $ f $ inline $ perguntando vetor $ inline $ \ omega $ inline $ , $ inline $ \ bf {x} _0 $ inline $ , $ inline $ \ bf {v} _0 $ inline $ e uma sequência de recorrência do formulário:$$ exibir $$ \ mathbf {x} _k = F \ mathbf {x} _ {k-1} $$ exibir $$
Calculamos essa sequência de recorrência para $ inline $ \ omega T \ approx \ frac {1} {100} $ inline $
Código 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()
Deixe-me lembrá-lo que, para o tipo np.array, o símbolo "@" indica a multiplicação da matriz. Distâncias e velocidades são medidas em papagaios, velocidade angular em rad / s. Também é necessário lembrar que, para obter um círculo, é necessário que os vetores de velocidade e velocidade angular sejam perpendiculares; caso contrário, uma espiral aparecerá em vez de um círculo.
Como resultado, definindo uma certa posição inicial, velocidade e velocidade angular, é possível obter essa trajetória
A precisão da coincidência do primeiro e do último ponto pode ser obtida como
>>> print(X[:3, 0] - X[:3,-1]) [-0.00051924 -0.0072984 0.0072984 ]
Com um raio de viragem da ordem de 150 unidades, o erro relativo não excede os valores da ordem $ inline $ 5 \ cdot 10 ^ {- 5} $ inline $ . Essa precisão é suficiente para o modelo FC, que monitora o alvo de viragem.
Conclusão
Se o FC anterior era usado principalmente para resolver problemas de navegação, onde o uso de modelos de movimento linear produzia um bom resultado, então com o desenvolvimento de aplicações modernas, como robótica, visão computacional, etc., aumentava a necessidade de modelos mais complexos de movimento de objetos. Além disso, a aplicação da abordagem acima permite sintetizar um modelo de FC discreto, sem nenhum custo específico, o que facilitará a tarefa dos desenvolvedores. A única limitação dessa abordagem é que o modelo contínuo de um sistema dinâmico deve ser descrito por um conjunto de equações lineares, ou pelo menos linearizáveis, no espaço de estados.
Resumindo o exposto, podemos fornecer um algoritmo para a síntese da matriz de transição FC:
- Escrevendo a equação diferencial do sistema
- Transição para vetor de estado e espaço de estado
- Linearização, se necessário
- Representação da matriz de transição na forma de um expoente da matriz e truncamento da série, se necessário
- Cálculo das matrizes restantes, levando em consideração a matriz de transição
O autor recebe críticas construtivas sobre erros cometidos, imprecisões, formulações incorretas, métodos não mencionados e muito mais. Obrigado pela atenção!
Literatura usada
[1] Medic J. Estimativas lineares estatisticamente ótimas e controle. Per. do inglês Ed. A.S. Shatalova Moscou. Editora "Energy", 1973, 440 p.
[2] Matveev V.V. Noções básicas da construção de sistemas inerciais de amarração de São Petersburgo: Centro de Pesquisa Estatal da Federação Russa OJSC Concern TsNII Elektropribor, 2009. - 280s. ISBN 978-5-900180-73-3