Filtro Kalman 1D
Em 1960, num artigo1 que agora já é famoso, Rudolf Kalman construiu um filtro que consiste em minimizar a variância (de variáveis aleatórias gaussianas) associada a uma previsão através de um algoritmo definido por recorrência: obter uma nova estimativa usando a anterior adicionando um termo de correcção proporcional ao erro de previsão. Simples, não?
A ideia deste texto é explicar de uma forma simples a implementação de um filtro de Kalman num Arduino e construir um sensor de intensidade luminosa usando um LDR. O hardware necessário é o seguinte:
Hardware e código inicial
- Arduino (o qualquer coisa equivalente)
- LDR (light-dependent resistor)
- 100kOhm ou 10kOhm (resistência)
É fácil de encontrar um código exemplo para se ligar um LDR ao Arduino. Fica aqui uma versão simples ;)
int sensorPin = A0; int sensorValue = 0; void setup(){ Serial.begin(9600); } void loop(){ sensorValue = analogRead(sensorPin); Serial.println(sensorValue); }
O gráfico seguinte mostra a variação da luminosidade quando tapo e destapo o LDR com a mão. Vê-se claramente as oscilações de 50Hz da rede eléctrica na luz do candeeiro.
Filtro de Kalman 1D
Quero então estimar o valor da luminosidade da sala onde me encontro. Para isso
admito que a intensidade luminosa é constante (ou antes que necessita de ser
"quase" constante de modo a que seja possível fazer uma medição) e que o valor
medido y
possa ser decomposto na soma de um valor médio m
mais um termo de flutuação
gaussiano v
com variância r
(valor inicial). Assim
y:=m+v
Admito também que para cada valor da luminosidade se tem a estimativa x
dada
por
x:=m+w
com o mesmo valor médio e flutuações gaussianas mas com
variância q
(valor inicial). Note-se que w
e v
são variáveis aleatórias gaussianas com média
zero e independentes.
O filtro de Kalman admite que as sucessivas aproximações (modelo) são obtidas
linearmente das anteriores, i.e. xf:=a xi
. Como estou a admitir
que os sucessivos valores da luminosidade são constantes ponho a:=1
x:=x
e
p:=p+q
onde p
é variância da aproximação seguinte (são todas variáveis gaussianas independentes, a
variância da soma é a soma das variâncias).
Ficam assim estabelecidas as regras de evolução da primeira parte do algoritmo de Kalman. Vamos à segunda parte. Esta segunda parte corresponde a estimar/prever a aproximação seguinte que é escrita em termos da aproximação anterior e o erro de previsão, i.e.,
x:=x+k(y-x)
onde k
é um parâmetro a determinar, usualmente chamado de ganho do filtro, e y-x
o erro de previsão. Como
determinar então o valor de k
? O valor
de k
é determinado de forma a minimizar a variância da nova estimativa
x+k(y-x)
.
Calculando a variância desta última quantidade obtém-se
p:=(1-k)^2*p+k^2*r
É condição necessária para que se tenha um extremo relativo da variância que a
derivada de p
em ordem a k
seja zero. Isto dá:
k:=p/(p+r)
e
p:=p*r/(p+r)
Curiosamente, ou talvez não, esta última fórmula corresponde formalmente ao valor do paralelo
de duas resistências com valores p
e r
.
Fico assim com as duas últimas fórmulas que precisava:
k:=p/(p+r) x:=x+k(y-x) p:=(1-k)*p
O gráfico seguinte mostra o que acontece quando tapo e destapo o LDR (linha azul) numa escala de 0-1023. As linhas a verde e vermelho correspondem à implementação de filtros de Kalman com parâmetros diferentes.
Happy hacking!
/* Simply implementation of a 1D Kalman filter */ /* for Arduino using a LDR. */ /* Some other examples can be done with this file, */ /* e.g. sonar, temperature measurements, sky is the limit. */ /* Happy hacking! */ /* Author: Tiago Charters de Azevedo */ /* Maintainer: Tiago Charters de Azevedo */ /* URL: http://diale.org/ */ /* Version: 0 */ /* Copyright (c) - 2018 Tiago Charters de Azevedo */ /* This program is free software; you can redistribute it and/or modify */ /* it under the terms of the GNU General Public License as published by */ /* the Free Software Foundation; either version 3, or (at your option) */ /* any later version. */ /* This program is distributed in the hope that it will be useful, */ /* but WITHOUT ANY WARRANTY; without even the implied warranty of */ /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */ /* GNU General Public License for more details. */ /* You should have received a copy of the GNU General Public License */ /* along with this program; if not, write to the Free Software */ /* Foundation, Inc., 51 Franklin Street, Fifth Floor, */ /* Boston, MA 02110-1301, USA. */ /* Commentary: */ int sensorPin = A0; int sensorValue = 0; // 1st filter parameters float q=0.125; float r=32; float p=1023; float x=0; float k=0; // 2nd filter parameters float q1=4; float r1=100; float p1=1023; float x1=0; float k1=0; void setup(){ Serial.begin(9600); } void loop(){ sensorValue = analogRead(sensorPin); // read sensor value // 1st Kalman filter p=p+q; k=p/(p+r); x=x+k*(float(sensorValue)-x); p=(1-k)*p; // 2nd Kalman filter p1=p1+q1; k1=p1/(p1+r1); x1=x1+k1*(float(sensorValue)-x1); p1=(1-k1)*p1; //Print values Serial.print(sensorValue); // raw value Serial.print(", "); Serial.print(int(x)); // 1st filter output Serial.print(", "); Serial.print(int(x1)); // 2nd filter output Serial.println(""); }
1.R. Kalmam, A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME–Journal of Basic Engineering, 82 (Series D): 35-45. 1960
Palavras chave/keywords: kalman, arduino, filtroCriado/Created: 27-03-2018 [18:40]
Última actualização/Last updated: 10-10-2022 [14:47]
(c) Tiago Charters de Azevedo