UNIVERSIDADE FEDERAL DE SO CENTRO FEDERAL DE EDUCAO

JOO DEL-REI PR-REITORIA DE TECNOLGICA DE MINAS GERAIS

PESQUISA DIRETORIA DE PESQUISA E PS-
GRADUAO

PROGRAMA DE PS-GRADUAO EM ENGENHARIA ELTRICA PPGEL

Sinais e Sistemas: FILTRO DE KALMAN PARA FUSO DE SENSORES

Waldri dos Santos Oliveira.

RESUMO:

O filtro de Kalman ser adotado na forma de um cdigo de programao em C, e coletado e

analizado via MATLAB, o algoritmo de filtro de Kalman, para aquisio de dados via hardwares,
microcontrolador (Arduino) e sensor IMU (MPU-6050) para o enquadramento referencial inercial
de corpo e referncia de um objeto. Aplicando o filtro de Kalman, aumenta-se a eficincia do
algoritmo, devido fuso de dados provenientes de diferentes sensores (acelermetro e giroscpio)
com diferentes tolerncias.

Palavras Chaves: Kalman; Filtro; Sensores.

1. INTRODUO

Rudolf Emil Kalman (1930), responsvel pela sua implementao, em 1958. O Filtro de
Kalman, um mtodo bastante utilizado quando se deseja atenuar a captao de rudos via software.
Este mtodo foi criado por Rudolf Kalman e tem por objetivo utilizar medies de grandezas
realizadas ao longo do tempo (contaminadas com rudo e outras incertezas) e gerar resultados que
tendam a se aproximar dos valores reais das grandezas medidas (WELCH; BISHOP, 2006).

1.1. A Importncia

O filtro de Kalman um filtro recursivo preditivo para medidas ruidosas. Uma das
aplicaes do filtro de Kalman sobre a normalizao, ou combinao poderada, para fuso
sensorual. (SABATELLI; GALGANI; FANUCC, ROCCHI, 2012)

1
1.2. Aplicao

A aplicao que ser abordada ao longo deste trabalho se dar sobre a fuso de sensores de
uma IMU (Inertial measurement unit), composta por Giroscpios e Acelermetros. A IMU, que
um sistema microeletromecnicos (Microelectromechanical systems - MENS) que registra
grandesas de velocidade (ou acelerao) e de orientao (ou toro), baseando-se sobre as foras
gravitacionais e vibratrias, utilizando uma combinao de acelermetro, giroscpio e/ou
magnetmetro as IMUs como so normalmente utilizadas para manobrar aeronaves, incluindo
veculos areos no tripulados (UAVs), embarcaes, aeronaves, naves espaciais, efetuadores em
braos robticos entre muitos outros, e, incluindo satlites e sondas. Recentes desenvolvimentos
permitem a produo de dispositivos GPS IMU-ativado. Um IMU permite que um vavegador GPS,
contnue a trabalhar quando o sinal GPS no esto disponveis, como em tneis, no interior de
edifcios, ou quando a interferncia eletrnica est presente.

Figura 1 Exemplo dos eixos e seus graus de liberdade monitorados por uma IMU. (INVENSENSE, 2013)

1.3. Sensor acelermetro

Um acelermetro um dispositivo que mede a acelerao total (esttica e dinmica). Devido

a este fato, o acelermetro detecta tanto acelerao angular (indiretamente pela gravidade), quanto
acelerao linear, (referente ao seu prprio deslocamento). Ou seja, a acelerao associada ao
fenmeno de peso/deslocamento experimentado por qualquer massa de teste em repouso (ou no)
no quadro de referncia espacial do dispositivo.

1.4. Sensor Giroscpio

2
 Um giroscpio convencional um mecanismo que compreende um rotor com dirio para
girar sobre um eixo, as revistas de o rotor ser montado no interior ou um cardan anel. J o
funcionamento de um giroscpio MENS, baseia-se se no fenmeno de vibrao piezo eltrico.

1.5. IMU

O IMU (Inertial Measurement Unit) ou unidade de medio inercial, previsto para emprego
o circuito integrado MPU-6050, que apresenta um giroscpio de trs eixos (x, y, z), um
acelermetro de trs eixos (x, y, z). Totalizando 6 DOF (degree of libert), graus de liberdade.

Figura 2 IMU proposto para o trabalho (INVENSENSE, 2013)

1.6. Fuso

O giroscpio responsvel pela medio das rotaes, ngulos yaw (guinada), pitch
(arfagem) e Roll (rolagem), da estrutura, do objeto ou aeronave. Abaixo temos um modelo
esquemtico de acordo com (RAOL, 1947), para uma aplicao simplificada do filtro de Kalman
para fuso de dois sensores.

Figura 3 - Conceito de fuso sensorial (RAOL, 1947)

3
2. REVISO BIBLIOGRFICA

2.1. Funcionamento do filtro de Kalman

Em (WELCH, BISHOP, 2006) temos o modelo da funo geral do filtro de Kalman.

Consiste em duas etapas, PREDIO e CORREO, baseado em tcnicas recursivas do sistema
em espaos de estado, sendo um a estimao sobre a dinmica do sistema. Durante a primeira etapa
realizada uma predio sobre a dinmica do modelo e no segundo passo uma correo, atuando na
covarincia do erro, neste sentido faz do Filtro de Kalman ser um estimador otimizador do estado
de (1) com a medio de (2).

= + + (1)

= + (2)

1
= (3)
0 1

= (4)
0

= (5)

As matrizes A e B so obtidas das propriedades construtivas do acelermetro, estes

parmetros podem ser modelados com informaes do fabricante e o emprego de equaes de
diferenas, e so dadas em funo da sada real do giroscpio , e do escorregamento em
graus por segundo (/s) ou , logo =[ ] e sua entrada de estado uma variao real
tambm em graus por segundo (/s) temos = . E H o modelo de observao, como o
estado real no pode ser observado apenas uma medida real do acelermetro (que possui medida
imprecisa real porem real), logo temos (6):

H = [1 0] (6)

Onde e variveis aleatrias representam o rudo do processo (ou sensor) e o de

medio, so consideradas como rudo branco gaussiano. Sendo que ( )~ ( , ) e ( )~( , ),
onde Q a matrizes de covarincia do rudo do erro do acelermetro, e R da medio (processo),
que so atualizadas a cada revoluo do filtro, porem sem prejuzo podem ser admite como
constantes. Que podem ser obtidas pelo datasheet da preciso e do erro de escorregamento ( ) do

4
acelermetro multiplicando por uma taxa ( ), resultando em (7). E temos (8) como erro estimado a
priori e (9) o erro estimado a posteriori do passo k. R a matriz de co-varincia. Deve ser
tipicamente o quadrado do desvio-padro do giroscpio, da folha de dados, ou apenas manter as
coisas estacionrio e registrar os dados por aproximadamente 5 minutos. Calculando a mdia e o
desvio std. = 2 (RAOL, 1947).
0
= 0 (7)

= [ ] (8)

= (9)

= (10)

O sobre escrito smbolos denotam valores de propagao (estimao) sem o sobre

escrito (a posteriori), e o denotam valores de aps atualizao (correo). Sendo o estado
real do sistema, o estado obtido pelo filtro de Kalman no estado estimado no tempo k. Ainda de
acordo com (WELCH, BISHOP, 2006), usando (9) e (10) obtemos o erro covariante a priori ou
erro covariante preditivo (11) e o erro covariante a posteriori ou erro covariante estimado (12):

= = . . + . . (11)

A estapa de atualizao (correo) corresponde a:

= [ ] = . . (12)

= . .( . . + ) (13)

= + .( ) (14)

=( . ) (15)

Onde o calculo do ganho dado em (13), atualizao do estado estimado (14), como uma
combinao linear do estado estimado a priori, ( ) e uma diferena ponderada entre uma medio
real ( ) e uma previso de medio ( ), em seguida a atualizao do erro de covarincia (15),
respectivamente fazem parte da segunda etapa do algoritmo, ou etapa de atualizao. Sendo P a
matriz de covarincia dos estados. Abaixo temos uma imagem esquemtica do funcionamento
recursivo das duas etapas de execuo do Filtro de Kalman:

5
 ATUALIZAO (CORREO):

PROPAGAO (ESTIMAO): 1. Calculo do ganho (K) de Kalman

1. Projeo de estado a frete = . ( . . + )

= + 2. Atualizao estimada com medio

2. Projeo de covarincia de erro a frete = + .( . )

= . . + 3. Atualizao de covarincia de erro.

=( . ).

Estimaes iniciais para e

Figura 4 Circuito modelo de Kalman (WELCH, BISHOP, 2006 adaptado)

2.2. Particularidades

De acordo com o encontrado em RAOL (1947), existem ao menos dois mtodos de

aplicao do filtro de Kalman para fuso sensorial: Algoritmo de atualizao de dados e Algoritmo
de propagao de estados. Alm termos o filtro clssico e o filtro completo, sendo que este ltimo
de acordo como FERDINANDO e Hany; KHOSWANTO 2012 afirmam requer maior custo
computacional. A partir de (WELCH, BISHOP, 2006), temos os pares de filtro de Kalman
discretos.

Realizando manipulaes matemticas e trigonomtricas adequadas, obtemos (16) onde o

ngulo estimado [ ] e calculado em funo do giroscpio ( [ ]) e acelermetro ( [ ]) e um
coeficiente de regulao. (VASCONCELLOS; CATUNDA, 2013)

[ ] = ( [ 1] + . [ ]) + (1 ) [ ] (16)

A equao (16) funciona como um filtro passa altas para o giroscpio e como um filtro
passa-baixa para o acelermetro, anulando os termos do giroscpio e discretizando a equao
resulta em (17).

= (1 ) (17)

A equao (17) corresponde funo de transferncia de um filtro passa baixas com polo
em . Sua resposta ao degrau (1 ) [ ], o que significa que um maior faz com que a
resposta ao degrau convirja mais rapidamente para seu valor de regime. Analisando novamente

6
(16), deixa claro que quando a velocidade angular fornecida pelo giroscpio for nula, o valor do
ngulo ir convergir para o valor do acelermetro, anulando o erro em regime do giroscpio (drift).

3. IMPLEMENTAO

De acordo com FERDINANDO, KHOSWANTO e PURWANTO (2012), a maioria das

implementaes do filtro de Kalnam so computacionais, isso se d devido ao principal problema
ser a linguagem de programao, para se implementar em micro controladores, os mais propcios a
esta tarefa os com suporte a linguagem em C.

3.1. Hardware

Os hardwares selecionados para o experimento so: uma placa micro controladora Arduino
Uno (ARDUINO, 2016), que trabalha com uma tenso de 5v (alimentada entre 7-12V), consumindo
20 a 50 mA, operando com 8 bits uma velocidade de 16 MHz, 26 pinos de I/O, dotada de um
micro controlador Atmega328P de fabricao da Atmel, que ira fazer a leitura do sensor MPU-
6050. Esta placa se responsvel por realizar as leituras do circuito integrado MPU-6050, dotado de
um giroscpio de trs eixos, um acelermetro de trs eixos e um termmetro digital (INVESENSE,
2012) e fazer a comunicao por porta serial com o Microcomputador, para posterior registro em
grficos via MatLab.

TABELA 1 Propriedade Arduino (INVENSENSE, 2013)

Microcontroller ATmega328P
Operating Voltage 5V
Input Voltage (recommended) 7-12V
Input Voltage (limit) 6-20V
Digital I/O Pins 14 (of which 6 provide PWM output)
PWM Digital I/O Pins 6
Analog Input Pins 6
DC Current per I/O Pin 20 mA
DC Current for 3.3V Pin 50 mA
Flash Memory 32 KB (ATmega328P)
of which 0.5 KB used by bootloader
SRAM 2 KB (ATmega328P)
EEPROM 1 KB (ATmega328P)
Clock Speed 16 MHz
Length 68.6 mm
Width 53.4 mm
Weight 25 g

7
3.2. Software.

Esta etapa consistem programar em linguagem C o filtro de Kalman no micro controlador

Arduino, e programar uma rotina de leitura e interpretao grfica no MatLab. Uma das vantagens
de se programar diretamente no Arduino e no no MatLab a reduo de rudo de medio,
proveniente da comunicao serial com o computador, o que implicaria em utilizar cabos USB com
blindagem, para atenuar este efeito, alm da evitar reduo de eficincia por delay de
processamento ou erros de perda de pacotes.

3.2.1. Filtro de Kalman digital em C

Foi programado o filtro de Kalman digital, em linguagem C, atravs da IDE prpria da

plataformar open source Arduino (ARDUINI, 2016), cujos detalhes podem ser observados no
Apndice B.

3.2.2. Funo de coleta e registro de dados

Foi elaborado uma funo em MatLab: SetupSerial6.m(porta,amostras), que recebe dois

argumentos: porta, e o amostras, o primeiro responsvel por identificar em que porta serial o
hardware esta conectado, e o segundo corresponder o numero de amostras do grfico que ser
plotado por esta funo. Este grfico registra trs variveis enviadas pelo micro controlador: xGyro
(giroscpio do eixo x), xAccel (acelermetro do eixo x) e xAccel_pred (ngulo preditivo aps
filtro de Kalman) coletadas em seu loop, registras dinamicamente, com atualizao dos intervalos
das abscissas, assim podemos observar grandes intervalos (exemplo amostras=1000).

4. PROBLEMA

Empregar os dados do acelermetro para minimizar o erro que o giroscpio apresenta erros
de deslizamento (drift), ou erros em regime (acelermetro est deslocado do centro de rotao). Em
contrapartida reduzir eliminar a amplificao de rudos dinmica caracterstica do acelermetro.
Realizando a fuso do sensor, atravs do algoritmo de filtro de Kalman para eliminar os rudos e
assim garantir uma medida angular adequada.

5. RESULTADOS
8
 A Conexo da IMU com o micro controlador dada via I2C, protocolo de dois fios com a
placa MPU-6050, a imagem (Figura 5) a seguir demonstra o esquema de ligao, a comunicao
com o computador via emulao USB-Serial, a velocidade de 9600 bps (bits por segundo).

Figura 5 Esquema de conexo MatLab / Arduino / MPU-6050 (autoria prpria)

Uma funo em cdigo .m recebe os dados e os registra on-line para anlise imediata ou
armazenamento. Realizando o comando: setupSerial6(COM3,200) no MatLab obtemos
duzentas amostras atravs da porta serial COM3, que podemos observar no grfico da Figura 6.

SERIAL COMMUNICATION MATLAB+ARDUINO

60

40

20
Volor das amostras

-20

-40

-60
0 20 40 60 80 100 120 140 160 180 200
Nmero de amostras

Figura 6 Resultado com a fuso

Como podemos observar na Figura 6, vemos em vermelho ( -o- ) apenas o sinal proveniente
do Giroscpio, o sinal isolado do acelermetro em azul ( -x- ), e o resultado da fuso dos sensores
em rosa ( ). Mantendo a IMU praticamente esttica, observamos que o resultado do giroscpio
diverge e inserindo rudos vemos que o acelermetro muito discrepante, em detalhe prximo da

9
amostra nmero 120, variar bruscamente (-18 a 15) em um intervalo de tempo curto. Este fenmeno
fica mais claro na imagem a seguir (Figura 7), aonde suas medidas chegam a extrapolar a escala do
grfico, no entanto a sada do filtro permanece estvel.

SERIAL COMMUNICATION MATLAB+ARDUINO

50

40

30

20
Valor das amostras

10

-10

-20

-30

-40

-50
0 20 40 60 80 100 120 140 160 180 200
Nmero de amostras

Figura 7 alto rudo.

A figura 8, podemos observar a eficincia do giroscpio para oscilaes rpidas e de

inclinao em curtas unidades de tempo, sendo que a sada do filtro de Kalman (em rosa) se
aproxima muito da resposta do giroscpio.

SERIAL COMMUNICATION MATLAB+ARDUINO

50

40

30

20

10
Valor das amostras

-10

-20

-30

-40

-50
0 10 20 30 40 50 60 70 80
Nmero de amostras

Figura 8 Seguimento giroscpio

10
6. CONCLUSES

O algoritmo de Kalman muito eficiente para medies ruidosas, e j conhecido pela

literatura sua aplicao em fuso sensorial, em alguns trabalhos sobre fuso de sensores
acelermetros e giroscpiocos, a pesar que em algumas publicaes este substitudo por uma
mdia ponderada simples. Para o funcionamento adequado do filtro de Kalman devemos inserir
valores apropriados de Q e R, onde Q esta ligada diretamente a atualizao da covarincia, ou seja,
se Q for muito pequeno o haver uma subestimao prejudicando a etapa de predio e estimao.
E como R esta ligada diretamente com S e consequentemente com o ganho K em (13), logo
prejudicando a etapa de atualizao e correo, reduzindo sua velocidade e consequentemente
eficincia. Como Q pertinente do sistema mais fcil de determinar, atravs do modelo, e no caso
deste trabalho, atravs dos dados do fabricante. R pertinente do processo, determinara-lo
consuma-se ser um processo mais impreciso, e muitas vezes estimado, estatisticamente atravs do
desvio padro, em um momento e ajustado experimentalmente em outro.

REFERNCIAS

[1] ARDUINO. Arduino & Genuino Products. Disponvel em:

<https://www.arduino.cc/en/main/arduinoBoardUno>. Acessado em 16/05/16.

[2] FERDINANDO, Hany; KHOSWANTO, Handry; PURWANTO, Djoko. Embedded

Kalman Filter For Inertial Measurement Unit (IMU) on the Atmega8535. 2012.

[3] INVEN SENSE. MPU-6000/6050 Product Specification. Disponvel em:

<https://www.cdiweb.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf> acessado
em 16/05/16.

[4] MATHWORKS. Arduino programing with MatLab. Disponvel em:

<http://www.mathworks.com/discovery/arduino-programming-matlab-
simulink.html?requestedDomain=www.mathworks.com>. Acessado em 27/05/2016.

[5] RAOL, J. R. (Jitendra R.). Multi-sensor data fusion with MATLAB. 1947-

[6] SABATELLI, Simone; GALGANI, Marco; FANUCC, Luca; ROCCHI, Alessandro. A

double stage Kalman filter for sensor fusion and orientation tracking in 9D IM. Dept. of
Information Engineering, University of Pisa, Pisa, Italy. 2012 IEEE.

11
[7] SABATELLI, Simone; SECHI, Francesco; ROCCHI, Alessandro; FANUCCI, Luca. A
sensor fusion algorithm for an integrated angular position estimation with inertial measurement
units. 2011 EDAA.

[8] VASCONCELLOS, Camila Simes da Costa Cunha; CATUNDA, Paulo Roberto Yamasaki.
Projeto, Construo e Controle de um Prottipo de um Quadrirrotor. Universidade Federal do Rio
de Janeiro. 114pg. 2013.

[9] WELCH, Greg; BISHOP Gary. An Introduction to the Kalman Filter. Department of
Computer Science University of North Carolina at Chapel Hill Chapel Hill. July 24, 2006.

7. APNDICE A

Para uma melhor compreenso do estudo segue o endereo de vdeo do experimento

completo em: www.youtu.be/FwQG_DPOCVY, que tambm pode ser visto em:
www.waldri.eng.br.

8. APNDICE B

Abaixo segue as manipulaes matemticas seguidas do correspondente em cdigo fonte C.:

= + (1)

1
= + (2)
0 1 0

+
= (3)

Primeiramente a partir de (1) em linhas de cdigo C teremos:

1. Angle += dt * rate;
2. Rate = newRate bias;

O Segundo passo considerando a partir de (11) e substituindo . . = temos:

= . . +

1 1 0
= . . +
0 1 0 1 0
12
 + ( + )
=
+

Em linguagem C a matriz (covarincia) fica assim:

3. P[0][0] += dt*(dt*P[1][1] - P[0][1] - P[1][0] + Q);

4. P[0][1] -= dt*(dt*P[1][1];
5. P[1][0] -= dt*P[1][1];
6. P[1][1] += Q*dt

Passo 3 (Atualizao com a mediao): Onde o erro de medio entre a medida ea

estimao do estado

= .

= [1 0]

7. Y=newAngle angle;

Passo 4, a partir de (13) temos:

=( . . + )

1
= [1 0]. . +
0

= +

8. S = P[0][0] + R_meansure;

Utilizando (13) novamente temos o passo 5:

= . .

1
= . .
0

13
 9. K[0] = P[0][0]/S;
10. K[1] = P[0][1]/S;

Passo 6 correo do angulo atravs do ganho do filtro e do erro de medio :

= + .

= + .

.
= +
.

Em linguagem C simplesmente temos:

11. angle += k[0]*y;

12. Bias += K[1]*y

Por ltomo (passo 7), temos:

=( . ).

1 0
= . [1 0] .
0 1

. .
=
. .

13. float P00_temp = P[0][0];

14. float P01_temp = P[0][1];
15. P[0][0] -= K[0] * P00_temp;
16. P[0][1] -= K[0] * P01_temp;
17. P[1][0] -= K[1] * P00_temp;
18. P[1][1] -= K[1] * P01_temp;

Definindo a partir da folha de dados do sensor (INVEN SENSE, 2013), obtemos erros de
medio e covarincia:
19. float Q_angle = 0.001;
20. float Q_gyroBias = 0.003;
21. float R_meansure = 0.03;
14
 Q_angle e Q_gyroBias, foram retirado da folha de dados em INVEN SENSE, 2013 e
R_mensure obtido por = 2 ou experimentalmente, ou ajustado experimentalmente.

15