# Arduino Ten filtering algorithm program complete （ Accurate Edition ）

• Limiting filtering （ Program judgment filtering ）
• Median filtering
• Arithmetic average filtering
• Recursive average filtering （ Moving average filtering ）
• Median average filtering method （ Anti pulse interference average filtering method ）
• Limiting average filtering method
• First order lag filtering
• Weighted recursive average filtering method
• Dithering filtering method
• The method of limiting amplitude and eliminating dithering
• Kalman filtering （ Non extended Kalman ）

The program defaults to int Type data to filter , If you need to filter other types , Just put all the int Replace with longfloat perhaps double that will do .

## Limiting filtering （ Program judgment filtering ）

``````/*
A、 name ： Limiting filtering （ It is also called program judgment filtering method ）
B、 Method ：
Judge by experience , Determine the maximum allowable deviation between two samples （ Set to A）,
Judge every time a new value is detected ：
If the difference between the current value and the last value <=A, Then this value is valid ,
If the difference between the current value and the last value >A, Then this value is invalid , Abandon this value , Replace the current value with the last value .
It can effectively overcome the impulse interference caused by accidental factors .
D、 shortcoming ：
There's no way to suppress that periodic interference .
Poor smoothness .
E、 Arrangement ：shenhaiyu 2013-11-01
*/

int Filter_Value;
int Value;

void setup() {
Serial.begin(9600);       //  Initialize serial communication
Value = 300;
}

void loop() {
Filter_Value = Filter();       //  Get the filter output value
Value = Filter_Value;          //  The value of the last valid sample , This variable is a global variable
Serial.println(Filter_Value); //  Serial output
delay(50);
}

//  Used to randomly generate a 300 The current value around
return random(295, 305);
}

//  Limiting filtering （ It is also called program judgment filtering method ）
#define FILTER_A 1
int Filter() {
int NewValue;
if(((NewValue - Value) > FILTER_A) || ((Value - NewValue) > FILTER_A))
return Value;
else
return NewValue;
}``````

## Median filtering

``````/*
A、 name ： Median filtering
B、 Method ：
Continuous sampling N Time （N Take an odd number ）, hold N The subsamples are sorted by size ,
Take the intermediate value as the current effective value .
It can effectively overcome the fluctuation interference caused by accidental factors ;
For temperature 、 The measured parameters with slow change of liquid level have good filtering effect .
D、 shortcoming ：
For flow 、 Fast changing parameters such as speed should not be .
E、 Arrangement ：shenhaiyu 2013-11-01
*/

int Filter_Value;

void setup() {
Serial.begin(9600);       //  Initialize serial communication
}

void loop() {
Filter_Value = Filter();       //  Get the filter output value
Serial.println(Filter_Value); //  Serial output
delay(50);
}

//  Used to randomly generate a 300 The current value around
return random(295, 305);
}

//  Median filtering
#define FILTER_N 101
int Filter() {
int filter_buf[FILTER_N];
int i, j;
int filter_temp;
for(i = 0; i < FILTER_N; i++) {
delay(1);
}
//  Sample values are arranged from small to large （ Bubbling ）
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];
}``````

## Arithmetic average filtering

``````/*
A、 name ： Arithmetic average filtering
B、 Method ：
Continuous access N Sample values are arithmetic averaged ：
N When it's worth more ： High signal smoothness , But the sensitivity is low ;
N It's worth less ： The signal smoothness is low , But the sensitivity is high ;
N Value selection ： General flow ,N=12; pressure ：N=4.
It is suitable for filtering signals with random interference ;
This signal is characterized by an average value , The signal fluctuates up and down near a certain range of values .
D、 shortcoming ：
It is not suitable for real-time control with slow measurement speed or fast data calculation speed ;
More wasteful RAM.
E、 Arrangement ：shenhaiyu 2013-11-01
*/

int Filter_Value;

void setup() {
Serial.begin(9600);       //  Initialize serial communication
}

void loop() {
Filter_Value = Filter();       //  Get the filter output value
Serial.println(Filter_Value); //  Serial output
delay(50);
}

//  Used to randomly generate a 300 The current value around
return random(295, 305);
}

//  Arithmetic average filtering
#define FILTER_N 12
int Filter() {
int i;
int filter_sum = 0;
for(i = 0; i < FILTER_N; i++) {
delay(1);
}
return (int)(filter_sum / FILTER_N);
}``````

## Recursive average filtering （ Moving average filtering ）

``````/*
A、 name ： Recursive average filtering （ It is also called moving average filtering method ）
B、 Method ：
Take the continuous acquisition of N Sample values as a queue , The length of the queue is fixed to N,
Every time a new data is sampled, it is put at the end of the queue , And throw away the data of the original team leader （ First in, first out principle ）,
Put... In the queue N Arithmetic average of data , New filtering results are obtained .
N Value selection ： Traffic ,N=12; pressure ,N=4; liquid surface ,N=4-12; temperature ,N=1-4.
It has a good inhibiting effect on periodic interference , High smoothness ;
For systems with high frequency oscillations .
D、 shortcoming ：
Low sensitivity , The inhibition of the occasional impulsive interference is poor ;
It is not easy to eliminate the sampling value deviation caused by pulse interference ;
It is not suitable for the occasion of serious pulse interference ;
More wasteful RAM.
E、 Arrangement ：shenhaiyu 2013-11-01
*/

int Filter_Value;

void setup() {
Serial.begin(9600);       //  Initialize serial communication
}

void loop() {
Filter_Value = Filter();       //  Get the filter output value
Serial.println(Filter_Value); //  Serial output
delay(50);
}

//  Used to randomly generate a 300 The current value around
return random(295, 305);
}

//  Recursive average filtering （ It is also called moving average filtering method ）
#define FILTER_N 12
int filter_buf[FILTER_N + 1];
int Filter() {
int i;
int filter_sum = 0;
for(i = 0; i < FILTER_N; i++) {
filter_buf[i] = filter_buf[i + 1]; //  All data moves left , The low post is still falling
filter_sum += filter_buf[i];
}
return (int)(filter_sum / FILTER_N);
}``````

## Median average filtering method （ Anti pulse interference average filtering method ）

``````/*
A、 name ： Median average filtering method （ It is also called the average filtering method of anti pulse interference ）
B、 Method ：
Take a group of queues, remove the maximum and minimum, and take the average ,
amount to “ Median filtering ”+“ Arithmetic average filtering ”.
Continuous sampling N Data , Take out a maximum and a minimum ,
And then calculate N-2 The arithmetic mean of data .
N Value selection ：3-14.
Integrated “ Median filtering ”+“ Arithmetic average filtering ” The advantages of the two filtering methods .
For occasional impulsive interference , It can eliminate the sampling value deviation caused by it .
It has a good inhibition on the cycle .
High smoothness , A system suitable for high frequency oscillation .
D、 shortcoming ：
The calculation speed is slow , It's the same as arithmetic average filtering .
More wasteful RAM.
E、 Arrangement ：shenhaiyu 2013-11-01
*/

int Filter_Value;

void setup() {
Serial.begin(9600);       //  Initialize serial communication
}

void loop() {
Filter_Value = Filter();       //  Get the filter output value
Serial.println(Filter_Value); //  Serial output
delay(50);
}

//  Used to randomly generate a 300 The current value around
return random(295, 305);
}

//  Median average filtering method （ It is also called the average filtering method of anti pulse interference ）（ Algorithm 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++) {
delay(1);
}
//  Sample values are arranged from small to large （ Bubbling ）
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;
}
}
}
//  Average after removing the maximum and minimum extremum
for(i = 1; i < FILTER_N - 1; i++) filter_sum += filter_buf[i];
return filter_sum / (FILTER_N - 2);
}

//   Median average filtering method （ It is also called the average filtering method of anti pulse interference ）（ Algorithm 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++) {
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  The purpose is to round
filter_sum = filter_sum / i;
return filter_sum;
}*/``````

## Limiting average filtering method

``````/*
A、 name ： Limiting average filtering method
B、 Method ：
amount to “ Limiting filtering ”+“ Recursive average filtering ”;
Each time the new data is sampled, the amplitude is limited ,
Then it is sent to the queue for recursive average filtering .
It combines the advantages of the two filtering methods ;
For occasional impulsive interference , It can eliminate the sampling value deviation caused by pulse interference .
D、 shortcoming ：
More wasteful RAM.
E、 Arrangement ：shenhaiyu 2013-11-01
*/

#define FILTER_N 12
int Filter_Value;
int filter_buf[FILTER_N];

void setup() {
Serial.begin(9600);       //  Initialize serial communication
filter_buf[FILTER_N - 2] = 300;
}

void loop() {
Filter_Value = Filter();       //  Get the filter output value
Serial.println(Filter_Value); //  Serial output
delay(50);
}

//  Used to randomly generate a 300 The current value around
return random(295, 305);
}

//  Limiting average filtering method
#define FILTER_A 1
int Filter() {
int i;
int filter_sum = 0;
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);
}``````

## First order lag filtering

``````/*
A、 name ： First order lag filtering
B、 Method ：
take a=0-1, The result of this filtering is =(1-a)* This sample value +a* Last filter result .
It has a good inhibiting effect on periodic interference ;
It is suitable for the occasion with high fluctuation frequency .
D、 shortcoming ：
Phase lag , Low sensitivity ;
The degree of lag depends on a Value size ;
Can not eliminate the filter frequency higher than the sampling frequency 1/2 The interference signal of .
E、 Arrangement ：shenhaiyu 2013-11-01
*/

int Filter_Value;
int Value;

void setup() {
Serial.begin(9600);       //  Initialize serial communication
Value = 300;
}

void loop() {
Filter_Value = Filter();       //  Get the filter output value
Serial.println(Filter_Value); //  Serial output
delay(50);
}

//  Used to randomly generate a 300 The current value around
return random(295, 305);
}

//  First order lag filtering
#define FILTER_A 0.01
int Filter() {
int NewValue;
Value = (int)((float)NewValue * FILTER_A + (1.0 - FILTER_A) * (float)Value);
return Value;
}``````

## Weighted recursive average filtering method

``````/*
A、 name ： Weighted recursive average filtering method
B、 Method ：
It is an improvement of the recursive average filtering method , That is, the data at different times are given different weights ;
Usually , The closer we get to the current data , The more power you have .
The larger the weight coefficient given to the new sample value , The higher the sensitivity , But the less smooth the signal is .
It is suitable for the object with large delay time constant , And systems with shorter sampling periods .
D、 shortcoming ：
For pure delay, the time constant is small 、 The sampling period is longer 、 Signals of slow change ;
Can't quickly respond to the severity of the system's current interference , The filtering effect is poor .
E、 Arrangement ：shenhaiyu 2013-11-01
*/

int Filter_Value;

void setup() {
Serial.begin(9600);       //  Initialize serial communication
}

void loop() {
Filter_Value = Filter();       //  Get the filter output value
Serial.println(Filter_Value); //  Serial output
delay(50);
}

//  Used to randomly generate a 300 The current value around
return random(295, 305);
}

//  Weighted recursive average filtering method
#define FILTER_N 12
int coe[FILTER_N] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};    //  Table of weighting coefficients
int sum_coe = 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12; //  Weighted coefficients and
int filter_buf[FILTER_N + 1];
int Filter() {
int i;
int filter_sum = 0;
for(i = 0; i < FILTER_N; i++) {
filter_buf[i] = filter_buf[i + 1]; //  All data moves left , The low post is still falling
filter_sum += filter_buf[i] * coe[i];
}
filter_sum /= sum_coe;
return filter_sum;
}``````

## Dithering filtering method

``````/*
A、 name ： Dithering filtering method
B、 Method ：
Set up a filter counter , Compare each sample value with the current valid value ：
If the sample value = Currently valid value , Then the counter is cleared ;
If the sample value <> Currently valid value , Then the counter +1, And determine whether the counter is >= ceiling N（ overflow ）;
If the counter overflows , The current value will be replaced by the current value , And clear the counter .
It has a good filtering effect for the measured parameters which change slowly ;
It can avoid the repeated opening of the controller near the critical value / Turn off the beat or the value jitter on the display .
D、 shortcoming ：
It is not suitable for fast changing parameters ;
If the value sampled at the time the counter overflows is exactly the interference value , The interference value will be imported into the system as a valid value .
E、 Arrangement ：shenhaiyu 2013-11-01
*/

int Filter_Value;
int Value;

void setup() {
Serial.begin(9600);       //  Initialize serial communication
Value = 300;
}

void loop() {
Filter_Value = Filter();       //  Get the filter output value
Serial.println(Filter_Value); //  Serial output
delay(50);
}

//  Used to randomly generate a 300 The current value around
return random(295, 305);
}

//  Dithering filtering method
#define FILTER_N 12
int i = 0;
int Filter() {
int new_value;
if(Value != new_value) {
i++;
if(i > FILTER_N) {
i = 0;
Value = new_value;
}
}
else
i = 0;
return Value;
}``````

## The method of limiting amplitude and eliminating dithering

``````/*
A、 name ： The method of limiting amplitude and eliminating dithering
B、 Method ：
amount to “ Limiting filtering ”+“ Dithering filtering method ”;
First limit the amplitude , After the shaking .
Inherited “ Limiting ” and “ Desquamation ” The advantages of ;
Improved “ Dithering filtering method ” Some of the flaws in , Avoid introducing interference values into the system .
D、 shortcoming ：
It is not suitable for fast changing parameters .
E、 Arrangement ：shenhaiyu 2013-11-01
*/

int Filter_Value;
int Value;

void setup() {
Serial.begin(9600);       //  Initialize serial communication
Value = 300;
}

void loop() {
Filter_Value = Filter();       //  Get the filter output value
Serial.println(Filter_Value); //  Serial output
delay(50);
}

//  Used to randomly generate a 300 The current value around
return random(295, 305);
}

//  The method of limiting amplitude and eliminating dithering
#define FILTER_A 1
#define FILTER_N 5
int i = 0;
int Filter() {
int NewValue;
int new_value;
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;
}``````

## Kalman filtering （ Non extended Kalman ）

``````#include <Wire.h> // I2C library, gyroscope

#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() {
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) {

//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 temp, x, y, z;

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(val);        // send value to write
Wire.endTransmission(); //end transmission
}

//reads num bytes starting from address register on ACC in to buff array
Wire.beginTransmission(DEVICE); //start transmission to ACC
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)
{