Filtro Kalman 1D

... implementação simples de um filtro de Kalman usando Arduino (ou outra coisa qualquer)

Em 1960, num artigo¹ 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

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)

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, filtro

Criado/Created: 27-03-2018 [18:40]

Última actualização/Last updated: 10-10-2022 [14:47]

Voltar à página inicial.