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?).

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.

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

$$\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}$

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

com

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

resultando no modelo de medição:

$$\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$

$$\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$

$$\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:

$$\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.

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 = \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.

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