Universidad Veracruzana

Faculta de ingeniera | Boca del rio

Maestra en Ingeniera Aplicada | Sensores y actuadores

Practica: Obtencin de seales de velocidad y posicin con

acelermetro, aplicando filtro de Kalman
Equipo 1:
Jacobo Torres Figueroa
Hero Molina Snchez
Juan Omar Gamboa Cruz
Roberto Cazares Centeno
1 Introduccin
Todas las mediciones que se realizan sobre una variable fsica poseen ruido, ya sea
por cuestiones inherentes al propio sistema del que se extrae la informacin, debidas al
instrumento de medicin (sensor), o tambin por factores ambientales. El ruido es mayor
mente considerable en las seales analgicas, y como la gran mayora de las mediciones
fsicas resultan en seales analgicas que posteriormente son convertidas a seales
digitales, es muy importante realizar un tratamiento para disminuir o en el mejor de los
casos eliminar dicho ruido. Para tal motivo existen mltiples algoritmos informticos (filtro
complementario) y arreglos elctricos, como los filtros pasivos: pasa altas y pasa bajas, que
pueden ser implementados fsicamente, y ayudan a disminuir las frecuencias que no se
desean presentes en la seal.
Uno de los algoritmos ms destacados es en este mbito, es el filtro de Kalman, que
fue desarrollado por Rudolf E. Kalman en la dcada de los 60s, el cual bsicamente es un
observador de estados ocultos de sistemas dinmicos lineales, cuando estos estn
sometidos a ruido blanco. El algoritmo se basa en la determinacin de una ganancia, en este
caso denominada ganancia de retroalimentacin de Kalman, que es calculada en tiempo
real por el algoritmo de forma ptima, basndose en las varianzas de los ruidos que afectan
al sistema. El filtro de Kalman tiene numerosas aplicaciones en tecnologa. Una aplicacin
comn es la gua, navegacin y control de vehculos, especialmente naves espaciales.
Adems el filtro es ampliamente usado en campos como procesamiento de seales, fusin
de datos y econometra.
En esta prctica se aborda la implementacin del filtro de Kalman, para la reduccin
del ruido en la seal (analgica) obtenida en un acelermetro en aceleracin lineal sobre el
eje x, para posteriormente obtener la velocidad y posicin lineal, mediante una doble
integracin numrica.
2 Objetivos
Obtener las seales de aceleracin lineal en los ejes , y de un acelermetro
analgico.
Aplicar el filtro de Kalman para la eliminacin de ruido en la seal de aceleracin,
medida en el acelermetro en el eje .
Obtener las seales de velocidad y posicin lineales, mediante integracin
numrica.
Graficar las seales obtenidas (aceleracin, velocidad y posicin) y concluir
resultados.
3 Marco terico
Filtro de Kalman
El filtro de Kalman es aplicable en la estimacin de estados en sistemas dinmicos
lineales invariantes en el tiempo, modelados en el espacio de estados descritos como:
() = ()() + ()() (1)
() = ()() + ()() (2)
Donde () es el vector de estados, () es la matriz de estados, () es la matriz de
entrada, () es el vector de entradas al sistema o vector de control, () es la matriz de
salida y () es conocida como matriz de transicin, la cual normalmente es cero.
Sin embargo, para la aplicacin algortmica del filtro de Kalman en una computadora, el
modelo descrito por (1) y (2), se utiliza en tiempo discreto:
( + 1) = () + () (3)
() = () + () (4)
Donde k indica que la seal es muestreada a un determinado periodo.
El filtro de Kalman estima el estado de un sistema lineal discreto en el tiempo (3) y (4), a
partir de un conjunto de medidas :
= 1 + + 1 (6)
= + (7)
Donde (6) representa la ecuacin del proceso, (7) la ecuacin de la medida (obtenida del
sensor), 1 es el ruido del proceso y es el ruido de la medida, los cuales tienen que ser
independientes, blancos y con distribucin gaussiana, es decir:
() = (0, ), () = (0, )
Donde y son las covarianzas del proceso y de la medicin respectivamente. En resumen
el filtro de Kalman permite estimar el estado a partir de las mediciones anteriores y de
las estimaciones anteriores.
El filtro de Kalman en tiempo discreto, suele describirse en dos etapas: Prediccin y
Correccin.
Etapa de Prediccin
Estimacin a priori (prediccin del siguiente estado en cada muestra):
|1 = 1|1
Covarianza del error asociada a la estimacin a priori (prediccin de la siguiente covarianza):
|1 = 1|1 +
Etapa de Correccin
Actualizacin de la medicin:
= |1
Clculo de la ganancia de Kalman:
1
= |1 ( |1 + )
Estimacin a posteriori:
| = |1 +
Covarianza del error asociada a la estimacin a posteriori:
| = ( )|1
Donde:
: Matriz de transicin de estados, que relaciona a |1 con 1|1 .
|1: La estimacin a priori del vector de estados.
|1: La covarianza del error asociada a la estimacin a priori.
: Vector de mediciones en el instante k (provenientes del sensor).
: Matriz que indica la relacin entre las mediciones y el vector de estado en el instante k,
suponiendo que no hay ruido en las mediciones.
: Matriz de covarianza del ruido de las mediciones, la cual depende de la resolucin de
los sensores.

En la figura 1 se observa un diagrama que representa la implementacin del filtro de

Kalman de forma recursiva.
Prediccin: Correccin:

Figura 1. Algoritmo recursivo del filtro de Kalman.

Acelermetro MAA7361
El MMA7361LC es un acelermetro micromecanizado capacitivo de baja potencia, con
acondicionamiento de seal, filtro de 1 polo, compensacin de temperatura, autoprueba,
deteccin de 0g que detecta la cada libre lineal y g-Select que permite seleccionar entre 2
sensibilidades . El offset y la sensibilidad de cero-g se ajustan de fbrica y no requieren
dispositivos externos. El MMA7361LC incluye un modo de reposo que lo hace ideal para
electrnica de mano con pilas.

Figura 2. Acelermetro utilizado.

Este mdulo puede utilizarse para la eliminacin de seal, filtro de paso bajo unipolar y
tecnologa de compensacin de temperatura porque utiliza bajo costo micro acelermetro
capacitivo y cuatro rangos que es opcional para el usuario que elija en cuatro de la
sensibilidad. Este dispositivo incluye un filtro de paso bajo y una compensacin de cero g.
Este producto tambin proporciona el modo sleep o hibernacin y por lo que es la opcin
ideal para los productos porttiles con dispositivos de batera recargable.
Caractersticas:
1.- 3mm x 5mm x 1.0mm encapsulado LGA-14
2.- Bajo consumo de corriente: 400 A
3.- Modo sleep o hibernacin: 3 A
4.- Funcionamiento de Baja Tensin: 2.2 V - 3.6 V
5.- Alta sensibilidad (800 mV / g @ 1,5 g)
6.- Sensibilidad seleccionable ( 1.5g, 6g)
7.- Tiempo de encendido rpido (0,5 ms Tiempo de respuesta habilitado)
8.- Autotest para diagnstico de deteccin de cada libre
9.- 0g-Detectar para proteccin contra cada libre
10.- Acondicionamiento de seal con filtro de paso bajo
11.- Diseo Robusto, alta Resistencia a los Choques
12.- Cumple con la norma RoHS
13.- Producto Preferido Ambientalmente
14.- Voltaje de Trabajo: + 3.3V y 5V
15.- Tamao: 22mm * 15mm * 9mm
16.- Bajo Costo
17.- Peso: 2g
La tarjeta que incluye un sensor acelermetro MMA7361L de tres ejes analgico de
Freescale. Esta board cuenta con:
1.- Entrada de voltaje de 5 V y 3.3 V.
2.- Pines analgicos de salida. La tarjeta cuenta con tres pines analgicos que expresan as
coordenadas X, Y, y Z, las cuales representan un voltaje en movimiento del sensor en el eje
correspondiente.
3.- Tres pines de control y uno de salida digital (SL y OG, GS y ST):
ST: Self Test. El sensor proporciona una funcin de auto verificacin que permite la
verificacin de la integridad mecnica y elctrica del acelermetro en cualquier momento
antes o despus de la instalacin.
0G: 0g-Detect. El sensor ofrece una funcin que proporciona una seal lgica alta cuando
los tres ejes estn en 0g. Esta caracterstica permite a la aplicacin de la proteccin lineal
cada libre si la seal est conectado a un pin de interrupcin o un pin de E / S de monitoreo
en un micro-controlador.
GS: g-Select. Esta caracterstica permite la seleccin entre dos sensibilidades, esto de la
entrada lgica colocado en el pin 10, la ganancia interna del dispositivo se cambia
permitiendo que funcione con una o sensibilidad 1,5 g o 6 g
SL: Sleep Mode. El acelermetro de 3 ejes proporciona un modo de reposo que es ideal
para la batera. Cuando el modo de apagado est activado, las salidas del dispositivo estn
apagados, proporcionando una reduccin significativa de la corriente de funcionamiento.
Una seal de entrada de nivel bajo en el pin 7 (Modo de suspensin) coloca el dispositivo
en este modo y reduce la corriente a 3 A tip.
4.- Indicador de encendido.
Diagrama de funcionamiento

Figura 2b. Diagrama esquemtico del acelermetro.

Nota: El GS es controlado por los puertos I/O del microcontrolador en nivel bajo, "0" es nivel
bajo, "1" es nivel alto. Cuando GS est vacante, la interfaz es adquiere "0", entonces la
seleccin de rango es de 1,5 g.
Si utiliza la alimentacin de +5 V, entonces conecte el pin de +5 V y GND Si utiliza la
alimentacin de + 3.3V, conecte el pin de +3.3 V y GND. Cuando SL est vacante, la interfaz
es aquiescentemente para "1" si se enciende, la X, Y, Z tienen valor de salida. 0G es
deteccin de cada libre, cuando es normal es "0", pero cuando "cada libre" Salida es "1" y
se puede conectar a una seal de disparo de alarma. Cuando SL est vacante, la interfaz
adquiere un valor de "1" si se encuentra encendido, entonces funcionar. Se puede
conectar con el puerto de I/Ode MCU, si se le da la seal "0", Hibernacin, menor consumo
de energa.
4 Desarrollo de la prctica
Circuito y adquisicin de la seal
La prctica est basada en la placa Arduino UNO, adquiriendo la informacin proveniente
del sensor a travs de tres puertos analgicos: A0, A1 y A2, en los cuales van conectados las
tres seales e aceleracin lineal AcelX, AcelY y AcelZ respectivamente, aunado a esto el
sensor es alimentado con 3.3V desde la misma placa (ver figura 2).

Acel x
MMA7361
Acel y

Acel z
Arduino UNO

Figura 2. Diagrama de conexiones del circuito utilizado.

Las tres seales son procesadas en Arduino, sin embargo en esta prctica solo se realiza el
tratamiento de filtrado e integracin nicamente en el eje Y, mediante el siguiente
fragmento de cdigo:
const int sensorPin = A1;

int sensorVal = analogRead(sensorPin);

float vol = map(sensorVal, 0, 1023, -550, 400);
float acelX = vol * 0.04;
Del cdigo se observa que se declar una variable con el nombre de sensorPin para
identificar al convertidor analgico digital A1, la seal proveniente del sensor se adquiere
mediante la variable sensorVal, utilizando la funcin analoRead(), posterior a ello es
necesario realizar un mapeo de la seal para calibrar el cero, esto es porque el sensor
entrega 2.45 v como mximo en el eje Y y 0.85 V como mnimo.

La calibracin a cero es implementada mediante la funcin map(). Donde se desea llevar de

un valor medido del ADC, a su respectivo voltaje, por eso se utiliza el rango de 0 a 1024. El
mapeo ajusta un rango positivo y negativo de 230, lo que significa que existe una relacin
donde:
230 = 9.81 (1)

Despejando x se tiene que x = 0.042, lo cual representa el factor por el cual debe
multiplicarse la variable para obtener finalmente una salida en aceleracin, como la
mostrada en la figura 4.
Aceleracin (m/s2)

Tiempo (s)
Figura 4. Seal de aceleracin en el eje Y con ruido.

Como se observa de la figura 4, la seal oscila en un rango de aproximadamente 9.81 2 ,

que corresponde correctamente con la magnitud fsica del campo gravitacional terrestre.

Implementacin de filtro de Kalman

Para la implementacin del filtro de Kalman, se hicieron las siguientes consideraciones para
el sistema:
= 1 + + 1
= +
A = 1, B = 0 puesto que no ay seales de control y H = 1 puesto que solo se puede observar
el voltaje del sensor, el sistema queda reescrito como:
= 1 + 1 (8)
= + (9)
Donde:
1 : Covarianza del proceso y : la covarianza de la medicin.
Para determinar la covarianza de la medicin R, se obtuvieron 1283 datos de aceleracin
en el eje Y medidos del sensor, posteriormente en Excel se obtuvo la varianza, como se
observa en la figura 5. Resultando en un valor de 3.9E-3, por lo que:
= =3.9E-4

Figura 5. Calculo de la varianza de la medicin utilizando Excel.

La covarianza del proceso Q, es imprctica de calcular, pues est determinada por diversos
factores difcilmente medibles: Temperatura, radiacin electromagntica, humedad,
precisin del sensor, etc. Para fines prcticos este valor se considera sintonizable, es decir
que se determina de forma experimental a partir de un valor base muy pequeo, en este
coso se prob con un valor de 1E-7, por tanto:
= 1 = 2E-4
Considerando lo anterior y asumiendo que I = 1 y que = = 1, el algoritmo recursivo
expuesto en la seccin 2, queda resumido en:
|1 = 1|1
Covarianza del error asociada a la estimacin a priori (prediccin de la siguiente covarianza):
|1 = 1|1 +
Etapa de Correccin
Actualizacin de la medicin:
= |1
Clculo de la ganancia de Kalman:
1
= |1 (|1 + )
Estimacin a posteriori:
| = |1 +
Covarianza del error asociada a la estimacin a posteriori:
| = (1 )|1
La implementacin del algoritmo de Kalman en Arudino se observa en el siguiente
fragmento de cdigo:
// Variables para el filtro de Kalman

float varVoltX = 3.9E-03; // varianza determinada usando excel con

los datos raw del sensor
float varProceso = 2e-4; //Estimacin experimental de la varianza
del proceso
float Pc = 0.0; // Covarianza del error asociada a la estimacin a
priori
float G = 0.0; // // ganancia de Kalman
float P = 1.0;
float Xp = 0.0;
float Zp = 0.0;
float Xe = 0.0;
float ye = 0.0;

//Filtro de Kalman

Pc = P + varProceso;
G = Pc/(Pc + varVoltX);
P = (1-G)*Pc; // Covarianza del error asociada a la estimacin a
posteriori en cada iteracin
Xp = Xe; //Valor a posteriori = Valor estimado (Xe = 0.0 primer
iteracin)
Zp = Xp; //Actualizacin del valor medido
Xe = G*(acelX-Zp)+Xp; // Valor estimado por el Filtro de Kalman*/
 En la figura 6 se observa el resultado de la seal filtrada mediante el algoritmo de Kalman
implementado.

Seal ruido
Seal filtrada
Aceleracin (m/s2)

Tiempo (s)
Figura 6. Grafica comparativa de seal filtrada vs seal ruidosa.
Por otra parte variando el valor de , se puede disminuir an ms el ruido en la seal, sin
embargo, la seal filtrada presentar un retraso respecto de la seal con ruido, ejemplo de
ello se aprecia en la figura 8.
Aceleracin (m/s2)

Tiempo (s)
Figura 7. Efecto de retraso en la seal filtrada considerando = 1 = 5E-6.
La seal filtrada fue graficada en el IDE de Arduino mediante el Serial Plotter (ver Figura 6 y
7), sin embargo, para la integracin de la seal, esta es enviada va puerto serie a la
computadora, a una taza de 9600 baudios, y mediante un enlace con Matlab Simulink se
procede a la integracin.
Comunicacin arduino-Simulink (doble integracin de la seal de aceleracin)
Para introducir los datos provenientes del puerto serie, el cual corresponde al puerto USB
donde est conectado Arduino a la computadora, es preciso ubicar el nmero exacto del
puesto COM asignado por el sistema operativo (Windows), para ello puede revisarse desde
el IDE de arduino siguientico la ruta mostrada en la figura 8.

Figura 8. Ubicacin del puerto COM asignado para la placa Arduino.

O bien tambin es posible conocer el puerto COM asignado, revisando el administrador de
dispositivos de Windows, como se precia en la figura 9.
 Figura 9. Puerto COM en administrador de dispositivos.
Conociendo el puerto COM asignado, se procede a utilizar el bloque de Simulink
denominado Query Instrument, ubicado dentro de la librera de Simulink en el apartado
Instrument Control Tollbox (figura 10).

Figura 10. Ubicacin del bloque Query Instrument, dentro de la librera de Simulink.
Para acceder a los parmetros del bloque, es necesario hacer doble clic sobre l, y se
desplegar una ventana como la mostrada en la fiura 11.
1.- En el parmetro Block Sample Time, tiene que indicrsele un valor de Inf, puesto que de
esta forma el bloque acceder a los datos del puerto serie, cuando se presente uno.
 Introducir: Inf

Figura 11. Especificacin del tiempo de muestreo del bloque.

2.- Se tiene que seleccionar el puerto COM accediendo a la pestaa hardware Confiuration,
en el men desplegable Port as como indicar el Baudrate a una taza de 9600 baudios por
segundo, siempre y cuando en el men Port aparezca el puerto COM asignado a la placa
Arduino.
Si en la lista Port no aparece el puerto COM al que se le asign la placa Arduino utilizada,
es preciso, utilizar un objeto de interface de Matlab.
Para indicarle al bloque que se utilizar un objeto de Matlab como interface para enlazar el
puerto serial con el COM no contenido en la lista, es necesario marcar el Radial Button
denominado: Use interface object from MATLAB workspace, como se indica en la figura
12.
 Marcar Use Interface
Introducir nombre
del objeto interfaz

Figura 12. Enlace con el objeto serial de Matlab.

El nombre objectSerial hace referencia a una variable de tipo Serial que se tiene que
declarar con cdigo de MATLAB, este cdigo tiene que introducirse en el CallBack del
archivo de Simulink, esto se realiza en el paso 4.
3.- En la pestaa Query, de la venta Block Parameters Query Instrument, en el men Data
Format, se tiene que indicar que los datos vienen en formato ASCII, en el campo ASCII
format string, se indica el formato en el que los datos son enviados desde Arduino, en este
caso solo se enva el dato de la aceleracin en el eje Y, mediante una variable de tipo
flotante, mediante la funcin Serial.println(Xe,5) desde Arduino, donde Xe es la la
aceleracin en Y estimada por el filtro de Kalman, por lo que se tiene que indicar en el
campo: %f, lo que indica que se espera un valor de tipo flotante.
En el men After Initial Response se tiene que indicar Repeat for a new data, lo que indica
al bloque que tiene que seguir a la escucha del puerto en busca de un nuevo dato (ver figura
13). Finalmente se da clic en el botn Aceptar, ya que estas son las configuraciones en el
bloque.
 Indicar formato ASCII

Repeat query for

new data

Figura 13. Configuracin de parmetros en la pestaa Query

4.- Para indicar a Simulink la variable de interfaz con el puerto Serie, que se nombr en el
paso 2 como objectSerial, se tiene que crear una funcin de CallBack dentro de la
propiedades del modelo de Simulink, para se accede a la ruta: File > Model Properties >
Model Properties.

Figura 14. Acceso a los parmetros del modelo para introducir la funcin de CallBack.
En la ventana de propiedades del modelo (ver figura 15), se introduce el siguiente cdigo
de Matlab, en el apartado de: InitFnc del cuadro Model CallBacks.
serialPort = 'COM11';
serialObject = serial(serialPort);
set(serialObject, 'BaudRate',9600);
fopen(serialObject);
pause(1);
En el cdio introducido se indica el puerto COM que se le asign a la placa Arduino, tambin
se declara la variable de tipo Serial, en este caso nombrada como serialObject, as mismo
se indica la velocidad de trasmisin de datos: 9600 baudios, que debe de coincidir con el de
Arduino. Final mente se tiene que abrir el puerto con la funcin fopen(). Este fragmento de
cdigo se correr cada vez que se ejecute la simulacin.

Figura 15. Introduccin de la seal de CallBack en las propiedades del modelo.

Con estas configuraciones se asegura el establecimiento de la comunicacin entre el
modelo de Simulink y el puerto COM pertinente.
Diagrama de bloques completo del modelo de Simulink
Para obtener la posicin y la velocidad mediante la seal de aceleracin filtrada, se requiri
integrar la seal dos veces. El diagrama a bloques completo del modelo de Simulink
construido se observa en la figura 16.

Figura 16. Diagrama del modelo de Simulink completo.

A continuacin se presenta una descripcin de cada bloque utilizado y su configuracin.
Configuracin de los integradores:
Para ambos integradores, se requiere indicar en los bloques el mtodo de integracin, que
en este caso se utiliz: Forward Euler, indicado en el men Integrator method de la ventana
de propiedades desplegada, adems se indica el paso de integracin en -1 (Sample time),
lo cual indica que el bloque integrar al ritmo que le lleguen nuevos datos (ver figura 17).

Mtodo de
integracin

Ganancia igual a 1

Tiempo de
muestreo

Figura 17. Configuracin del bloque de integracin.

Bloque de interruptor manual (Manual Switch)
La funcin de este bloque en el modelo, es para tener control del paso de la seal de
aceleracin una vez que esta se ha estabilizado, ya que cuando se corre la simulacin suelen
ocurrir errores de lectura en los primeros instantes, de forma que con este bloque se
asegura que el flujo de la seal de aceleracin comience en un instante apropiado.
Bloque de zona muerta (Dead Zone)
Se establece un umbral de medida mnima en la seal de aceleracin que se considera cero,
esto es realizado con el objetivo de compensacin de las vibraciones, que causan una
variacin mnima en la seal de aceleracin, que finalmente provocan error de acarreo en
la integracin. Al entrar en las propiedades del bloque se establece el rango indicado en la
Figura 18.

Figura 18. Establecimiento del umbral de la zona muerta, de -0.5 a 0.5 se toma como cero.
Bloque To Workspace
Este bloque sirve para transferir los datos desde Simulink hacia el workspace de Matlab,
realizando esto con el objetivo del posterior graficado de los datos. Dentro del bloque tiene
que especificarse el nombre de la variable con el que se guardarn los datos y el tipo de
variable, es recomendable que sea de tipo Array, para facilidad en el tratamiento de los
datos, como se observa en la figura 19.
 Nombre de la
variable

Tipo de dato

Figura 19. Establecimiento de los parmetros del bloque To Workspace.

Final mente se tiene que configurar los parmetros de la simulacin (ver figura 20).

Figura 20. Acceso a los parmetros de la simulacin.

En la ventana de configuracin, tiene que indicarse el solucionador numrico (solver), en
este caso se us Ode1 (Euler), con un paso de integracin fijo (Fixed-step) de 0.03s. El
tiempo de simulacin comienza en 0s y en tiempo final se indica: inf, para que la simulacin
dure el tiempo deseado, tal como se observa en la figura 21.
 Figura 21. Configuracin de los parmetros de simulacin.
Con las configuraciones indicadas es posible comenzar la simulacin.
Resultados
En la figura 22 se observa la grfica obtenida en el eje Y en aceleracin, velocidad y posicin,
obtenida de la doble integracin numrica mediante el mtodo Forward Euler, a un paso
de integracin de 0.03s.

Figura 22. Trayectoria de aceleracin, velocidad y posicin en el eje Y, obtenido en

Simulink.
En la figura 22 se observa que el resultado fue satisfactorio en cuando a la doble integracin,
adems se observa la zona muerta en la seal de aceleracin.
Conclusiones
Fue posible aplicar correctamente el filtro de Kalman, para la disminucin del ruido en la
seal proveniente de un acelermetro. Adems de que se pudo obtener la seal de
velocidad y posicin al integrar de forma numrica la seal de aceleracin, sin embargo, se
observ un comportamiento no deseado, en cuanto a que la velocidad no regresa a cero,
cuando no hay aceleracin, y por tanto la seal de posicin se dispara.
Bibliografa
(Datasheet) MMA7361 - Triaxial Acceleration Sensor (1-page): ETC Datasheet PDF
(2017). Recuperado de http://www.datasheetspdf.com/PDF/MMA7361/900699/1
Kalman, R. E.; A New Approach to Linear Filtering and Prediction Problems,
Transactions of the ASME - Journal of Basic Engineering Vol. 82: pag. 35-45 (1960).
Anexo A: Cdigo Arduino
const int sensorPin = A1;
// Variables para el filtro de Kalman
float varVoltX = 3.9E-03; // varianza determinada usando excel con los datos
float varProcess = 25e-5; //Estimacin empirica de la varianza del proceso
float Pc = 0.0;
float G = 0.0;
float P = 1.0;
float Xp = 0.0;
float Zp = 0.0;
float Xe = 0.0;
float ye = 0.0;
void setup() {
Serial.begin(9600);
}

void loop() {
int sensorVal = analogRead(sensorPin);
float vol = map(sensorVal, 0, 1023, -550, 400);
float acelX = vol * 0.04;

//Filtro de Kalman
Pc = P + varProcess;
G = Pc/(Pc + varVoltX); // ganancia de Kalman
P = (1-G)*Pc;
Xp = Xe;
Zp = Xp;
Xe = G*(acelX-Zp)+Xp; // Valor estimado por el Filtro de Kalman*/

Serial.print(acelX);//Desactivar cuando se simula en simulink

Serial.print(',');//Desactivar cuando se simula en simulink
Serial.println(Xe,5);// Valor estimado filtro de Kalman, se env a simulink
delay(30);
}