Filtro Kalman - Implementação, Parâmetros e Ajuste

Primeiro de tudo, é a primeira vez que tento fazer um filtro Kalman.

Anteriormente, postei a pergunta a seguir Filtre o ruído e as variações dos valores de velocidade no StackOverflow, que descrevem o plano de fundo desta publicação. Este é um exemplo típico de valores que estou tentando filtrar. Eles não precisam necessariamente diminuir, o que é o caso aqui. Mas a taxa de mudança é normalmente assim

X ------- Y
16 --- 233,75
24 --- 234,01
26 --- 234,33
32 --- 234,12
36 --- 233,85
39 --- 233,42
47 --- 233,69
52 --- 233,68
55 --- 233,76
60 --- 232,97
66 --- 233,31
72 --- 233,99

Eu implementei meu Kalman Filter de acordo com este tutorial: Kalman Filter for Dummies .

Minha implementação é assim (pseudocódigo).

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

Começo com uma estimativa prévia de 0. Isso parece funcionar bem. Mas o que noto é que o kalmanGain diminuirá cada vez que essa atualização for executada, o que significa que confio nos meus novos valores menos quanto mais tempo o filtro estiver em execução (?). Eu não quero isso

Eu deixei de usar uma média móvel (simples e exponencial ponderada) para usar isso. No momento, nem consigo obter resultados tão bons quanto isso.

Minha pergunta é se esta é a implementação correta e se minha variação anterior de erro e desvio padrão parecem bons de acordo com os valores de amostra que eu publiquei? Na verdade, meus parâmetros são escolhidos aleatoriamente para ver se eu poderia obter bons resultados. Eu tentei várias faixas diferentes, mas com resultados ruins. Se você tiver alguma sugestão de alterações que eu possa fazer, isso seria muito apreciado. Me desculpe se faltam algumas coisas óbvias. Primeira postagem aqui também.

Os filtros Kalman são úteis quando o sinal de entrada consiste em observações ruidosas de algum estado do sistema dinâmico linear. Dada uma série de observações do estado do sistema, o filtro Kalman visa fornecer recursivamente estimativas cada vez melhores do estado do sistema subjacente. Para aplicá-lo com sucesso, é necessário ter um modelo para a dinâmica do sistema cujo estado você está estimando. Conforme descrito em detalhes na Wikipedia , este modelo descreve como se espera que o sistema subjacente do estado mude em uma etapa, dado seu estado anterior, quaisquer entradas no sistema e um componente estocástico distribuído gaussiano chamado ruído do processo.

Com isso dito, não está claro em sua pergunta se você tem algum modelo subjacente. A postagem vinculada indicava que você está consumindo valores de velocidade de um sensor. Eles podem ser modelados como observações diretas do estado de um sistema (onde o estado é sua velocidade) ou observações indiretas de seu estado (onde o estado é sua posição, por exemplo). Mas, para usar a estrutura de Kalman, você precisa escolher um modelo de como esse estado deve evoluir com o passar do tempo; essas informações adicionais são usadas para gerar a estimativa ideal. Um filtro Kalman não é uma caixa preta mágica que apenas "limpa" um sinal aplicado a ele.

Com isso dito, o fenômeno que você aludiu, onde o filtro Kalman se tornará cada vez mais confiante em sua própria saída, a ponto de as observações de entrada serem progressivamente ignoradas, acontece na prática. Isso pode ser atenuado aumentando manualmente os valores na matriz de covariância do ruído do processo. Então, qualitativamente, o modelo para a transição de estado do sistema contém um componente estocástico maior, de modo que a capacidade do estimador de prever com precisão o próximo estado, dado que o estado atual é diminuído. Isso reduzirá sua dependência de sua estimativa atual do estado do sistema e aumentará sua dependência de observações subsequentes, impedindo o efeito "ignorar a entrada".

Se entendi direito, você tem algo em movimento e pode observar a velocidade e essa velocidade é barulhenta. A partir das suas medidas, você observa dois tipos de variações. \

1. Variações causadas pelo ruído
2. Variações porque o objeto está realmente mudando a velocidade (por exemplo, girando)

A razão pela qual seu ganho de Kalman chega a zero é que você assumiu implicitamente que a velocidade do objeto é constante e tudo que você precisa fazer é estimar essa velocidade real.

" Ei, eu tenho um objeto que está se movendo a uma velocidade constante e quero estimar essa velocidade constante "

Seu modelo é assim, onde é a velocidade no tempo e é a medida correspondente.$x_k$$k$$y_k$

$$x_k = x_{k-1}$$ $$y_k = x_k + q_k$$

Mas seu objeto não está se movendo dessa maneira. Sua velocidade está mudando e você não sabe como e quando vai mudar.

O que você tem a dizer é:

" Ei, eu tenho um objeto que está se movendo a uma velocidade, mas não tenho certeza de como está mudando sua velocidade "

Existem várias maneiras de fazer isso: A maneira mais simples é adicionar incerteza ao seu estado.

$$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$$ $$y_k = x_k + q_k$$ onde e são assumidos como ruído branco.$q_k$$v_k$

Suas equações do Filtro Kalman ficarão assim:

$$\hat y_{k|k-1} = \hat x_{k|k-1}$$ $$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$$ $$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$$ $$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$$ $$P_{k+1|k} =P_{k|k} + Q^s$$

No seu caso, o 0.05valor é a covariância do ruído de observação, . Para fazer essa alteração, tudo o que você precisa fazer é definir , o ruído de estado covarinace (a incerteza em seu estado) com algum valor constante.Q $Q^o$$Q^s$

No seu código, a pequena modificação seria:

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

Ao não adicionar o stateVarianceou no seu código, você assumiu que é zero.$Q^s$

Esse stateVariancevalor pode ser o que você quiser. É baseado na sua confiança em quanto a velocidade realmente mudará. Se você acha que a velocidade permanecerá bastante constante, defina isso como um número pequeno.

Dessa forma, seu Kalman Gain não chegará a zero.

Você precisa de um sistema dinâmico para usar um filtro Kalman.

eu sugeriria

$$y = \sum\limits_{i =0}^n a_i\, x^i$$

$$a[k+1] = a[k] + w$$ $$cov(w) = Q$$ $$z = \sum\limits_{i =0}^n a_i\, x^i = y$$

$x$$a$

Eu acho que você poderia usar algumas idéias da teoria clássica de controle, por exemplo, controlador PID .

O seu sinal Y pode ser o ponto de ajuste do controlador u (t). A planta de processo é apenas 1 e y (t) será filtrada. Tudo o que você precisa fazer é definir os parâmetros (sintonia) P, I e D para obter o que deseja.

A saída y (t) tentará "seguir" a entrada u (t), mas os parâmetros controlam como será esse rastreamento.

O ganho diferencial D tornará sua resposta sensível a mudanças rápidas de erros. No seu caso, acho que D deve ser pequeno. Você não quer que y (t) mude se u (t) mudar abruptamente.

O ganho integral 'I' tornará sua resposta sensível ao erro acumulado. Você deve colocar um valor alto lá. Se u (t) muda de nível e o mantém lá, o erro será gerado e você deseja que y (t) faça o mesmo.

O ganho P pode dar uma sintonia fina. Enfim, tente brincar com os parâmetros e veja o que você recebe.

Existem métodos de ajuste complicados, no entanto, mas não acredito que você precise.

Boa sorte.

Aqui está uma implementação simples e limpa do Kalman Filter que segue as notações como na página da Wikipedia. https://github.com/zziz/kalman-filter