STM32算法_代码007

1.通过 编码器对返回的错误 速度进行滤波

#define MOTOR_BUFF_CIRCLE_SIZE 4
#define STATIC_ENCODER_VALUE   6

int32_t LMotor_Encoder_buff[MOTOR_BUFF_CIRCLE_SIZE] = {0};
uint8_t LEindex = 0;
int32_t LMotor_Encoder_last = 0;
int32_t L_Encoder_change = 0;

int32_t RMotor_Encoder_buff[MOTOR_BUFF_CIRCLE_SIZE] = {0};
uint8_t REindex = 0;
int32_t RMotor_Encoder_last = 0;
int32_t R_Encoder_change = 0;

uint8_t Robot_static_flag = 0;
uint8_t Robot_dynamic_flag = 0;

int32_t Encoder_buff_change_value(int32_t *buff,uint8_t size)
{ 
	int i = 0;
	int32_t value = 0;
	
	for(i = 0; i < size;i++)
	{
		value += buff[i]; 
	}
  return value;
}

u8 Speed_Filter_send(int32_t L_speed,int32_t R_speed)
{	 
	CAN1Sedbuf[0]=L_speed;
	CAN1Sedbuf[1]=L_speed&gt;&gt;8;
	CAN1Sedbuf[2]=L_speed&gt;&gt;16;
	CAN1Sedbuf[3]=L_speed&gt;&gt;24;
	CAN1Sedbuf[4]=R_speed;
	CAN1Sedbuf[5]=R_speed&gt;&gt;8;
	CAN1Sedbuf[6]=R_speed&gt;>16;
	CAN1Sedbuf[7]=R_speed>>24;	
	CAN1_Send(0x183,8);	 
	Delay_Us(200);
}

void Updata_Motor_Speed(void)
{		
//	if((Motor_Vel_receive[0] < 10) &amp;&amp; (Motor_Vel_receive[0] > -10))
//	{
//	  Motor_Vel_receive[0] = 0;
//	}

//	if((Motor_Vel_receive[1] < 10) &amp;&amp; (Motor_Vel_receive[1] > -10))
//	{
//	  Motor_Vel_receive[1] = 0;
//	}	
	
	LEindex = LEindex % MOTOR_BUFF_CIRCLE_SIZE;
	LMotor_Encoder_buff[LEindex] = Motor_Encoder_receive[0] - LMotor_Encoder_last;
	LEindex++;
	LMotor_Encoder_last = Motor_Encoder_receive[0];
	L_Encoder_change = Encoder_buff_change_value(LMotor_Encoder_buff,MOTOR_BUFF_CIRCLE_SIZE);
	
	REindex = REindex % MOTOR_BUFF_CIRCLE_SIZE;
	RMotor_Encoder_buff[REindex] = Motor_Encoder_receive[1] - RMotor_Encoder_last;
	REindex++;
	RMotor_Encoder_last = Motor_Encoder_receive[1];
	R_Encoder_change = Encoder_buff_change_value(RMotor_Encoder_buff,MOTOR_BUFF_CIRCLE_SIZE);	

	if( (abs(L_Encoder_change) < STATIC_ENCODER_VALUE) &amp;&amp; (abs(R_Encoder_change) < STATIC_ENCODER_VALUE) )
	{
	  //static
		Robot_static_flag = 1;
		Robot_dynamic_flag = 0;
	}
	else
	{
	 //dynamic
	 Robot_static_flag = 0;
	 Robot_dynamic_flag = 1;
	}
	
	if( (Robot_static_flag == 1) &amp;&amp; (Robot_dynamic_flag == 0) )
	{
		Motor_Speed[0] = 0;
		Motor_Speed[1] = 0;
		Motor_Odom = 0;
		Motor_gyro_z = 0;		
		Speed_Filter_send(0,0);
	}
	else
	{
		Motor_Speed[0] = rpm2vel(Motor_Vel_receive[0]);
		Motor_Speed[1] = -rpm2vel(Motor_Vel_receive[1]);

		Motor_Odom = (Motor_Speed[0] + Motor_Speed[1])/2.0f;
		Motor_gyro_z = ((Motor_Speed[1] - Motor_Speed[0])/WHRRL_L);		
		Speed_Filter_send(Motor_Vel_receive[0],Motor_Vel_receive[1]);
	}
	Motor_L_Encoder = Encoder2Distance(Motor_Encoder_receive[0]);
	Motor_R_Encoder = -Encoder2Distance(Motor_Encoder_receive[1]);
}

2.中位值平均 滤波

/***note
input:Speed_input_value;
outout:Speed_output_value;**/

#define FILTER_BUFFER_SIZE 4
uint8 speed_filter[FILTER_BUFFER_SIZE] ={0};

void TMSpeed_filter(void) 
{
    static unit8 ad_save_location=0;

    speed_filter[ad_save_location]  = Speed_input_value;    /*sample value get input value*/  
    if (ad_save_location > (FILTER_BUFFER_SIZE-1)) 
    { 
        ad_save_location = 0;
        TM_filter();                            //中位值平均滤波
    }
    else
    {
        ad_save_location++;
    }
}

/*************************************************************************** 
 * Function:    void compositor(u8 channel)
 * Description: 中位值平均滤波
 * Parameters:  None                
 * Returns:       
 * Author:      Teana
 **************************************************************************/
void compositor(void)
{
    unit8 exchange;
    unit8 i,j;
    u16 change_value;
    
    for (j=FILTER_BUFFER_SIZE;j>1;j--)
    {
        for (i=0;i<j-1;i++)
        {
            exchange = 0;
            if (speed_filter[i]>speed_filter[i+1])
            {
                change_value = speed_filter[i];
                speed_filter[i] = speed_filter[i+1];
                speed_filter[i+1] = change_value;
                exchange = 1;
            }
        }
        if (exchange == 0)
            return;
    }
}

void TM_filter(void)
{
    unit8 index;
    unit8 count;
    u16 sum_data = 0;
    
        compositor(); //filter up-down
        for (count=1;count<FILTER_BUFFER_SIZE-1;count++)
        {
            sum_data +=speed_filter[count];
        }
        Speed_output_value= sum_data / (FILTER_BUFFER_SIZE - 2);
        sum_data = 0;

}

3.PID算法

pid.h

// protection against multiple includes
#ifndef SAXBOPHONE_PID_H
#define SAXBOPHONE_PID_H

#ifdef __cplusplus
extern "C"{
#endif

    typedef struct pid_calibration {
        /*
         * struct PID_Calibration
         * 
         * Struct storing calibrated PID constants for a PID Controller
         * These are used for tuning the algorithm and the values they take are
         * dependent upon the application - (in other words, YMMV...)
         */
        double kp; // Proportional gain
        double ki; // Integral gain
        double kd; // Derivative gain
    } PID_Calibration;


    typedef struct pid_state {
        /*
         * struct PID_State
         * 
         * Struct storing the current state of a PID Controller.
         * This is used as the input value to the PID algorithm function, which also
         * returns a PID_State struct reflecting the adjustments suggested by the algorithm.
         * 
         * NOTE: The output field in this struct is set by the PID algorithm function, and
         * is ignored in the actual calculations.
         */
        double actual; // The actual reading as measured
        double target; // The desired reading
        double time_delta; // Time since last sample/calculation - should be set when updating state
        // The previously calculated error between actual and target (zero initially)
        double previous_error;
        double integral; // Sum of integral error over time
        double output; // the modified output value calculated by the algorithm, to compensate for error
    } PID_State;


    /*
     * PID Controller Algorithm implementation
     * 
     * Given a PID calibration for the P, I and D values and a PID_State for the current
     * state of the PID controller, calculate the new state for the PID Controller and set
     * the output state to compensate for any error as defined by the algorithm
     */
    PID_State pid_iterate(PID_Calibration calibration, PID_State state);


#ifdef __cplusplus
} // extern "C"
#endif

// end of header
#endif

pid.c

#include "pid.h"

PID_State pid_iterate(PID_Calibration calibration, PID_State state) {
    // calculate difference between desired and actual values (the error)
    double error = state.target - state.actual;
    // calculate and update integral
    state.integral += (error * state.time_delta);
    // calculate derivative
    double derivative = (error - state.previous_error) / state.time_delta;
    // calculate output value according to algorithm
    state.output = (
        (calibration.kp * error) + (calibration.ki * state.integral) + (calibration.kd * derivative)
    );
    // update state.previous_error to the error value calculated on this iteration
    state.previous_error = error;
    // return the state struct reflecting the calculations
    return state;
}

test.c

#include "pid.h"


// initialise blank PID_Calibration struct and blank PID_State struct
PID_Calibration calibration;
PID_State state;


void setup() {
    // configure the calibration and state structs
    // dummy gain values
    calibration.kp = 1.0;
    calibration.ki = 1.0;
    calibration.kd = 1.0;
    // an initial blank starting state
    state.actual = 0.0;
    state.target = 0.0;
    state.time_delta = 1.0; // assume an arbitrary time interval of 1.0
    state.previous_error = 0.0;
    state.integral = 0.0;
    // start the serial line at 9600 baud!
    Serial.begin(9600);
}


void loop() {
    // read in two bytes from serial, assume first is the target value and
    // second is the actual value. Output calculated result on serial
    if (Serial.available() >= 2) {
        // retrieve one byte as target value, cast to double and store in state struct
        state.target = (double) Serial.read();
        // same as above for actual value
        state.actual = (double) Serial.read();
        // now do PID calculation and assign output back to state
        state = pid_iterate(calibration, state);
        // print results back on serial
        Serial.print("Target:t");
        Serial.println(state.target);
        Serial.print("Actual:t");
        Serial.println(state.actual);
        Serial.print("Output:t");
        Serial.println(state.output);
    }
}

4.STM32 滤波 算法

#include "led.h"
#include "delay.h"
#include "sys.h"
#include "usart.h"
#include "lcd.h"
#include "adc.h"
#include "usartbo.h"

u16 ftable[255] = {
	2048, 2098, 2148, 2198, 2248, 2298, 2348, 2398, 2447, 2496,
 2545, 2594, 2642, 2690, 2737, 2785, 2831, 2877, 2923, 2968,
 3013, 3057, 3100, 3143, 3185, 3227, 3267, 3307, 3347, 3385,
 3423, 3460, 3496, 3531, 3565, 3598, 3631, 3662, 3692, 3722,
 3750, 3778, 3804, 3829, 3854, 3877, 3899, 3920, 3940, 3958,
 3976, 3992, 4007, 4021, 4034, 4046, 4056, 4065, 4073, 4080,
 4086, 4090, 4093, 4095, 4095, 4095, 4093, 4090, 4086, 4080,
 4073, 4065, 4056, 4046, 4034, 4021, 4007, 3992, 3976, 3958,
 3940, 3920, 3899, 3877, 3854, 3829, 3804, 3778, 3750, 3722,
 3692, 3662, 3631, 3598, 3565, 3531, 3496, 3460, 3423, 3385,
 3347, 3307, 3267, 3227, 3185, 3143, 3100, 3057, 3013, 2968,
 2923, 2877, 2831, 2785, 2737, 2690, 2642, 2594, 2545, 2496,
 2447, 2398, 2348, 2298, 2248, 2198, 2148, 2098, 2047, 1997,
 1947, 1897, 1847, 1797, 1747, 1697, 1648, 1599, 1550, 1501,
 1453, 1405, 1358, 1310, 1264, 1218, 1172, 1127, 1082, 1038,
 995, 952, 910, 868, 828, 788, 748, 710, 672, 635,
 599, 564, 530, 497, 464, 433, 403, 373, 345, 317,
 291, 266, 241, 218, 196, 175, 155, 137, 119, 103,
 88, 74, 61, 49, 39, 30, 22, 15, 9, 5,
 2, 0, 0, 0, 2, 5, 9, 15, 22, 30,
 39, 49, 61, 74, 88, 103, 119, 137, 155, 175,
 196, 218, 241, 266, 291, 317, 345, 373, 403, 433,
 464, 497, 530, 564, 599, 635, 672, 710, 748, 788,
 828, 868, 910, 952, 995, 1038, 1082, 1127, 1172, 1218,
 1264, 1310, 1358, 1405, 1453, 1501, 1550, 1599, 1648, 1697,
 1747, 1797, 1847, 1897, 1947
};

int a=0;
int b=1;
/*//
方法一：限幅滤波法
方法：根据经验判断，确定两次采样允许的最大偏差值（设为A），每次检测到新值时判断：
      如果本次值与上次值之差<=A，则本次值有效，
      如果本次值与上次值之差>A，则本次值无效，放弃本次值，用上次值代替本次值。
优点：能克服偶然因素引起的脉冲干扰
缺点：无法抑制周期性的干扰，平滑度差
//*/

#define  A 51
u16 Value1;

u16 filter1() 
{
  u16 NewValue;
	Value1 = ftable[b-1];
  NewValue = ftable[b];
	b++;
	a++;
	if(a==255) a=0;
	if(b==255) b=1;
  if(((NewValue - Value1) > A) || ((Value1 - NewValue) > A))
	{
		print_host(ftable[a],NewValue);
    return NewValue;
	}
  else
	{
		 print_host(ftable[a],Value1);
     return Value1;
	}
}

/*//
方法二：中位值滤波法
方法： 连续采样N次（N取奇数），把N次采样值按大小排列，取中间值为本次有效值。
优点：克服偶然因素（对温度、液位的变化缓慢的被测参数有良好的滤波效果）
缺点：对流量、速度等快速变化的参数不宜
//*/
//#define N 3

//u16 value_buf[N]; 
//u16 filter2()
//{  
//  u16 count,i,j,temp;
//  for(count=0;count<N;count++)
//  {
//    value_buf[count] =  ftable[a];
//	  a++;
//	  if(a==255) a=0;
//  }
//	for (j=0;j<N-1;j++)
//	{
//		 for (i=0;i<N-j;i++)
//		 {
//			if ( value_buf[i] >  value_buf[i+1] )
//			{
//			 temp = value_buf[i];
//			 value_buf [i]= value_buf[i+1]; 
//			 value_buf[i+1] = temp;
//			}
//		 }
//	}
	printf("%dn",value_buf[(N-1)/2]);
//	return value_buf[(N-1)/2];
//}
//void pros2()
//{
//   print_host(4,filter2());
//}
/*//
方法三：算术平均滤波法
方法：连续取N个采样值进行算术平均运算：（ N值的选取：一般流量，N=12；压力：N=4。）
      N值较大时：信号平滑度较高，但灵敏度较低；
      N值较小时：信号平滑度较低，但灵敏度较高；     
优点：适用于对一般具有随机干扰的信号进行滤波；这种信号的特点是有一个平均值，信号在某一数值范围附近上下波动
缺点：对于测量速度较慢或要求数据计算速度较快的实时控制不适用，比较浪费RAM。
//*/

//#define N 5
//u16 filter3()
//{
//	u16 sum = 0,count;
//	for ( count=0;count<N;count++)
//	{
//		sum = sum+ ftable[a];
//		a++;
//		if(a==255) a=0;
//	}
//	print_host(4,sum/N);
	printf("%dn",sum/N);
//	return (sum/N);
//}

/*//
方法四：递推平均滤波法（又称滑动平均滤波法）
方法： 把连续取得的N个采样值看成一个队列，队列的长度固定为N，
       每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据（先进先出原则），
       把队列中的N个数据进行算术平均运算，获得新的滤波结果。
       N值的选取：流量，N=12；压力，N=4；液面，N=4-12；温度，N=1-4。
优点：对周期性干扰有良好的抑制作用，平滑度高；
      适用于高频振荡的系统。
缺点：灵敏度低，对偶然出现的脉冲性干扰的抑制作用较差；
      不易消除由于脉冲干扰所引起的采样值偏差；
      不适用于脉冲干扰比较严重的场合；
      比较浪费RAM。
//*/

//#define FILTER4_N 3
//u16 filter_buf[FILTER4_N + 1];
//u16 filter4() 
//{
//  int i;
//  int filter_sum = 0;
//  filter_buf[FILTER4_N] = ftable[a];		
//	a++;
//	if(a==255) a=0;
//  for(i = 0; i < FILTER4_N; i++) 
//	{
//    filter_buf[i] = filter_buf[i + 1]; // 所有数据左移，低位仍掉
//    filter_sum += filter_buf[i];
//  }
	printf("%dn",filter_sum / FILTER4_N);
//  return (int)(filter_sum / FILTER4_N);
//}


//void pros4(void)
//{
//	u16 i=0;
//  print_host(4,filter4());
//}
/*//
方法五：中位值平均滤波法（又称防脉冲干扰平均滤波法）
方法： 采一组队列去掉最大值和最小值后取平均值，     （N值的选取：3-14）。 
      相当于“中位值滤波法”+“算术平均滤波法”。
      连续采样N个数据，去掉一个最大值和一个最小值，
      然后计算N-2个数据的算术平均值。    
优点： 融合了“中位值滤波法”+“算术平均滤波法”两种滤波法的优点。
       对于偶然出现的脉冲性干扰，可消除由其所引起的采样值偏差。
       对周期干扰有良好的抑制作用。
       平滑度高，适于高频振荡的系统。
缺点：对于测量速度较慢或要求数据计算速度较快的实时控制不适用，比较浪费RAM。
//*/

//#define N 3
//int filter5() 
//{
//  int i, j;
//  int filter_temp, filter_sum = 0;
//  int filter_buf[N];
//  for(i = 0; i < N; i++) 
//	{
//    filter_buf[i] = ftable[a];
//		a++;
//		if(a==255)   a=0;
//    delay_us(10);
//  }
//  // 采样值从小到大排列（冒泡法）
//  for(j = 0; j < N - 1; j++) 
//	{
//    for(i = 0; i < 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 < N - 1; i++) filter_sum += filter_buf[i];
	printf("%dn",filter_sum / ( N - 2));
//  return filter_sum / (N - 2);
//}

//void pros5(void)
//{
//	u16 i=0;
//	for(i=0;i<255;i++)
//	{
//     print_host(ftable[i],filter5());
//	}
//}

/*//
方法六：限幅平均滤波法
方法： 相当于“限幅滤波法”+“递推平均滤波法”；
       每次采样到的新数据先进行限幅处理，
       再送入队列进行递推平均滤波处理。
优点： 融合了两种滤波法的优点；
      对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差。
缺点：比较浪费RAM。
//*/

//#define FILTER6_N 3
//#define FILTER6_A 51
//int filter_buf[FILTER6_N];

//int filter6() 
//{
//  int i;
//  int filter_sum = 0;
//  filter_buf[FILTER6_N - 1] = ftable[a];		
//	a++;
//	if(a==255)   a=0;
//  if(((filter_buf[FILTER6_N - 1] - filter_buf[FILTER6_N - 2]) > FILTER6_A) || ((filter_buf[FILTER6_N - 2] - filter_buf[FILTER6_N - 1]) > FILTER6_A))
//    filter_buf[FILTER6_N - 1] = filter_buf[FILTER6_N - 2];
//  for(i = 0; i < FILTER6_N - 1; i++) 
//	{
//    filter_buf[i] = filter_buf[i + 1];
//    filter_sum += filter_buf[i];
//  }
	printf("%dn",filter_sum / ( FILTER6_N - 1));
//  return filter_sum / (FILTER6_N - 1);
//}

//void pros6(void)
//{
//  print_host(4,filter6());
//}
/*//
方法七：一阶滞后滤波法
方法： 取a=0-1，本次滤波结果=(1-a)*本次采样值+a*上次滤波结果。
优点：  对周期性干扰具有良好的抑制作用；
        适用于波动频率较高的场合。
       平滑度高，适于高频振荡的系统。
缺点： 相位滞后，灵敏度低；
      滞后程度取决于a值大小；
      不能消除滤波频率高于采样频率1/2的干扰信号。
//*/

//#define FILTER7_A 0.01
//u16 Value;
//u16 filter7() 
//{
//  int NewValue;
//	Value = ftable[b-1];		
//  NewValue = ftable[b];		
//	b++;
//	if(b==255)   b=1;
//  Value = (int)((float)NewValue * FILTER7_A + (1.0 - FILTER7_A) * (float)Value);
	printf("%dn",Value);
//  return Value;
//}
//void pros7(void)
//{
//	u16 i=0;
//	for(i=0;i<255;i++)
//	{
//     print_host(ftable[i],filter7());
//	}
//}

/*//
方法八：加权递推平均滤波法
方法： 是对递推平均滤波法的改进，即不同时刻的数据加以不同的权；
       通常是，越接近现时刻的数据，权取得越大。
       给予新采样值的权系数越大，则灵敏度越高，但信号平滑度越低。
优点： 适用于有较大纯滞后时间常数的对象，和采样周期较短的系统。
缺点：  对于纯滞后时间常数较小、采样周期较长、变化缓慢的信号；
       不能迅速反应系统当前所受干扰的严重程度，滤波效果差。
//*/

//#define FILTER8_N 12
//int coe[FILTER8_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[FILTER8_N + 1];
//int filter8() 
//{
//  int i;
//  int filter_sum = 0;
//  filter_buf[FILTER8_N] = ftable[a];		
//	a++;
//	if(a==255)   a=0;
//  for(i = 0; i < FILTER8_N; i++) 
// {
//    filter_buf[i] = filter_buf[i + 1]; // 所有数据左移，低位仍掉
//    filter_sum += filter_buf[i] * coe[i];
//  }
//  filter_sum /= sum_coe;
	printf("%dn",filter_sum);
//  return filter_sum;
//}

//void pros8(void)
//{
//	u16 i=0;
//	for(i=0;i<255;i++)
//	{
//     print_host(ftable[i],filter8());
//	}
//}
/*//
方法九： 消抖滤波法
方法：  设置一个滤波计数器，将每次采样值与当前有效值比较：
       如果采样值=当前有效值，则计数器清零；
       如果采样值<>当前有效值，则计数器+1，并判断计数器是否>=上限N（溢出）；
       如果计数器溢出，则将本次值替换当前有效值，并清计数器。
优点：  对于变化缓慢的被测参数有较好的滤波效果；
        可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动。
缺点：  对于快速变化的参数不宜；
       如果在计数器溢出的那一次采样到的值恰好是干扰值,则会将干扰值当作有效值导入系统。
//*/

//#define FILTER9_N 51
//u16 i = 0;
//u16 Value;
//u16 filter9() 
//{
//  int new_value;
//	Value = ftable[b-1];
//  new_value = ftable[b];		
//	b++;
//	if(b==255)   b=1;
//  if(Value != new_value) 
//	{
//    i++;
//    if(i > FILTER9_N) 
//		{
//      i = 0;
//      Value = new_value;
//    }
//  }
//  else   i = 0;
//  return Value;
//}

//void pros9(void)
//{
//	u16 i=0;
//	for(i=0;i<255;i++)
//	{
//     print_host(ftable[i],filter9());
//	}
//}

/*//
方法十：限幅消抖滤波法
方法： 相当于“限幅滤波法”+“消抖滤波法”；
       先限幅，后消抖。
优点：  继承了“限幅”和“消抖”的优点；
        改进了“消抖滤波法”中的某些缺陷，避免将干扰值导入系统。
缺点：   对于快速变化的参数不宜。
//*/

//#define FILTER10_A 51
//#define FILTER10_N 5
//u16 i = 0;
//u16 Value;

//u16 filter10() 
//{
//  u16 NewValue;
//  u16 new_value;
//	Value = ftable[b-1];
//  NewValue = ftable[b];		
//	b++;
//	if(b==255)   b=1;
//  if(((NewValue - Value) > FILTER10_A) || ((Value - NewValue) > FILTER10_A))
//    new_value = Value;
//  else
//    new_value = NewValue;
//  if(Value != new_value) 
//	{
//    i++;
//    if(i > FILTER10_N) 
//		{
//      i = 0;
//      Value = new_value;
//    }
//  }
//  else   i = 0;
//  return Value;
//}

//void pros10(void)
//{
//	u16 i=0;
//	for(i=0;i<255;i++)
//	{
//     print_host(ftable[i],filter10());
//	}
//}

/*//
         主函数
//*/
int main(void)
{ 
	delay_init();	    	 //延时函数初始化	  
	uart_init(256000);	 	//串口初始化为9600
	LED_Init();		  		//初始化与LED连接的硬件接口
	while(1)
	{
		filter1();
//		filter2();
//		pros2();
//		filter3();
//	  filter4();
//		pros4();
//	  filter5();
//		pros5();
		
//		filter6();
//		pros6();
//		filter7();
//		pros7();
//		filter8();
//		pros8();
//		filter9();
//		pros9();
//		filter10();
//		pros10();
		delay_ms(20);
	}											    
}

5.互补滤波器对IMU解算的roll和pitch进行线性 叠加，给出更准确的估计。

Kalman Filter 更适合 9 轴的传感器，也就是在 6 轴的基础上（3-axis Accel + 3-axis Gyro）融合 3 轴的磁力计。对于一个只有 6 轴 IMU 的 MCU，轻量级的互补滤波器 (Complementary Filter) 更加合适，利用 3 轴陀螺仪和 3 轴加速度计来估计开发板的姿态 (Pitch, Roll, Yaw)。

#include <math.h>
#include <icm20608.h>
#include <stdio.h>

// Sample Frequency 100 Hz
const rt_int32_t TIME_STEP_MS = 10;

// For 250 deg/s range, check the datasheet
double gSensitivity = 131;

// For 2g range, check the datasheet
double aSensitivity = 16384;

// Raw data from the IMU
rt_int16_t accel_x, accel_y, accel_z;
rt_int16_t gyro_x, gyro_y, gyro_z;

// Predicted Orientation (Gyro)
double gx = 0, gy = 0, gz = 0;

// Predicted Orientation (Acc)
double ax = 0, ay = 0;

double accelX = 0, accelY = 0, accelZ = 0;
double gyrX = 0, gyrY = 0, gyrZ = 0;

int main(void)
{
    icm20608_device_t imu = icm20608_init("i2c3");
    if(imu != RT_NULL)
    {
        rt_kprintf("Initialized IMUn");
    }

    icm20608_calib_level(imu, 500);

    while (1)
    {
        rt_tick_t prevTime = rt_tick_get();

        if (icm20608_get_accel(imu, &accel_x, &accel_y, &accel_z) == RT_EOK)
        {
            if(icm20608_get_gyro(imu, &gyro_x, &gyro_y, &gyro_z) == RT_EOK)
            {
                accelX = accel_x / aSensitivity;
                accelY = accel_y / aSensitivity;
                accelZ = accel_z / aSensitivity;

                gyrX = gyro_x / gSensitivity;
                gyrY = gyro_y / gSensitivity;
                gyrZ = gyro_z / gSensitivity;

                // angles based on accelerometer
                ax = atan2(accelY, accelZ) * 180 / M_PI;                                      // roll
                ay = atan2(-accelX, sqrt( pow(accelY, 2) + pow(accelZ, 2))) * 180 / M_PI;    // pitch

                // This is incorrect, many tutorials make this mistake
                // ax = atan2(accelY, sqrt( pow(accelX, 2) + pow(accelZ, 2))) * 180 / M_PI;    // roll

                // angles based on gyro (deg/s)
                gx = gx + gyrX * TIME_STEP_MS / 1000;
                gy = gy + gyrY * TIME_STEP_MS / 1000;
                gz = gz + gyrZ * TIME_STEP_MS / 1000;

                // complementary filter
                gx = gx * 0.96 + ax * 0.04;
                gy = gy * 0.96 + ay * 0.04;

                printf("%d %d %d %d %d %d %.4f %.4f %.4f n", accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, gx, gy, gz);
            }
        };

        while( (rt_tick_get() - prevTime) < rt_tick_from_millisecond(TIME_STEP_MS));
    }
}