Directorio de artículos
¿Qué tipos de filtrado existen?
Respuesta: Las señales de entrada analógicas de los sistemas de control por microordenador generalmente contienen diversos ruidos e interferencias, que provienen de la propia fuente de señal medida, sensores, interferencias externas, etc. Para una medición y control precisos, se deben eliminar el ruido y las interferencias de la señal bajo prueba:
El filtrado se divide en filtrado de hardware y filtrado de software:
l Filtrado de hardware: utilice filtros analógicos como filtros RC compuestos por resistencias y condensadores de determinadas especificaciones o filtros LC compuestos por inductores y condensadores para el filtrado;
l Filtrado de software: también conocido como filtrado digital, que reduce o debilita la influencia del ruido mediante determinados procedimientos de cálculo o juicio. 1. ¿Qué tipos de filtrado existen?
Respuesta: Las señales de entrada analógicas de los sistemas de control por microordenador generalmente contienen diversos ruidos e interferencias, que provienen de la propia fuente de señal medida, sensores, interferencias externas, etc. Para una medición y control precisos, se deben eliminar el ruido y las interferencias de la señal bajo prueba:
El filtrado se divide en filtrado de hardware y filtrado de software:
l Filtrado de hardware: utilice filtros analógicos como filtros RC compuestos por resistencias y condensadores de determinadas especificaciones o filtros LC compuestos por inductores y condensadores para el filtrado;
l Filtrado de software: también conocido como filtrado digital, que reduce o debilita la influencia del ruido mediante determinados procedimientos de cálculo o juicio.
Filtrado de hardware
¿Qué es un filtro?
Varias señales de sensores transportarán algunas señales de ruido hasta cierto punto, por lo que el filtro puede reducir y eliminar mejor el ruido y restaurar las señales verdaderas y útiles.
Un filtro es un circuito que elimina o "filtra" un rango específico de componentes de frecuencia. En otras palabras, separa el espectro de frecuencia de la señal en componentes de frecuencia que pasarán y componentes de frecuencia que serán bloqueados.
Si no tiene mucha experiencia con el análisis en el dominio de la frecuencia, es posible que aún no esté seguro de cuáles son estos componentes de frecuencia y cómo coexisten en una señal que no puede tener múltiples valores de voltaje al mismo tiempo. Veamos un breve ejemplo para ayudar a aclarar este concepto.
Digamos que tenemos una señal de audio que consta de una onda sinusoidal perfecta de 5 kHz. Sabemos cómo se ve una onda sinusoidal en el dominio del tiempo, en el dominio de la frecuencia solo vemos un "pico" de frecuencia a 5 kHz. Ahora supongamos que activamos un oscilador de 500 kHz para introducir ruido de alta frecuencia en la señal de audio.
La señal que se ve en el osciloscopio sigue siendo solo una secuencia de voltajes, con un valor en cada momento, pero la señal se verá diferente porque sus cambios en el dominio del tiempo ahora deben reflejar la onda sinusoidal de 5 kHz y las fluctuaciones del ruido de alta frecuencia.
Sin embargo, en el dominio de la frecuencia, las ondas sinusoidales y el ruido son componentes de frecuencia separados que coexisten en una señal. Las ondas sinusoidales y el ruido ocupan diferentes partes de la representación en el dominio de frecuencia de la señal, lo que significa que podemos filtrar el ruido con circuitos que dirigen la señal a través de frecuencias bajas y bloquean las frecuencias altas. El diagrama esquemático de la distribución de diferentes partes del dominio de frecuencia de las señales de onda sinusoidal y de ruido es el siguiente.
Tipo de filtro
Los filtros se pueden colocar en categorías amplias correspondientes a las características generales de la respuesta de frecuencia del filtro. Si el filtro deja pasar las frecuencias bajas y bloquea las frecuencias altas, se llama filtro de paso bajo; si bloquea las frecuencias bajas y deja pasar las frecuencias altas, es un filtro de paso alto. También hay filtros de paso de banda, que sólo dejan pasar un rango relativamente estrecho de frecuencias, y filtros de eliminación de banda, que sólo bloquean un rango relativamente estrecho de frecuencias. La representación en el dominio de la frecuencia de cada filtro se muestra en la siguiente figura.
Los filtros también se pueden clasificar según el tipo de componentes utilizados para implementar el circuito. Los filtros pasivos utilizan resistencias, condensadores e inductores; estos componentes no están equipados para proporcionar amplificación, por lo que los filtros pasivos solo pueden mantener o reducir la amplitud de la señal de entrada. Por otro lado, un filtro activo puede filtrar una señal y aplicar ganancia, ya que incluye componentes activos como transistores o amplificadores operacionales, como se muestra en la siguiente figura.
Este filtro activo de paso bajo se basa en la popular topología Sallen-Key. Los artículos relacionados se encuentran aquí: Filtros activos Sallen-Key .
Filtro de paso bajo RC
Para crear un filtro pasivo de paso bajo, necesitamos combinar elementos resistivos con elementos reactivos. En otras palabras, necesitamos un circuito formado por una resistencia y un condensador o inductor. Teóricamente, la topología de paso bajo de resistencia-inductor (RL) es equivalente a la topología de paso bajo de resistencia-condensador (RC) en términos de capacidades de filtrado. Pero en la práctica, el esquema resistencia-condensador es más común, por lo que el resto de este artículo se centrará en los filtros de paso bajo RC. El diagrama esquemático del filtro de paso bajo RC es el siguiente.
Se puede crear una respuesta de paso bajo RC colocando una resistencia en serie con la ruta de la señal y un capacitor en paralelo con la carga, como se muestra. En el diagrama, la carga es un solo componente, pero en circuitos reales puede ser más complejo, como un convertidor analógico a digital, un amplificador o la etapa de entrada de un osciloscopio utilizado para medir la respuesta de un filtro.
La acción de filtrado de una topología de paso bajo RC se puede analizar intuitivamente si nos damos cuenta de que las resistencias y los condensadores forman un divisor de voltaje dependiente de la frecuencia. Vuelva a dibujar el filtro de paso bajo RC para que parezca un divisor de voltaje, como se muestra a continuación.
Cuando la frecuencia de la señal de entrada es baja, la impedancia del capacitor es alta en relación con la impedancia de la resistencia; por lo tanto, la mayor parte del voltaje de entrada cae a través del capacitor (y a través de la carga, en paralelo con el capacitor). Cuando la frecuencia de entrada es alta, la impedancia del capacitor es menor en relación con la impedancia de la resistencia, lo que significa que el voltaje a través de la resistencia se reduce y se transfiere menos voltaje a la carga. Por lo tanto, las frecuencias bajas pasan y las altas se bloquean.
Esta explicación cualitativa de la función de paso bajo del RC es un primer paso importante, pero no es muy útil cuando realmente necesitamos diseñar el circuito porque los términos "alta frecuencia" y "baja frecuencia" son muy vagos. Los ingenieros necesitan crear circuitos que pasen y bloqueen frecuencias específicas. Por ejemplo, en el sistema de audio anterior, queremos preservar la señal de 5 kHz y suprimir la señal de 500 kHz. Esto significa que necesitamos un filtro que bloquee la transición de paso entre 5 kHz y 500 kHz.
Frecuencia de corte del filtro RC
El rango de frecuencia en el que el filtro no causa una atenuación significativa se llama banda de paso, y el rango de frecuencia en el que el filtro sí causa una atenuación significativa se llama banda de parada. Los filtros analógicos, como los filtros de paso bajo RC, siempre pasan gradualmente de la banda de paso a la banda de parada. Esto significa que no hay forma de identificar una frecuencia en la que el filtro deja de pasar la señal y comienza a bloquearla. Sin embargo, los ingenieros necesitan una forma cómoda y concisa de resumir la respuesta de frecuencia de un filtro, y aquí es donde entra en juego el concepto de frecuencia de corte.
Cuando observa el gráfico de respuesta de frecuencia de un filtro RC, notará que el término "frecuencia de corte" no es muy preciso. Una imagen en la que el espectro de la señal se "corta" por la mitad, uno de los cuales se conserva y el otro se descarta, no es aplicable porque la atenuación aumenta gradualmente a medida que la frecuencia se mueve desde debajo del límite hasta arriba del mismo.
La frecuencia de corte del filtro de paso bajo RC es en realidad la frecuencia a la que la amplitud de la señal de entrada se reduce en 3 dB (se eligió este valor porque una reducción de 3 dB en la amplitud corresponde a una reducción del 50 % en la potencia). Por lo tanto, la frecuencia de corte también se denomina frecuencia de -3 dB, que en realidad es un nombre más preciso e informativo. El término ancho de banda se refiere al ancho de la banda de paso del filtro, que en el caso de un filtro de paso bajo es igual a la frecuencia de -3 dB, como se muestra en la siguiente figura.
La figura anterior muestra las características generales de la respuesta de frecuencia de un filtro de paso bajo RC con un ancho de banda igual a -3dB de frecuencia.
Como se mencionó anteriormente, el comportamiento de paso bajo de un filtro RC es causado por la interacción entre la impedancia independiente de la frecuencia de la resistencia y la impedancia dependiente de la frecuencia del capacitor. Para determinar los detalles de la respuesta de frecuencia de un filtro, necesitamos analizar matemáticamente la relación entre la resistencia (R) y la capacitancia (C), y podemos manipular estos valores para diseñar un filtro que cumpla con especificaciones precisas. La frecuencia de corte (fC) del filtro de paso bajo RC se calcula de la siguiente manera:
Veamos un ejemplo de diseño simple. Los valores de los condensadores son más limitantes que los valores de las resistencias, por lo que comenzaremos con un valor de condensador común (como 10 nF) y luego usaremos esa fórmula para determinar el valor de resistencia requerido. El objetivo es diseñar un filtro que preserve la forma de onda de audio de 5 kHz y suprima la forma de onda de ruido de 500 kHz. Probaremos con una frecuencia de corte de 100kHz, más adelante en el artículo analizaremos más detenidamente el efecto de este filtro en los dos componentes de frecuencia, la fórmula es la siguiente.
Por tanto, la resistencia de 160Ω combinada con el condensador de 10nF nos dará un filtro muy cercano a la respuesta de frecuencia deseada.
Visualización de respuesta del filtro
La forma más conveniente de evaluar el efecto de un filtro en una señal es examinar un gráfico de la respuesta de frecuencia del filtro. Estos gráficos, a menudo llamados diagramas de Bode, tienen amplitud (en decibelios) en el eje vertical y frecuencia en el eje horizontal; el eje horizontal generalmente tiene una escala logarítmica tal que la distancia física entre 1 Hz y 10 Hz es la misma que entre 10 Hz y 10 Hz. Misma distancia física entre 100Hz, 100Hz y 1kHz, etc. Esta configuración nos permite evaluar de forma rápida y precisa el comportamiento del filtro en un amplio rango de frecuencia. La siguiente figura es un ejemplo de un diagrama de respuesta de frecuencia.
Cada punto de la curva representa la amplitud que tendría la señal de salida si la amplitud de la señal de entrada fuera 1V y la frecuencia fuera igual al valor correspondiente en el eje horizontal. Por ejemplo, cuando la frecuencia de entrada es de 1 MHz, la amplitud de salida (suponiendo una amplitud de entrada de 1 V) será de 0,1 V (ya que -20 dB corresponde a un factor de reducción diez veces mayor).
La forma general de esta curva de respuesta de frecuencia le resultará muy familiar a medida que pase más tiempo trabajando con circuitos de filtro. La curva es casi completamente plana en la banda de paso y luego comienza a disminuir más rápido a medida que la frecuencia de entrada se acerca a la frecuencia de corte. Finalmente, la tasa de cambio de atenuación (llamada caída) se estabiliza en 20 dB/década, es decir, por cada aumento de diez veces en la frecuencia de entrada, la amplitud de la señal de salida disminuye en 20 dB.
Evaluar el rendimiento del filtro de paso bajo
Si trazamos cuidadosamente la respuesta de frecuencia del filtro que diseñamos anteriormente en este artículo, veremos que la respuesta de amplitud a 5 kHz es esencialmente 0 dB (es decir, atenuación casi nula) y la respuesta de amplitud a 500 kHz es aproximadamente -14 dB (correspondiente a 0,2 de ganancia). ). Estos valores son consistentes con los cálculos que realizamos en la sección anterior.
Dado que un filtro RC siempre realiza una transición gradual de la banda de paso a la banda de supresión, y debido a que la atenuación nunca llega al infinito, no podemos diseñar un filtro "perfecto", uno que no tenga ningún efecto sobre la onda sinusoidal y elimine por completo el ruido. En cambio, siempre necesitamos hacer concesiones. Si acercamos la frecuencia de corte a los 5kHz tendremos más atenuación del ruido, pero también se atenuará más la onda sinusoidal que queremos enviar al altavoz. Si acercamos la frecuencia de corte a 500 kHz, tenemos menos atenuación en las frecuencias de onda sinusoidal, pero también menos atenuación en las frecuencias de ruido.
Anteriormente hemos discutido las formas en que los filtros modifican la amplitud de varios componentes de frecuencia en una señal. Sin embargo, además de los efectos de amplitud, los elementos reactivos del circuito siempre introducen cambios de fase.
cambio de fase del filtro de paso bajo
El concepto de fase se refiere al valor de una señal periódica en un momento específico dentro del ciclo. Entonces, cuando decimos que un circuito provoca un cambio de fase, queremos decir que crea una desviación entre la señal de entrada y la señal de salida: las señales de entrada y salida ya no comienzan y terminan sus períodos al mismo tiempo. El valor del cambio de fase (como 45° o 90°) indica la cantidad de desviación producida.
Cada elemento reactivo en el circuito introduce un cambio de fase de 90°, pero este cambio de fase no ocurre simultáneamente. La fase de la señal de salida, al igual que la amplitud de la señal de salida, cambia gradualmente a medida que aumenta la frecuencia de entrada. Hay un elemento reactivo (condensador) en el filtro de paso bajo RC, por lo que el circuito eventualmente introducirá un cambio de fase de 90°.
Al igual que con la respuesta de amplitud, la respuesta de fase se evalúa más fácilmente examinando un gráfico cuyo eje horizontal representa la frecuencia logarítmica. La siguiente descripción representa el patrón general; consulte la imagen a continuación para obtener más detalles.
- El cambio de fase es inicialmente de 0°.
- El cambio de fase aumenta gradualmente hasta alcanzar los 45° en la frecuencia de corte, y durante esta parte de la respuesta la tasa de cambio aumenta gradualmente.
- Después de la frecuencia de corte, el cambio de fase continúa aumentando, pero la tasa de cambio disminuye gradualmente
- A medida que el cambio de fase se acerca gradualmente a 90°, la tasa de cambio se vuelve muy pequeña.
La línea continua es la respuesta de amplitud y la línea discontinua es la respuesta de fase. La frecuencia de corte es de 100 kHz. Tenga en cuenta que el cambio de fase en la frecuencia de corte es de 45°.
filtro de paso bajo de segundo orden
Hasta ahora hemos asumido que un filtro de paso bajo RC consta de una resistencia y un condensador. Esta configuración es un filtro de primer orden. sugerencia relacionada:
El "orden" de un filtro pasivo está determinado por la cantidad de componentes reactivos (es decir, condensadores o inductores) en el circuito. Los filtros de orden superior tienen más componentes reactivos, lo que resulta en más cambios de fase y caídas más pronunciadas, siendo esta última la principal motivación para aumentar el orden de los filtros.
Agregar un elemento reactivo al filtro, por ejemplo, de primer orden a segundo orden o de segundo orden a tercer orden, aumenta la atenuación máxima en 20 dB/década.
Los filtros de segundo orden generalmente se construyen alrededor de un circuito resonante que consta de un inductor y un capacitor, una topología llamada RLC (Resistor-Inductor-Capacitor). Sin embargo, también es posible crear filtros RC de segundo orden. Como se muestra en la figura siguiente, todo lo que necesitamos hacer es conectar en cascada dos filtros RC de primer orden.
Si bien esta topología ciertamente produce una respuesta de segundo orden, no se usa ampliamente y su respuesta de frecuencia generalmente no es tan buena como la de un filtro activo de segundo orden o un filtro RLC de segundo orden.
Respuesta de frecuencia del filtro RC de segundo orden.
Podemos intentar diseñar un filtro de primer orden de acuerdo con la frecuencia de corte requerida y luego seleccionar dos de ellos para conectarlos en serie para formar un filtro de paso bajo RC de segundo orden. Esto permite que el filtro muestre una respuesta de frecuencia general similar, con una caída máxima de 40 dB/década en lugar de 20 dB/década.
Sin embargo, si miramos más de cerca la respuesta, vemos una reducción en la frecuencia de -3dB. El filtro RC de segundo orden no se comporta como se esperaba porque las dos etapas de filtrado no son independientes, por lo que no se pueden simplemente conectar los dos filtros entre sí y analizar el circuito como un filtro de paso bajo de primer orden superpuesto con un filtrado de paso bajo idéntico. .
Además, incluso si insertamos un buffer entre las dos etapas para que el RC de primer orden y el RC de segundo orden puedan usarse como filtros independientes, la atenuación en la frecuencia de corte original será de 6 dB en lugar de 3 dB. Esto se debe precisamente a que las dos etapas funcionan de forma independiente. El primer filtro tiene 3 dB de atenuación en la frecuencia de corte, mientras que el segundo filtro añade otros 3 dB de atenuación, como se muestra a continuación.
Una limitación fundamental de los filtros de paso bajo RC de segundo orden es que el diseñador no puede ajustar la transición de la banda de paso a la banda de parada ajustando el factor Q del filtro; este parámetro indica qué tan amortiguada está la respuesta de frecuencia. Si se conectan en cascada dos filtros de paso bajo RC idénticos, la función de transferencia general corresponde a una respuesta de segundo orden, pero el factor Q es siempre 0,5. Con Q=0,5, el filtro está al borde de estar sobreamortiguado, lo que hace que la respuesta de frecuencia "caiga" en la región de transición. Los filtros activos de segundo orden y los filtros resonantes de segundo orden no tienen esta limitación; los diseñadores pueden controlar el factor Q para ajustar la respuesta de frecuencia en la región de transición.
Resumir
Todas las señales eléctricas contienen una mezcla de componentes de frecuencia deseados y componentes de frecuencia no deseados. Los componentes de frecuencia no deseados suelen ser causados por ruido e interferencias y, en algunos casos, pueden afectar negativamente al rendimiento del sistema.
Los filtros son circuitos que reaccionan de diferentes maneras a diferentes partes del espectro de una señal. Un filtro de paso bajo está diseñado para pasar componentes de baja frecuencia mientras bloquea los componentes de alta frecuencia.
La frecuencia de corte de un filtro de paso bajo representa la región de frecuencia donde el filtro cambia de una atenuación baja a una atenuación significativa.
El voltaje de salida de un filtro de paso bajo RC se puede calcular considerando el circuito como un divisor de voltaje que consta de una resistencia (independiente de la frecuencia) y una reactancia (dependiente de la frecuencia).
Una gráfica de amplitud (en dB, en el eje vertical) versus frecuencia logarítmica (en Hz, en el eje horizontal) es una manera conveniente y eficiente de comprobar el comportamiento teórico de un filtro. También se puede utilizar la fase versus la frecuencia logarítmica. para determinar la cantidad de cambio de fase que se aplicará a la señal de entrada.
Los filtros de segundo orden tienen una caída más pronunciada; esta respuesta de segundo orden es útil cuando la señal no proporciona una separación de banda ancha entre los componentes de frecuencia deseados y no deseados.
Es posible crear un filtro de paso bajo RC de segundo orden construyendo dos filtros de paso bajo RC de primer orden idénticos y luego conectando la salida de uno a la entrada del otro, pero terminará con una frecuencia general de -3 dB más baja de lo esperado. .
Filtrado de software
| Método de filtrado | método | ventaja | defecto | la complejidad |
| método de filtro limitante | Con base en el juicio empírico, se determina que el valor de desviación máximo permitido para dos muestreos se establece en A, y se juzga cada vez que se detecta un nuevo valor; si la diferencia entre este valor y el último valor es <= A, se es válido; si la diferencia entre este valor y el último valor es Si la diferencia de valores es> A, entonces este valor no es válido, descarte este valor y reemplace este valor con el último valor. | Puede superar eficazmente la interferencia de pulso causada por factores accidentales | No se pueden suprimir las interferencias periódicas, poca suavidad | Aún no calculado |
| método de filtro mediano | Muestreo N veces seguidas (N toma un número impar), organiza los N valores de muestreo según el tamaño y toma el valor medio como el valor efectivo de este tiempo. | Puede superar eficazmente la interferencia de fluctuación causada por factores accidentales y tiene un buen efecto de filtrado en los parámetros medidos que cambian lentamente en temperatura y nivel de líquido. | No apto para parámetros que cambian rápidamente, como el caudal y la velocidad. | |
| Método de filtrado de media aritmética | Muestreo continuo de valores N para el cálculo del promedio aritmético; cuando el valor N es grande: la suavidad de la señal es alta, pero la sensibilidad es baja; cuando el valor N es pequeño: la suavidad de la señal es baja, pero la sensibilidad es alta; N selección de valor: caudal general, N = 12; presión: N = 4 | Es adecuado para filtrar señales que generalmente tienen interferencias aleatorias, de modo que la señal se caracteriza por un valor promedio y la señal fluctúa alrededor de un cierto rango de valores. | No es adecuado para el control de pruebas que requieren una velocidad de medición lenta o una velocidad de cálculo de datos rápida, lo cual es un desperdicio de RAM. | |
| método de filtrado promedio recursivo | Tome N valores de muestreo consecutivos como una cola; la longitud de la cola se fija en N; cada vez que se muestrean nuevos datos, se colocan al final de la cola y los datos originales al principio de la cola son descartado (primero en entrar, primero en salir); Calcular el promedio aritmético de N datos para obtener un nuevo resultado de filtrado; Vale la pena seleccionar N: flujo: N = 12; presión: N = 4; nivel de líquido: N = 4-12; temperatura : norte = 1-4 | Tiene un buen efecto de supresión de interferencias periódicas, alta suavidad y es adecuado para sistemas con oscilaciones de alta frecuencia. | Baja sensibilidad, mala supresión de interferencias de pulsos ocasionales, difícil de eliminar la desviación del valor de muestreo causada por interferencias de pulsos, no adecuado para ocasiones con interferencias de pulsos graves y desperdicio de RAM. | |
| Método de filtrado promedio mediano | Equivalente al "método de filtrado de mediana + método de filtrado de media aritmética", muestree continuamente N datos, elimine un valor máximo y un valor mínimo y luego calcule la media aritmética de N-2 datos, N = 3-14 | Combinando las ventajas de los dos filtros, puede eliminar la desviación del valor de muestreo causada por la interferencia ocasional del filtro de pulso. | La velocidad de medición es lenta y, al igual que el filtrado de media aritmética, desperdicia RAM. | |
| método de filtrado promedio recortado | Es equivalente a "limitar dos partes + método de filtrado promedio recursivo" Los nuevos datos muestreados cada vez se limitan primero y luego se envían a la cola para el filtrado promedio recursivo. | Combinando las ventajas de los dos filtros, puede eliminar la desviación del valor de muestreo causada por la interferencia ocasional del filtro de pulso. | Un desperdicio de RAM | |
| Método de filtrado de retraso de primer orden | Tome a = 0-1, (1-a) este valor de muestreo + un último resultado de filtrado | Tiene un buen efecto de supresión de interferencias periódicas y es adecuado para ocasiones con alta frecuencia de fluctuación. | Retraso de fase, baja sensibilidad, el grado de retraso depende del tamaño de a, y la señal de interferencia cuya frecuencia de filtrado es superior a la mitad de la frecuencia de muestreo no se puede eliminar | |
| filtro de promedio recursivo ponderado | Es una mejora con respecto al método de filtrado promedio recursivo, es decir, se dan diferentes pesos a los datos en diferentes momentos. Por lo general, cuanto más cerca de los datos en el momento actual, mayor será el peso. Cuanto mayor sea el coeficiente de ponderación dado al nuevo valor de muestra, mayor será la sensibilidad, pero menor será la suavidad de la señal. | Adecuado para sistemas con grandes constantes de tiempo de retardo puro | Para una señal con una constante de tiempo de retardo pura pequeña, un período de muestreo largo y una señal que cambia lentamente, no puede reflejar rápidamente la gravedad de la interferencia actual del sistema y el efecto de filtrado es deficiente. | |
| Método de filtrado antirrebote | Configure un contador de filtro para comparar cada valor muestreado con el valor efectivo actual: si el valor muestreado = el valor efectivo actual, el contador se borra; si el valor muestreado </>el valor efectivo actual, el contador + 1, y determine si el contador >= Límite superior N (desbordamiento); si el contador se desborda, reemplace el valor válido actual con este valor y borre el contador | Para parámetros medidos que cambian lentamente, el mejor efecto de filtrado puede evitar saltos repetidos de encendido/apagado del controlador cerca del valor crítico o fluctuación numérica en la pantalla. | No es adecuado para parámetros que cambian rápidamente. Si el valor de muestreo cuando el contador se desborda resulta ser un valor de interferencia, el valor de interferencia se importará al sistema como un valor válido. | |
| Limitar el método de filtrado antirrebote | Equivalente a "método de filtrado limitador + método de filtrado antirrebote", limitar primero y luego eliminar rebotes | Herede las ventajas de limitación y antirrebote, mejore los defectos en el método de filtro antirrebote y evite introducir valores de interferencia en el sistema. | No apto para parámetros que cambian rápidamente | |
| filtro de mantequilla | Adecuado para muestreo AD, se producirán fluctuaciones periódicas del pulso | Ocupa demasiada RAM, lleva mucho tiempo calcularlo y no es adecuado para parámetros que cambian rápidamente |
Editado de: https://www.jianshu.com/p/38ed6ff1b8c0
Nombre: Wang Keyi
Identificación de estudiante: 20021110373T
Reimpreso de: http://www.wtoutiao.com/p/1fe9dPI.html
[Introducción a Embedded Niu] Resume las características y el alcance de aplicación de algunos algoritmos de filtrado digital de uso común.
[Nariz de toro integrada] 1. Método de filtrado digital para superar la interferencia de pulsos grandes; 2. Método de filtrado promedio para suprimir el ruido de alta frecuencia de pequeña amplitud; 3. Método de filtrado compuesto
[texto de vaca incrustado]
Filtrado digital (filtrado digital): utilizando equipos digitales, a través de un determinado algoritmo, se procesa la señal, se filtra la señal de una determinada banda de frecuencia y el proceso de obtención de una nueva señal se denomina filtrado digital.
Los filtros digitales se pueden dividir en dos partes: filtros clásicos y filtros modernos. El filtro clásico supone que los componentes útiles y los componentes filtrados deseados en la señal de entrada x(n) están ubicados en diferentes bandas de frecuencia. Por lo tanto, podemos filtrar el ruido a través de un sistema lineal. Si el espectro del ruido y la señal se superponen entre sí, entonces el filtro clásico no puede cumplir con los requisitos de filtrado. Generalmente hay filtros de paso alto, filtros de paso bajo, filtros de paso de banda y filtros de parada de banda. Los filtros modernos estiman señales útiles y señales de ruido a partir de señales ruidosas. Este método trata tanto la señal como el ruido como señales aleatorias, utiliza sus características estadísticas, como la función de autocorrelación, la función de correlación cruzada, el espectro de potencia automática, el espectro de potencia cruzada, etc., para guiar el algoritmo de estimación de la señal y luego usa Equipo digital para implementarlo. En la actualidad, existen principalmente filtros digitales como el filtro Wiener, el filtro Kalman y el filtro adaptativo.
A continuación se presentan 8 métodos de filtrado de uso común:
(1) Método de filtrado digital para superar la interferencia de pulsos grandes:
Es el primer paso en el procesamiento de datos del instrumento para superar la perturbación repentina causada por factores accidentales en el entorno externo del instrumento o la interferencia de picos causada por errores de bits causados por la inestabilidad interna del instrumento. Generalmente se utiliza un método de filtrado no lineal simple.
1. Método de filtrado limitador (también conocido como método de filtrado de juicio de programa) El filtrado limitador consiste en juzgar el cambio de amplitud de la señal medida a través de un programa, eliminando así la interferencia de pulso agudo en la señal que cambia lentamente.
A. Método: basándose en el juicio empírico, determine el valor de desviación máximo permitido para dos muestreos (establecido en A) Juzgue cada vez que se detecta un nuevo valor: si la diferencia entre este valor y el valor anterior es A, entonces este valor es no válido y abandonado. Este valor, reemplace este valor con el último valor
B. Ventajas: Puede superar eficazmente la interferencia de pulsos causada por factores accidentales. Desventajas: No se pueden suprimir las interferencias periódicas y la suavidad deficiente.
C. Ámbito de aplicación: valores medidos que cambian lentamente
2. Método de filtrado mediano
El filtro de mediana es un filtro no lineal típico, tiene un funcionamiento sencillo y puede proteger bien la información detallada de la señal mientras filtra el ruido impulsivo.
A. Método: muestree continuamente N veces (N es un número impar), organice los N valores muestreados según el tamaño (principalmente utilizando el método de burbujeo) y esta vez tome el valor medio como valor efectivo.
B. Ventajas: Puede superar eficazmente la interferencia de fluctuación (pulso) causada por factores accidentales. Desventajas: No es adecuado para parámetros que cambian rápidamente, como el caudal y la velocidad.
C. Ámbito de aplicación: Tiene un buen efecto de filtrado sobre los parámetros medidos que cambian lentamente en temperatura y nivel de líquido.
(2) Método de filtrado promedio para suprimir el ruido de alta frecuencia de pequeña amplitud
Ruido electrónico de alta frecuencia y pequeña amplitud: ruido térmico de dispositivos electrónicos, ruido de cuantificación A/D, etc. Generalmente se utiliza un filtro lineal con características de paso bajo: método de filtro promedio aritmético, método de filtro promedio ponderado, método de filtro promedio ponderado deslizante, método de filtro de retardo de primer orden, etc.
3. Método de filtrado del promedio aritmético El método de filtrado del promedio aritmético consiste en sumar N valores de muestreo consecutivos y luego tomar el promedio aritmético como el valor filtrado de esta medición.
A. Método: Tome continuamente N valores de muestra y realice el cálculo promedio aritmético. Cuando el valor N es mayor: la suavidad de la señal es mayor, pero la sensibilidad es menor. Cuando el valor N es menor: la suavidad de la señal es menor, pero la sensibilidad es mayor Selección del valor N: Caudal general, N=12; presión: N=4
B. Ventajas: Es muy eficaz para filtrar señales de interferencia aleatoria mezcladas en la señal medida. La característica de la señal medida es que hay un valor promedio y la señal fluctúa dentro de un cierto rango de valores. Desventajas: No es fácil eliminar el error causado por la interferencia del pulso. Para sistemas en tiempo real con una velocidad de muestreo lenta o una alta tasa de actualización de datos, no se puede utilizar el método de filtrado de media aritmética. Un desperdicio de RAM
4. Método de filtrado de media recursiva (también conocido como método de filtrado de media móvil)
Para sistemas en tiempo real con una velocidad de muestreo lenta o una alta tasa de actualización de datos, no se puede utilizar el método de filtrado de media aritmética. El método de filtro de media móvil trata los datos de medición N como una cola. La longitud de la cola se fija en N. Cada vez que se realiza un nuevo muestreo, el resultado de la medición se coloca al final de la cola y los datos al principio de la La cola original se elimina, de modo que siempre esté en la cola. Hay N datos "más recientes".
A. Método: tome N valores de muestreo consecutivos como una cola y la longitud de la cola se fija en N. Cada vez que se muestrean nuevos datos, se colocan al final de la cola y los datos originales en el El encabezado de la cola se descarta (principio de primero en entrar, primero en salir) Realice una operación de promedio aritmético en los N datos en la cola para obtener un nuevo resultado de filtrado Selección de valor N: flujo, N=12; presión: N=4; nivel de líquido , N=4 12; temperatura, N= 1 4
B. Ventajas: Tiene un buen efecto de supresión de interferencias periódicas y una alta suavidad.Es adecuado para sistemas oscilantes de alta frecuencia. Desventajas: baja sensibilidad, mala supresión de interferencias de pulsos ocasionales, difícil eliminar la desviación del valor de muestreo causada por la interferencia de pulsos, no adecuado para ocasiones con interferencias de pulsos graves, más desperdicio de RAM
5. Método de filtrado promedio recursivo ponderado
A. Método: Es una mejora del método de filtrado promedio recursivo, es decir, se agregan diferentes pesos a los datos en diferentes momentos, generalmente cuanto más cerca de los datos actuales, mayor es el peso. Cuanto mayor sea el peso dado a los nuevos valores muestreados, mayor será la sensibilidad pero menor será la suavidad de la señal.
B. Ventajas: Es adecuado para objetos con una constante de tiempo de retardo pura grande y un sistema con un período de muestreo corto. Desventajas: la constante de tiempo de retardo puro es pequeña, el período de muestreo es largo, la señal que cambia lentamente no puede reflejar rápidamente la gravedad de la interferencia actual del sistema y el efecto de filtrado es deficiente
6. Método de filtrado de retardo de primer orden El filtro digital de paso bajo de primer orden utiliza software para implementar el filtrado RC de hardware para suprimir las señales de interferencia. En los canales de entrada analógica, los filtros analógicos RC de retardo de primer orden se utilizan comúnmente para suprimir la interferencia.
Cuando se utiliza este método para lograr interferencias de baja frecuencia, el primer problema encontrado es que se requiere que el filtro tenga una constante de tiempo grande (constante de tiempo = RC) y una red RC de alta precisión. Cuanto mayor sea la constante de tiempo, mayor será el valor RC requerido y la corriente de fuga inevitablemente aumentará, reduciendo así la precisión de la red RC. El método de filtrado digital con retardo de primer orden puede superar las deficiencias de este filtro analógico y es más adecuado cuando los requisitos constantes del filtro son grandes.
A. Método: a=Tf/(Tf+T)Tf es la constante de tiempo de filtrado. T es el período de muestreo. Este resultado del filtrado = (1-a) este valor de muestreo + el último resultado del filtrado.
B. Ventajas: Tiene un buen efecto de supresión de interferencias periódicas y es adecuado para ocasiones con alta frecuencia de fluctuación. Desventajas: retardo de fase, baja sensibilidad, el grado de retardo depende del valor de a, las señales perturbadoras con una frecuencia de filtrado superior a la mitad de la frecuencia de muestreo no se pueden eliminar.
(3) Método de filtrado compuesto
En aplicaciones prácticas, a veces es necesario eliminar la interferencia de pulsos a gran escala y lograr el suavizado de datos. Por lo tanto, dos o más métodos introducidos anteriormente se utilizan a menudo en combinación para formar un filtrado compuesto. Algoritmo de filtrado promedio de-extremo: primero use el algoritmo de filtro mediano para filtrar la interferencia impulsiva en los valores muestreados y luego promedie los valores muestreados restantes. Muestre continuamente N veces, elimine los valores máximo y mínimo y luego encuentre el promedio de las N-2 muestras restantes. Obviamente, este método puede suprimir las interferencias aleatorias y filtrar las interferencias de pulsos obvias.
7. Método de filtrado promedio medio (también conocido como método de filtrado promedio de interferencia anti-pulso)
El método de filtro de promedio medio es equivalente al "método de filtro de valor medio" + "método de filtro de promedio aritmético".
A. Método: muestrear continuamente N datos, eliminar un valor máximo y un valor mínimo, y luego calcular la media aritmética de los datos N-2. Selección del valor N: 3~14
B. Ventajas: combina las ventajas de dos métodos de filtrado: este método no solo puede suprimir la interferencia aleatoria, sino también filtrar la interferencia de pulso obvia. Desventajas: la velocidad de medición es lenta y, al igual que el método de filtrado de media aritmética, desperdicia RAM.
8. Limitar el método de filtrado promedio
En situaciones en las que la interferencia de pulsos es grave, si se utiliza el método de promedio general, la interferencia se promediará en el resultado. El método de filtrado promedio limitante es equivalente al "método de filtrado limitante" + "método de filtrado promedio recursivo"
A. Método: los nuevos datos muestreados cada vez se someten primero a un procesamiento de recorte y luego se envían a la cola para un procesamiento de filtrado promedio recursivo.
B. Ventajas: Combina las ventajas de los dos métodos de filtrado. Para la interferencia de pulso ocasional, se puede eliminar la desviación del valor de muestreo causada por la interferencia de pulso. Desventajas: desperdicio de RAM
C. Ámbito de aplicación: la señal que cambia lentamente se ha editado desde: https://www.jianshu.com/p/38ed6ff1b8c0
Nombre: Wang Keyi
Identificación de estudiante: 20021110373T
Reimpreso de: http://www.wtoutiao.com/p/1fe9dPI.html
[Introducción a Embedded Niu] Resume las características y el alcance de aplicación de algunos algoritmos de filtrado digital de uso común.
[Nariz de toro integrada] 1. Método de filtrado digital para superar la interferencia de pulsos grandes; 2. Método de filtrado promedio para suprimir el ruido de alta frecuencia de pequeña amplitud; 3. Método de filtrado compuesto
[texto de vaca incrustado]
Filtrado digital (filtrado digital): utilizando equipos digitales, a través de un determinado algoritmo, se procesa la señal, se filtra la señal de una determinada banda de frecuencia y el proceso de obtención de una nueva señal se denomina filtrado digital.
Los filtros digitales se pueden dividir en dos partes: filtros clásicos y filtros modernos. El filtro clásico supone que los componentes útiles y los componentes filtrados deseados en la señal de entrada x(n) están ubicados en diferentes bandas de frecuencia. Por lo tanto, podemos filtrar el ruido a través de un sistema lineal. Si el espectro del ruido y la señal se superponen entre sí, entonces el filtro clásico no puede cumplir con los requisitos de filtrado. Generalmente hay filtros de paso alto, filtros de paso bajo, filtros de paso de banda y filtros de parada de banda. Los filtros modernos estiman señales útiles y señales de ruido a partir de señales ruidosas. Este método trata tanto la señal como el ruido como señales aleatorias, utiliza sus características estadísticas, como la función de autocorrelación, la función de correlación cruzada, el espectro de potencia automática, el espectro de potencia cruzada, etc., para guiar el algoritmo de estimación de la señal y luego usa Equipo digital para implementarlo. En la actualidad, existen principalmente filtros digitales como el filtro Wiener, el filtro Kalman y el filtro adaptativo.
A continuación se presentan 8 métodos de filtrado de uso común:
(1) Método de filtrado digital para superar la interferencia de pulsos grandes:
Es el primer paso en el procesamiento de datos del instrumento para superar la perturbación repentina causada por factores accidentales en el entorno externo del instrumento o la interferencia de picos causada por errores de bits causados por la inestabilidad interna del instrumento. Generalmente se utiliza un método de filtrado no lineal simple.
1. Método de filtrado limitador (también conocido como método de filtrado de juicio de programa) El filtrado limitador consiste en juzgar el cambio de amplitud de la señal medida a través de un programa, eliminando así la interferencia de pulso agudo en la señal que cambia lentamente.
A. Método: basándose en el juicio empírico, determine el valor de desviación máximo permitido para dos muestreos (establecido en A) Juzgue cada vez que se detecta un nuevo valor: si la diferencia entre este valor y el valor anterior es A, entonces este valor es no válido y abandonado. Este valor, reemplace este valor con el último valor
B. Ventajas: Puede superar eficazmente la interferencia de pulsos causada por factores accidentales. Desventajas: No se pueden suprimir las interferencias periódicas y la suavidad deficiente.
C. Ámbito de aplicación: valores medidos que cambian lentamente
2. Método de filtrado mediano
El filtro de mediana es un filtro no lineal típico, tiene un funcionamiento sencillo y puede proteger bien la información detallada de la señal mientras filtra el ruido impulsivo.
A. Método: muestree continuamente N veces (N es un número impar), organice los N valores muestreados según el tamaño (principalmente utilizando el método de burbujeo) y esta vez tome el valor medio como valor efectivo.
B. Ventajas: Puede superar eficazmente la interferencia de fluctuación (pulso) causada por factores accidentales. Desventajas: No es adecuado para parámetros que cambian rápidamente, como el caudal y la velocidad.
C. Ámbito de aplicación: Tiene un buen efecto de filtrado sobre los parámetros medidos que cambian lentamente en temperatura y nivel de líquido.
(2) Método de filtrado promedio para suprimir el ruido de alta frecuencia de pequeña amplitud
Ruido electrónico de alta frecuencia y pequeña amplitud: ruido térmico de dispositivos electrónicos, ruido de cuantificación A/D, etc. Generalmente se utiliza un filtro lineal con características de paso bajo: método de filtro promedio aritmético, método de filtro promedio ponderado, método de filtro promedio ponderado deslizante, método de filtro de retardo de primer orden, etc.
3. Método de filtrado del promedio aritmético El método de filtrado del promedio aritmético consiste en sumar N valores de muestreo consecutivos y luego tomar el promedio aritmético como el valor filtrado de esta medición.
A. Método: Tome continuamente N valores de muestra y realice el cálculo promedio aritmético. Cuando el valor N es mayor: la suavidad de la señal es mayor, pero la sensibilidad es menor. Cuando el valor N es menor: la suavidad de la señal es menor, pero la sensibilidad es mayor Selección del valor N: Caudal general, N=12; presión: N=4
B. Ventajas: Es muy eficaz para filtrar señales de interferencia aleatoria mezcladas en la señal medida. La característica de la señal medida es que hay un valor promedio y la señal fluctúa dentro de un cierto rango de valores. Desventajas: No es fácil eliminar el error causado por la interferencia del pulso. Para sistemas en tiempo real con una velocidad de muestreo lenta o una alta tasa de actualización de datos, no se puede utilizar el método de filtrado de media aritmética. Un desperdicio de RAM
4. Método de filtrado de media recursiva (también conocido como método de filtrado de media móvil)
Para sistemas en tiempo real con una velocidad de muestreo lenta o una alta tasa de actualización de datos, no se puede utilizar el método de filtrado de media aritmética. El método de filtro de media móvil trata los datos de medición N como una cola. La longitud de la cola se fija en N. Cada vez que se realiza un nuevo muestreo, el resultado de la medición se coloca al final de la cola y los datos al principio de la La cola original se elimina, de modo que siempre esté en la cola. Hay N datos "más recientes".
A. Método: tome N valores de muestreo consecutivos como una cola y la longitud de la cola se fija en N. Cada vez que se muestrean nuevos datos, se colocan al final de la cola y los datos originales en el El encabezado de la cola se descarta (principio de primero en entrar, primero en salir) Realice una operación de promedio aritmético en los N datos en la cola para obtener un nuevo resultado de filtrado Selección de valor N: flujo, N=12; presión: N=4; nivel de líquido , N=4 12; temperatura, N= 1 4
B. Ventajas: Tiene un buen efecto de supresión de interferencias periódicas y una alta suavidad.Es adecuado para sistemas oscilantes de alta frecuencia. Desventajas: baja sensibilidad, mala supresión de interferencias de pulsos ocasionales, difícil eliminar la desviación del valor de muestreo causada por la interferencia de pulsos, no adecuado para ocasiones con interferencias de pulsos graves, más desperdicio de RAM
5. Método de filtrado promedio recursivo ponderado
A. Método: Es una mejora del método de filtrado promedio recursivo, es decir, se agregan diferentes pesos a los datos en diferentes momentos, generalmente cuanto más cerca de los datos actuales, mayor es el peso. Cuanto mayor sea el peso dado a los nuevos valores muestreados, mayor será la sensibilidad pero menor será la suavidad de la señal.
B. Ventajas: Es adecuado para objetos con una constante de tiempo de retardo pura grande y un sistema con un período de muestreo corto. Desventajas: la constante de tiempo de retardo puro es pequeña, el período de muestreo es largo, la señal que cambia lentamente no puede reflejar rápidamente la gravedad de la interferencia actual del sistema y el efecto de filtrado es deficiente
6. Método de filtrado de retardo de primer orden El filtro digital de paso bajo de primer orden utiliza software para implementar el filtrado RC de hardware para suprimir las señales de interferencia. En los canales de entrada analógica, los filtros analógicos RC de retardo de primer orden se utilizan comúnmente para suprimir la interferencia.
Cuando se utiliza este método para lograr interferencias de baja frecuencia, el primer problema encontrado es que se requiere que el filtro tenga una constante de tiempo grande (constante de tiempo = RC) y una red RC de alta precisión. Cuanto mayor sea la constante de tiempo, mayor será el valor RC requerido y la corriente de fuga inevitablemente aumentará, reduciendo así la precisión de la red RC. El método de filtrado digital con retardo de primer orden puede superar las deficiencias de este filtro analógico y es más adecuado cuando los requisitos constantes del filtro son grandes.
A. Método: a=Tf/(Tf+T)Tf es la constante de tiempo de filtrado. T es el período de muestreo. Este resultado del filtrado = (1-a) este valor de muestreo + el último resultado del filtrado.
B. Ventajas: Tiene un buen efecto de supresión de interferencias periódicas y es adecuado para ocasiones con alta frecuencia de fluctuación. Desventajas: retardo de fase, baja sensibilidad, el grado de retardo depende del valor de a, las señales perturbadoras con una frecuencia de filtrado superior a la mitad de la frecuencia de muestreo no se pueden eliminar.
(3) Método de filtrado compuesto
En aplicaciones prácticas, a veces es necesario eliminar la interferencia de pulsos a gran escala y lograr el suavizado de datos. Por lo tanto, dos o más métodos introducidos anteriormente se utilizan a menudo en combinación para formar un filtrado compuesto. Algoritmo de filtrado promedio de-extremo: primero use el algoritmo de filtro mediano para filtrar la interferencia impulsiva en los valores muestreados y luego promedie los valores muestreados restantes. Muestre continuamente N veces, elimine los valores máximo y mínimo y luego encuentre el promedio de las N-2 muestras restantes. Obviamente, este método puede suprimir las interferencias aleatorias y filtrar las interferencias de pulsos obvias.
7. Método de filtrado promedio medio (también conocido como método de filtrado promedio de interferencia anti-pulso)
El método de filtro de promedio medio es equivalente al "método de filtro de valor medio" + "método de filtro de promedio aritmético".
A. Método: muestrear continuamente N datos, eliminar un valor máximo y un valor mínimo, y luego calcular la media aritmética de los datos N-2. Selección del valor N: 3~14
B. Ventajas: combina las ventajas de dos métodos de filtrado: este método no solo puede suprimir la interferencia aleatoria, sino también filtrar la interferencia de pulso obvia. Desventajas: la velocidad de medición es lenta y, al igual que el método de filtrado de media aritmética, desperdicia RAM.
8. Limitar el método de filtrado promedio
En situaciones en las que la interferencia de pulsos es grave, si se utiliza el método de promedio general, la interferencia se promediará en el resultado. El método de filtrado promedio limitante es equivalente al "método de filtrado limitante" + "método de filtrado promedio recursivo"
A. Método: los nuevos datos muestreados cada vez se someten primero a un procesamiento de recorte y luego se envían a la cola para un procesamiento de filtrado promedio recursivo.
B. Ventajas: Combina las ventajas de los dos métodos de filtrado. Para la interferencia de pulso ocasional, se puede eliminar la desviación del valor de muestreo causada por la interferencia de pulso. Desventajas: desperdicio de RAM
C. Ámbito de aplicación: señal que cambia lentamente
1. Método de filtro limitador (también conocido como método de filtro de evaluación del programa)
/*
A、名称：限幅滤波法（又称程序判断滤波法）
B、方法：
根据经验判断，确定两次采样允许的最大偏差值（设为A），
每次检测到新值时判断：
如果本次值与上次值之差<=A，则本次值有效，
如果本次值与上次值之差>A，则本次值无效，放弃本次值，用上次值代替本次值。
C、优点：
能有效克服因偶然因素引起的脉冲干扰。
D、缺点：
无法抑制那种周期性的干扰。
平滑度差。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
int Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
Value = 300;
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Value = Filter_Value; // 最近一次有效采样的值，该变量为全局变量
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 限幅滤波法（又称程序判断滤波法）
#define FILTER_A 1
int Filter() {
int NewValue;
NewValue = Get_AD();
if(((NewValue - Value) > FILTER_A) || ((Value - NewValue) > FILTER_A))
return Value;
else
return NewValue;
}
/*
A、名称：限幅滤波法（又称程序判断滤波法）
B、方法：
根据经验判断，确定两次采样允许的最大偏差值（设为A），
每次检测到新值时判断：
如果本次值与上次值之差<=A，则本次值有效，
如果本次值与上次值之差>A，则本次值无效，放弃本次值，用上次值代替本次值。
C、优点：
能有效克服因偶然因素引起的脉冲干扰。
D、缺点：
无法抑制那种周期性的干扰。
平滑度差。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
int Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
Value = 300;
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Value = Filter_Value; // 最近一次有效采样的值，该变量为全局变量
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 限幅滤波法（又称程序判断滤波法）
#define FILTER_A 1
int Filter() {
int NewValue;
NewValue = Get_AD();
if(((NewValue - Value) > FILTER_A) || ((Value - NewValue) > FILTER_A))
return Value;
else
return NewValue;
}
2. Método de filtrado mediano
/*
A、名称：中位值滤波法
B、方法：
连续采样N次（N取奇数），把N次采样值按大小排列，
取中间值为本次有效值。
C、优点：
能有效克服因偶然因素引起的波动干扰；
对温度、液位的变化缓慢的被测参数有良好的滤波效果。
D、缺点：
对流量、速度等快速变化的参数不宜。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 中位值滤波法
#define FILTER_N 101
int Filter() {
int filter_buf[FILTER_N];
int i, j;
int filter_temp;
for(i = 0; i < FILTER_N; i++) {
filter_buf[i] = Get_AD();
delay(1);
}
// 采样值从小到大排列（冒泡法）
for(j = 0; j < FILTER_N - 1; j++) {
for(i = 0; i < FILTER_N - 1 - j; i++) {
if(filter_buf[i] > filter_buf[i + 1]) {
filter_temp = filter_buf[i];
filter_buf[i] = filter_buf[i + 1];
filter_buf[i + 1] = filter_temp;
}
}
}
return filter_buf[(FILTER_N - 1) / 2];
}
3. Método de filtrado de media aritmética
/*
A、名称：算术平均滤波法
B、方法：
连续取N个采样值进行算术平均运算：
N值较大时：信号平滑度较高，但灵敏度较低；
N值较小时：信号平滑度较低，但灵敏度较高；
N值的选取：一般流量，N=12；压力：N=4。
C、优点：
适用于对一般具有随机干扰的信号进行滤波；
这种信号的特点是有一个平均值，信号在某一数值范围附近上下波动。
D、缺点：
对于测量速度较慢或要求数据计算速度较快的实时控制不适用；
比较浪费RAM。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 算术平均滤波法
#define FILTER_N 12
int Filter() {
int i;
int filter_sum = 0;
for(i = 0; i < FILTER_N; i++) {
filter_sum += Get_AD();
delay(1);
}
return (int)(filter_sum / FILTER_N);
}
/*
A、名称：算术平均滤波法
B、方法：
连续取N个采样值进行算术平均运算：
N值较大时：信号平滑度较高，但灵敏度较低；
N值较小时：信号平滑度较低，但灵敏度较高；
N值的选取：一般流量，N=12；压力：N=4。
C、优点：
适用于对一般具有随机干扰的信号进行滤波；
这种信号的特点是有一个平均值，信号在某一数值范围附近上下波动。
D、缺点：
对于测量速度较慢或要求数据计算速度较快的实时控制不适用；
比较浪费RAM。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 算术平均滤波法
#define FILTER_N 12
int Filter() {
int i;
int filter_sum = 0;
for(i = 0; i < FILTER_N; i++) {
filter_sum += Get_AD();
delay(1);
}
return (int)(filter_sum / FILTER_N);
}
4. Método de filtrado de media recursiva (también conocido como método de filtrado de media móvil)
/*
A、名称：递推平均滤波法（又称滑动平均滤波法）
B、方法：
把连续取得的N个采样值看成一个队列，队列的长度固定为N，
每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据（先进先出原则），
把队列中的N个数据进行算术平均运算，获得新的滤波结果。
N值的选取：流量，N=12；压力，N=4；液面，N=4-12；温度，N=1-4。
C、优点：
对周期性干扰有良好的抑制作用，平滑度高；
适用于高频振荡的系统。
D、缺点：
灵敏度低，对偶然出现的脉冲性干扰的抑制作用较差；
不易消除由于脉冲干扰所引起的采样值偏差；
不适用于脉冲干扰比较严重的场合；
比较浪费RAM。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 递推平均滤波法（又称滑动平均滤波法）
#define FILTER_N 12
int filter_buf[FILTER_N + 1];
int Filter() {
int i;
int filter_sum = 0;
filter_buf[FILTER_N] = Get_AD();
for(i = 0; i < FILTER_N; i++) {
filter_buf[i] = filter_buf[i + 1]; // 所有数据左移，低位仍掉
filter_sum += filter_buf[i];
}
return (int)(filter_sum / FILTER_N);
}
/*
A、名称：递推平均滤波法（又称滑动平均滤波法）
B、方法：
把连续取得的N个采样值看成一个队列，队列的长度固定为N，
每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据（先进先出原则），
把队列中的N个数据进行算术平均运算，获得新的滤波结果。
N值的选取：流量，N=12；压力，N=4；液面，N=4-12；温度，N=1-4。
C、优点：
对周期性干扰有良好的抑制作用，平滑度高；
适用于高频振荡的系统。
D、缺点：
灵敏度低，对偶然出现的脉冲性干扰的抑制作用较差；
不易消除由于脉冲干扰所引起的采样值偏差；
不适用于脉冲干扰比较严重的场合；
比较浪费RAM。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 递推平均滤波法（又称滑动平均滤波法）
#define FILTER_N 12
int filter_buf[FILTER_N + 1];
int Filter() {
int i;
int filter_sum = 0;
filter_buf[FILTER_N] = Get_AD();
for(i = 0; i < FILTER_N; i++) {
filter_buf[i] = filter_buf[i + 1]; // 所有数据左移，低位仍掉
filter_sum += filter_buf[i];
}
return (int)(filter_sum / FILTER_N);
}
5. Método de filtrado promedio medio (también conocido como método de filtrado promedio de interferencia anti-pulso)
/*
A、名称：中位值平均滤波法（又称防脉冲干扰平均滤波法）
B、方法：
采一组队列去掉最大值和最小值后取平均值，
相当于“中位值滤波法”+“算术平均滤波法”。
连续采样N个数据，去掉一个最大值和一个最小值，
然后计算N-2个数据的算术平均值。
N值的选取：3-14。
C、优点：
融合了“中位值滤波法”+“算术平均滤波法”两种滤波法的优点。
对于偶然出现的脉冲性干扰，可消除由其所引起的采样值偏差。
对周期干扰有良好的抑制作用。
平滑度高，适于高频振荡的系统。
D、缺点：
计算速度较慢，和算术平均滤波法一样。
比较浪费RAM。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 中位值平均滤波法（又称防脉冲干扰平均滤波法）（算法1）
#define FILTER_N 100
int Filter() {
int i, j;
int filter_temp, filter_sum = 0;
int filter_buf[FILTER_N];
for(i = 0; i < FILTER_N; i++) {
filter_buf[i] = Get_AD();
delay(1);
}
// 采样值从小到大排列（冒泡法）
for(j = 0; j < FILTER_N - 1; j++) {
for(i = 0; i < FILTER_N - 1 - j; i++) {
if(filter_buf[i] > filter_buf[i + 1]) {
filter_temp = filter_buf[i];
filter_buf[i] = filter_buf[i + 1];
filter_buf[i + 1] = filter_temp;
}
}
}
// 去除最大最小极值后求平均
for(i = 1; i < FILTER_N - 1; i++) filter_sum += filter_buf[i];
return filter_sum / (FILTER_N - 2);
}
// 中位值平均滤波法（又称防脉冲干扰平均滤波法）（算法2）
/*
#define FILTER_N 100
int Filter() {
int i;
int filter_sum = 0;
int filter_max, filter_min;
int filter_buf[FILTER_N];
for(i = 0; i < FILTER_N; i++) {
filter_buf[i] = Get_AD();
delay(1);
}
filter_max = filter_buf[0];
filter_min = filter_buf[0];
filter_sum = filter_buf[0];
for(i = FILTER_N - 1; i > 0; i--) {
if(filter_buf[i] > filter_max)
filter_max=filter_buf[i];
else if(filter_buf[i] < filter_min)
filter_min=filter_buf[i];
filter_sum = filter_sum + filter_buf[i];
filter_buf[i] = filter_buf[i - 1];
}
i = FILTER_N - 2;
filter_sum = filter_sum - filter_max - filter_min + i / 2; // +i/2 的目的是为了四舍五入
filter_sum = filter_sum / i;
return filter_sum;
}*/
/*
A、名称：中位值平均滤波法（又称防脉冲干扰平均滤波法）
B、方法：
采一组队列去掉最大值和最小值后取平均值，
相当于“中位值滤波法”+“算术平均滤波法”。
连续采样N个数据，去掉一个最大值和一个最小值，
然后计算N-2个数据的算术平均值。
N值的选取：3-14。
C、优点：
融合了“中位值滤波法”+“算术平均滤波法”两种滤波法的优点。
对于偶然出现的脉冲性干扰，可消除由其所引起的采样值偏差。
对周期干扰有良好的抑制作用。
平滑度高，适于高频振荡的系统。
D、缺点：
计算速度较慢，和算术平均滤波法一样。
比较浪费RAM。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 中位值平均滤波法（又称防脉冲干扰平均滤波法）（算法1）
#define FILTER_N 100
int Filter() {
int i, j;
int filter_temp, filter_sum = 0;
int filter_buf[FILTER_N];
for(i = 0; i < FILTER_N; i++) {
filter_buf[i] = Get_AD();
delay(1);
}
// 采样值从小到大排列（冒泡法）
for(j = 0; j < FILTER_N - 1; j++) {
for(i = 0; i < FILTER_N - 1 - j; i++) {
if(filter_buf[i] > filter_buf[i + 1]) {
filter_temp = filter_buf[i];
filter_buf[i] = filter_buf[i + 1];
filter_buf[i + 1] = filter_temp;
}
}
}
// 去除最大最小极值后求平均
for(i = 1; i < FILTER_N - 1; i++) filter_sum += filter_buf[i];
return filter_sum / (FILTER_N - 2);
}
// 中位值平均滤波法（又称防脉冲干扰平均滤波法）（算法2）
/*
#define FILTER_N 100
int Filter() {
int i;
int filter_sum = 0;
int filter_max, filter_min;
int filter_buf[FILTER_N];
for(i = 0; i < FILTER_N; i++) {
filter_buf[i] = Get_AD();
delay(1);
}
filter_max = filter_buf[0];
filter_min = filter_buf[0];
filter_sum = filter_buf[0];
for(i = FILTER_N - 1; i > 0; i--) {
if(filter_buf[i] > filter_max)
filter_max=filter_buf[i];
else if(filter_buf[i] < filter_min)
filter_min=filter_buf[i];
filter_sum = filter_sum + filter_buf[i];
filter_buf[i] = filter_buf[i - 1];
}
i = FILTER_N - 2;
filter_sum = filter_sum - filter_max - filter_min + i / 2; // +i/2 的目的是为了四舍五入
filter_sum = filter_sum / i;
return filter_sum;
}*/
6. Limitar el método de filtrado promedio
/*
A、名称：限幅平均滤波法
B、方法：
相当于“限幅滤波法”+“递推平均滤波法”；
每次采样到的新数据先进行限幅处理，
再送入队列进行递推平均滤波处理。
C、优点：
融合了两种滤波法的优点；
对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差。
D、缺点：
比较浪费RAM。
E、整理：shenhaiyu 2013-11-01
*/
#define FILTER_N 12
int Filter_Value;
int filter_buf[FILTER_N];
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
filter_buf[FILTER_N - 2] = 300;
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 限幅平均滤波法
#define FILTER_A 1
int Filter() {
int i;
int filter_sum = 0;
filter_buf[FILTER_N - 1] = Get_AD();
if(((filter_buf[FILTER_N - 1] - filter_buf[FILTER_N - 2]) > FILTER_A) || ((filter_buf[FILTER_N - 2] - filter_buf[FILTER_N - 1]) > FILTER_A))
filter_buf[FILTER_N - 1] = filter_buf[FILTER_N - 2];
for(i = 0; i < FILTER_N - 1; i++) {
filter_buf[i] = filter_buf[i + 1];
filter_sum += filter_buf[i];
}
return (int)filter_sum / (FILTER_N - 1);
}
7. Método de filtrado de retraso de primer orden
/*
A、名称：一阶滞后滤波法
B、方法：
取a=0-1，本次滤波结果=(1-a)*本次采样值+a*上次滤波结果。
C、优点：
对周期性干扰具有良好的抑制作用；
适用于波动频率较高的场合。
D、缺点：
相位滞后，灵敏度低；
滞后程度取决于a值大小；
不能消除滤波频率高于采样频率1/2的干扰信号。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
int Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
Value = 300;
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 一阶滞后滤波法
#define FILTER_A 0.01
int Filter() {
int NewValue;
NewValue = Get_AD();
Value = (int)((float)NewValue * FILTER_A + (1.0 - FILTER_A) * (float)Value);
return Value;
}
8. Método de filtrado promedio recursivo ponderado
/*
A、名称：加权递推平均滤波法
B、方法：
是对递推平均滤波法的改进，即不同时刻的数据加以不同的权；
通常是，越接近现时刻的数据，权取得越大。
给予新采样值的权系数越大，则灵敏度越高，但信号平滑度越低。
C、优点：
适用于有较大纯滞后时间常数的对象，和采样周期较短的系统。
D、缺点：
对于纯滞后时间常数较小、采样周期较长、变化缓慢的信号；
不能迅速反应系统当前所受干扰的严重程度，滤波效果差。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 加权递推平均滤波法
#define FILTER_N 12
int coe[FILTER_N] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; // 加权系数表
int sum_coe = 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12; // 加权系数和
int filter_buf[FILTER_N + 1];
int Filter() {
int i;
int filter_sum = 0;
filter_buf[FILTER_N] = Get_AD();
for(i = 0; i < FILTER_N; i++) {
filter_buf[i] = filter_buf[i + 1]; // 所有数据左移，低位仍掉
filter_sum += filter_buf[i] * coe[i];
}
filter_sum /= sum_coe;
return filter_sum;
}
9. Método de filtrado anti-rebote
/*
A、名称：消抖滤波法
B、方法：
设置一个滤波计数器，将每次采样值与当前有效值比较：
如果采样值=当前有效值，则计数器清零；
如果采样值<>当前有效值，则计数器+1，并判断计数器是否>=上限N（溢出）；
如果计数器溢出，则将本次值替换当前有效值，并清计数器。
C、优点：
对于变化缓慢的被测参数有较好的滤波效果；
可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动。
D、缺点：
对于快速变化的参数不宜；
如果在计数器溢出的那一次采样到的值恰好是干扰值,则会将干扰值当作有效值导入系统。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
int Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
Value = 300;
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 消抖滤波法
#define FILTER_N 12
int i = 0;
int Filter() {
int new_value;
new_value = Get_AD();
if(Value != new_value) {
i++;
if(i > FILTER_N) {
i = 0;
Value = new_value;
}
}
else
i = 0;
return Value;
}
10. Limitar el método de filtrado antirrebote
/*
A、名称：限幅消抖滤波法
B、方法：
相当于“限幅滤波法”+“消抖滤波法”；
先限幅，后消抖。
C、优点：
继承了“限幅”和“消抖”的优点；
改进了“消抖滤波法”中的某些缺陷，避免将干扰值导入系统。
D、缺点：
对于快速变化的参数不宜。
E、整理：shenhaiyu 2013-11-01
*/
int Filter_Value;
int Value;
void setup() {
Serial.begin(9600); // 初始化串口通信
randomSeed(analogRead(0)); // 产生随机种子
Value = 300;
}
void loop() {
Filter_Value = Filter(); // 获得滤波器输出值
Serial.println(Filter_Value); // 串口输出
delay(50);
}
// 用于随机产生一个300左右的当前值
int Get_AD() {
return random(295, 305);
}
// 限幅消抖滤波法
#define FILTER_A 1
#define FILTER_N 5
int i = 0;
int Filter() {
int NewValue;
int new_value;
NewValue = Get_AD();
if(((NewValue - Value) > FILTER_A) || ((Value - NewValue) > FILTER_A))
new_value = Value;
else
new_value = NewValue;
if(Value != new_value) {
i++;
if(i > FILTER_N) {
i = 0;
Value = new_value;
}
}
else
i = 0;
return Value;
}
11. Filtro Kalman (Kalman no extendido)
#include <Wire.h> // I2C library, gyroscope
// Accelerometer ADXL345
#define ACC (0x53) //ADXL345 ACC address
#define A_TO_READ (6) //num of bytes we are going to read each time (two bytes for each axis)
// Gyroscope ITG3200
#define GYRO 0x68 // gyro address, binary = 11101000 when AD0 is connected to Vcc (see schematics of your breakout board)
#define G_SMPLRT_DIV 0x15
#define G_DLPF_FS 0x16
#define G_INT_CFG 0x17
#define G_PWR_MGM 0x3E
#define G_TO_READ 8 // 2 bytes for each axis x, y, z
// offsets are chip specific.
int a_offx = 0;
int a_offy = 0;
int a_offz = 0;
int g_offx = 0;
int g_offy = 0;
int g_offz = 0;
char str[512];
void initAcc() {
//Turning on the ADXL345
writeTo(ACC, 0x2D, 0);
writeTo(ACC, 0x2D, 16);
writeTo(ACC, 0x2D, 8);
//by default the device is in +-2g range reading
}
void getAccelerometerData(int* result) {
int regAddress = 0x32; //first axis-acceleration-data register on the ADXL345
byte buff[A_TO_READ];
readFrom(ACC, regAddress, A_TO_READ, buff); //read the acceleration data from the ADXL345
//each axis reading comes in 10 bit resolution, ie 2 bytes. Least Significat Byte first!!
//thus we are converting both bytes in to one int
result[0] = (((int)buff[1]) << 8) | buff[0] + a_offx;
result[1] = (((int)buff[3]) << 8) | buff[2] + a_offy;
result[2] = (((int)buff[5]) << 8) | buff[4] + a_offz;
}
//initializes the gyroscope
void initGyro()
{
/*****************************************
* ITG 3200
* power management set to:
* clock select = internal oscillator
* no reset, no sleep mode
* no standby mode
* sample rate to = 125Hz
* parameter to +/- 2000 degrees/sec
* low pass filter = 5Hz
* no interrupt
******************************************/
writeTo(GYRO, G_PWR_MGM, 0x00);
writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF
writeTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19
writeTo(GYRO, G_INT_CFG, 0x00);
}
void getGyroscopeData(int * result)
{
/**************************************
Gyro ITG-3200 I2C
registers:
temp MSB = 1B, temp LSB = 1C
x axis MSB = 1D, x axis LSB = 1E
y axis MSB = 1F, y axis LSB = 20
z axis MSB = 21, z axis LSB = 22
*************************************/
int regAddress = 0x1B;
int temp, x, y, z;
byte buff[G_TO_READ];
readFrom(GYRO, regAddress, G_TO_READ, buff); //read the gyro data from the ITG3200
result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
result[3] = (buff[0] << 8) | buff[1]; // temperature
}
float xz=0,yx=0,yz=0;
float p_xz=1,p_yx=1,p_yz=1;
float q_xz=0.0025,q_yx=0.0025,q_yz=0.0025;
float k_xz=0,k_yx=0,k_yz=0;
float r_xz=0.25,r_yx=0.25,r_yz=0.25;
//int acc_temp[3];
//float acc[3];
int acc[3];
int gyro[4];
float Axz;
float Ayx;
float Ayz;
float t=0.025;
void setup()
{
Serial.begin(9600);
Wire.begin();
initAcc();
initGyro();
}
//unsigned long timer = 0;
//float o;
void loop()
{
getAccelerometerData(acc);
getGyroscopeData(gyro);
//timer = millis();
sprintf(str, "%d,%d,%d,%d,%d,%d", acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2]);
//acc[0]=acc[0];
//acc[2]=acc[2];
//acc[1]=acc[1];
//r=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
gyro[0]=gyro[0]/ 14.375;
gyro[1]=gyro[1]/ (-14.375);
gyro[2]=gyro[2]/ 14.375;
Axz=(atan2(acc[0],acc[2]))*180/PI;
Ayx=(atan2(acc[0],acc[1]))*180/PI;
/*if((acc[0]!=0)&&(acc[1]!=0))
{
Ayx=(atan2(acc[0],acc[1]))*180/PI;
}
else
{
Ayx=t*gyro[2];
}*/
Ayz=(atan2(acc[1],acc[2]))*180/PI;
//kalman filter
calculate_xz();
calculate_yx();
calculate_yz();
//sprintf(str, "%d,%d,%d", xz_1, xy_1, x_1);
//Serial.print(xz);Serial.print(",");
//Serial.print(yx);Serial.print(",");
//Serial.print(yz);Serial.print(",");
//sprintf(str, "%d,%d,%d,%d,%d,%d", acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2]);
//sprintf(str, "%d,%d,%d",gyro[0],gyro[1],gyro[2]);
Serial.print(Axz);Serial.print(",");
//Serial.print(Ayx);Serial.print(",");
//Serial.print(Ayz);Serial.print(",");
//Serial.print(str);
//o=gyro[2];//w=acc[2];
//Serial.print(o);Serial.print(",");
//Serial.print(w);Serial.print(",");
Serial.print("\n");
//delay(50);
}
void calculate_xz()
{
xz=xz+t*gyro[1];
p_xz=p_xz+q_xz;
k_xz=p_xz/(p_xz+r_xz);
xz=xz+k_xz*(Axz-xz);
p_xz=(1-k_xz)*p_xz;
}
void calculate_yx()
{
yx=yx+t*gyro[2];
p_yx=p_yx+q_yx;
k_yx=p_yx/(p_yx+r_yx);
yx=yx+k_yx*(Ayx-yx);
p_yx=(1-k_yx)*p_yx;
}
void calculate_yz()
{
yz=yz+t*gyro[0];
p_yz=p_yz+q_yz;
k_yz=p_yz/(p_yz+r_yz);
yz=yz+k_yz*(Ayz-yz);
p_yz=(1-k_yz)*p_yz;
}
//---------------- Functions
//Writes val to address register on ACC
void writeTo(int DEVICE, byte address, byte val) {
Wire.beginTransmission(DEVICE); //start transmission to ACC
Wire.write(address); // send register address
Wire.write(val); // send value to write
Wire.endTransmission(); //end transmission
}
//reads num bytes starting from address register on ACC in to buff array
void readFrom(int DEVICE, byte address, int num, byte buff[]) {
Wire.beginTransmission(DEVICE); //start transmission to ACC
Wire.write(address); //sends address to read from
Wire.endTransmission(); //end transmission
Wire.beginTransmission(DEVICE); //start transmission to ACC
Wire.requestFrom(DEVICE, num); // request 6 bytes from ACC
int i = 0;
while(Wire.available()) //ACC may send less than requested (abnormal)
{
buff[i] = Wire.read(); // receive a byte
i++;
}
Wire.endTransmission(); //end transmission
}