Estimando a velocidade a partir da posição e aceleração conhecidas

Estou empolgado em modelar um modelo de sistema, ou seja, obter meu vetor de estado e vetor de entrada. Meu palpite é que posição e velocidade são vetor de estado e aceleração é vetor de entrada. Meu segundo palpite é que todas as três quantidades estão no vetor de estado e nenhuma no vetor de entrada.

Então ... o que é vetor de estado e o que é vetor de entrada no meu caso?

Informação adicional:

Recebo medições do sensor de posição e do sensor de aceleração. Tudo está acontecendo em 1D, por exemplo, em linha reta. Quero mesclar essas leituras (e remover o ruído) para obter uma estimativa de velocidade para cada passo de tempo.

Essas equações descrevem o sistema; Não tenho certeza se eles são modelados corretamente. Se eu entendi direito, é seguro prever que a aceleração é constante (mesmo que na realidade mude) - porque a matriz de covariância do processo corrige essa suposição (certo?). insira a descrição da imagem aqui

Também tenho alguns dados de exemplo para trabalhar (os valores de entrada não são barulhentos aqui por simplicidade):

 time    pos     acc      what I should get as output (velocity)
[0.0s]  0.000, -0.000  | 18.850
[0.1s]  1.885, -0.113  | 18.850
[0.2s]  3.768, -0.227  | 18.839
[0.3s]  5.650, -0.340  | 18.816
[0.4s]  7.528, -0.452  | 18.782
[0.5s]  9.401, -0.565  | 18.737

ADIÇÃO 2:

Para uma melhor comunicação, estou criando uma nova resposta, mas deve ser tratado como um comentário para a primeira resposta. Jason, você já me ajudou tremendamente e estou realmente agradecido pelo seu tempo. Ainda tenho problemas com isso - os resultados do Kalman Filter não são os esperados. Você pode encontrar o tempo, consulte o seguinte, obrigado. Eu já lhe devo uma cerveja ou duas (ou cofres, se quiser) - se você tiver paypal, entre em contato comigo no primoz [at] codehunter.eu :)

Eu implementei o modelo que Jason havia proposto na primeira resposta. Eu adicionei o idiota como quarta variável de estado. Depois de horas analisando, decidi voltar aqui para obter ajuda. Os valores que recebo da KF não são os esperados. A tabela abaixo representa os dados das 10 primeiras iterações do algoritmo. Observe como o idiota está aumentando a cada passo, tornando outras estimativas erradas. Após um segundo, a diferença entre a aceleração real e a estimativa é superior a 1m / s² (consulte a tabela na última linha)!

           real           measured                   estimated                    real 
time   pos     acc       pos    acc        pos      acc    jerk    vel[!]       velocity 
0.0   0.000  -0.000    -0.040  0.030  |  -0.300   -0.060   0.000   18.850  <-->  18.850
0.1   1.885  -0.113     1.965 -0.153  |   1.585   -0.061  -0.006   18.844  <-->  18.844
0.2   3.768  -0.227     3.778 -0.247  |   3.469   -0.066  -0.035   18.835  <-->  18.827
0.3   5.650  -0.340     5.750 -0.370  |   5.351   -0.090  -0.122   18.815  <-->  18.799
0.4   7.528  -0.452     7.358 -0.452  |   7.228   -0.152  -0.291   18.769  <-->  18.759
0.5   9.401  -0.565     9.251 -0.555  |   9.094   -0.282  -0.574   18.673  <-->  18.708
0.6   11.269 -0.677    11.309 -0.717  |   10.938  -0.518  -1.006   18.494  <-->  18.646
0.7   13.130 -0.788    13.260 -0.758  |   12.752  -0.840  -1.490   18.233  <-->  18.573
0.8   14.983 -0.899    15.043 -0.949  |   14.520  -1.286  -2.096   17.854  <-->  18.488
0.9   16.827 -1.009    16.977 -1.089  |   16.235  -1.838  -2.770   17.362  <-->  18.393
1.0   18.661 -1.118    18.831 -1.168  |   17.890  -2.477  -3.476   16.762  <-->  18.287

Minhas matrizes estão aqui:

O que está causando essa adição em cada timestep para idiota ? Alguma das minhas matrizes está errada?

O mesmo acontece com a primeira solução (apenas modelo de 3 estados) - a aceleração não está mudando como deveria.

ÚLTIMA EDIÇÃO:

Finalmente consegui fazê-lo funcionar. Não tenho certeza se houve um erro de implementação ou matrizes de P&Q erradas.

kalman-filters

— Primož Kralj
fonte

Para obter uma resposta útil, você precisa fornecer mais informações. Descreva o sistema. Mostre-nos quais equações você montou.

— Jim Clay

Obrigado pela sua resposta. Eu forneci informações adicionais.

— Primož Kralj

Por favor, alguém me ajude com isso - sinto como se estivesse perdendo a cabeça.

— Primož Kralj

Não vejo nenhum problema óbvio. É provável que você tenha um erro de implementação.

— Jason R #

@Phonon, sem ressentimentos. Eu sinto que eu estive em uma montanha-russa embora :)

— Primož Kralj

Respostas:

Você não faria uma suposição de aceleração constante nesse caso. Você normalmente faria isso se não tivesse meios de medir a aceleração do sistema, mas diz que é observável no seu caso. A maneira mais óbvia de modelar esse sistema seria usar o vetor de estado

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \end{matrix}]

$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \end{array} \right ]$

onde é a posição, é a velocidade e é a aceleração do sistema no instante instantâneo . Como você diz que pode medir a posição e a aceleração do sistema, o vetor de medição seria: $x_k$ $v_k$ $a_k$ $k$ $\mathbf{z_k}$

z_{k} = H x_{k} + v_{k}

$\mathbf{z_k} = \mathbf{Hx_k} + \mathbf{v_k}$

com

H = [\begin{array}{cc} 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}]

$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 \\ 0 && 0 && 1 \end{array} \right]$

resultando no modelo de medição:

z_{k} = [\begin{matrix} x_{k} \\ a_{k} \end{matrix}] + v_{k}

$\mathbf{z_k} = \left[ \begin{array}{c} x_k \\ a_k \end{array} \right] + \mathbf{v_k}$

onde é o ruído (gaussiano) inerente à medição. Agora, a menos que você esteja adicionando alguma força conhecida ao sistema que afetaria sua dinâmica, o vetor de entrada . Veja esta pergunta recente para uma discussão mais detalhada dos vários termos do modelo de Kalman, que você precisa saber e o que não . $\mathbf{v_k}$ $\mathbf{u_k} = \mathbf{0}$

Edit: Com relação ao comentário abaixo sobre como lidar com a matriz de transição de estado para o termo de aceleração: existem algumas maneiras diferentes de lidar com isso. Você pode assumir um modelo de zero-jerk (jerk é a derivada do tempo da aceleração); isso é equivalente a supor que . Ou você pode adicionar um quarto elemento ao seu vetor de estado: $a_{k+1}=a_k$

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \\ \overset{⃛}{x_{k}} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \\ j_{k} \end{matrix}]

$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \\ \dddot{x_k} \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \\ j_k \end{array} \right ]$

onde é o jerk do sistema, ou taxa de variação temporal da aceleração, no instante . Como você não está medindo diretamente o jerk, você teria uma matriz de medição de: $j_k$ $k$

H = [\begin{array}{cc} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}]

$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 && 0\\ 0 && 0 && 1 && 0 \end{array} \right]$

E a matriz de transição do estado se tornaria:

F = [\begin{array}{cccc} 1 & d t & \frac{1}{2} d t^{2} & \frac{1}{6} d t^{3} \\ 0 & 1 & d t & \frac{1}{2} d t^{2} \\ 0 & 0 & 1 & d t \\ 0 & 0 & 0 & 1 \end{array}]

$\mathbf{F} = \left[ \begin{array}{cccc} 1 && dt && \frac{1}{2}dt^2 && \frac{1}{6} dt^3 \\ 0 && 1 && dt && \frac{1}{2}dt^2 \\ 0 && 0 && 1 && dt \\ 0 && 0 && 0 && 1 \end{array} \right]$

Este é um modelo de "movimento constante" (semelhante aos modelos de velocidade constante ou aceleração constante que você pode ver em exemplos simples); sua matriz de transição de estado faz uma suposição implícita de que o empurrão é constante para todos os valores de k. Provavelmente não é verdade. Para lidar com esse aspecto do problema, você introduziria um termo de ruído do processo no componente jerk da equação de transição de estado.

Qualitativamente, isso permite que você expresse que não tem certeza de como o termo idiota mudará de tempos em tempos, mas que espera que as mudanças sejam aleatórias com alguma distribuição gaussiana. Essa é uma ferramenta frequentemente usada para "ajustar" modelos de filtro adaptável como este até encontrar um conjunto de parâmetros que funcione bem para o seu aplicativo.

— Jason R
fonte

Obrigado, você realmente acendeu uma luz na minha cabeça. Mas ainda não entendo qual é a minha aceleração que espera estar no próximo passo (se não for constante) - isso é importante para criar a matriz de transição A, a última linha para ser mais precisa. O que você sugeriria?

— Primož Kralj

Há uma confusão toda com as questões de migração. Parte da sua resposta se foi. Espero mods reparar isso. Além disso, veja a adição 2 na minha pergunta original.

— Primož Kralj

Consegui recuperar o texto editado e reaplicá-lo à minha resposta.

— Jason R

Você poderia explicar como obter covariância de processo para a primeira solução (3 variáveis de estado)? Não consigo encontrar um link decente on-line que o explique completamente na prática.

— Primož Kralj

Estou assumindo que suas variáveis de estado são de acordo com a resposta de Jason. Se for esse o caso, acredito que o seu problema pode ser sua matriz (ruído do processo): como é zero em todos os lugares, exceto na atualização do empurrão, os únicos termos que atualizam os valores da posição, velocidade e aceleração são as partes determinísticas do atualizar equações (exceto onde a natureza estocástica do empurrão é transmitida). $Q$

Tente usar uma matriz como: $Q$

Q = [\begin{array}{cccc} 0.00001 & 0 & 0 & 0 \\ 0 & 0.00001 & 0 & 0 \\ 0 & 0 & 0.00001 & 0 \\ 0 & 0 & 0 & 0.001 \end{array}]

$Q = \left [ \begin{array}{cccc} 0.00001 & 0& 0& 0\\ 0 & 0.00001 & 0 & 0\\ 0 & 0 & 0.00001 & 0 \\ 0 & 0& 0& 0.001 \end{array} \right ]$

Se esses elementos diagonais são zero, o modelo está dizendo "Não espero que a posição, velocidade ou aceleração mude (exceto pelas equações de atualização)" - o que claramente não é o caso.

— Peter K.
fonte

Boa explicação - mas por que escolher 0,001, por que não 2,84, por exemplo?

— Primož Kralj

Obrigado! O motivo para escolher 0,00001 sobre 2,84: Bem, 2,84 é MUITO grande para uma variação. Supus que você tivesse um raciocínio válido por trás do número do idiota (0,001) e que os outros números deveriam ser significativamente menores que isso. Se não houver motivo real para escolher o número do empurrão, tente o mesmo valor em todos os elementos diagonais.

— Peter K.

Mas o que isso significa na prática? Se eu disser que meu sensor de posição tem variação 0,00001 (primeiro elemento diagonal na matriz Q), isso significa que o erro é de + - 0,00001 metros, por exemplo (acho que não)? Eu acho que você escolhe uma variação menor para um sensor mais preciso (posição) e uma variação maior para um sensor com menos precisão (aceleração).

— Primož Kralj

Se você diz que a variação de uma medida é , na estrutura do filtro Kalman, você está dizendo que é gaussiano distribuído com um desvio padrão de . Portanto, ~ 68% das vezes, a magnitude do erro de medição será menor que , ~ 95% das vezes, será menor que e assim por diante.

0.00001

$0.00001$

\sqrt{0.00001}

$\sqrt{0.00001}$

\sqrt{0.00001}

$\sqrt{0.00001}$

2 \sqrt{0.00001}

$2\sqrt{0.00001}$

— Jason R

Obrigado a ambos, espero que este tópico também ajude futuros visitantes.

— Primož Kralj

Eu acho que o problema está em Q. Q estão cobrando K com seus valores. Consigo bons resultados quando Q é pré-carregado com poderes de delta_t.

Veja qualquer artigo sobre o modelo de salsicha. Tem uma boa matriz Q.

A00 deve ser múltiplo da potência delta_t de 5 A11 delta_t potência de 3 A22 delta_t potência de 1 A01 múltiplo da potência delta_t de 4

E assim por diante

Olhe para a

https://www.google.com/url?sa=t&source=web&rct=j&url=https://pdfs.semanticscholar.org/9c1b/db6b5a25c1231e775c654f731e1ca1fef31a.pdf&ved=2ahUKEWj4vJbVDB4L

— Jm Rams
fonte