GD32F103单片机卡尔曼滤波实现及效果

卡尔曼滤波在数据去噪，获得真实值的运用中广泛使用，在采集一些信号时，很实用。

测试方法：通过将DAC输出与ADC连接，，DAC输出一固定值
通过ADC采集数据，由于ADC精度受限以及一些干扰，ADC采集到
的数据在固定值附近波动，通过卡尔曼滤波估计一下ADC采集的真实值。
这个与之前的PID测试有点区别，PID是设定目标，自动调节DAC，使
ADC采集数据接近目标。

1、卡尔曼滤波结构体：type_KalmanFilter

//卡尔曼滤波结构体
typedef struct KalmanFilter 
{
	float x_mea;	 // 测量值
	float x_est;	 // 估计值
	float e_mea;	 // 测量偏差
	float e_est;	 // 估计偏差
	float Kk; 		 // Karlman 增益
}type_KalmanFilter;

2、初始化，为结构体成员赋值

//Kalman初始化
void Kalman_Init(type_KalmanFilter* kalmanFilter, float FirstMeaValue, float E_mea, float FirstEstValue, float E_est) 
{
	kalmanFilter->x_est = FirstEstValue;
	kalmanFilter->x_mea = FirstMeaValue;
	kalmanFilter->e_est = E_est;
	kalmanFilter->e_mea = E_mea;
	kalmanFilter->Kk = Kk_calc(kalmanFilter->e_est, kalmanFilter->e_mea);
}

3、把新采集到的数据作为输入，更新

//Kalman更新
void Kalman_Update(type_KalmanFilter* kalmanFilter, float newMeaValue) 
{
	float temp = kalmanFilter->e_est;
	kalmanFilter->x_est = kalmanFilter->x_est + kalmanFilter->Kk * (newMeaValue - kalmanFilter->x_est);
	kalmanFilter->x_mea = newMeaValue;
	kalmanFilter->Kk = Kk_calc(kalmanFilter->e_est, kalmanFilter->e_mea);
	kalmanFilter->e_est = (1 - kalmanFilter->Kk) * temp;
}

4、端口设置：包括ADC接口和DAC接口

void ad_da_init(void)
{
	//时钟配置
	rcu_periph_clock_enable(RCU_GPIOA);
	rcu_periph_clock_enable(RCU_AF);
	rcu_periph_clock_enable(RCU_ADC0);
	rcu_periph_clock_enable(RCU_DAC);
	
	rcu_adc_clock_config(RCU_CKADC_CKAPB2_DIV12);
	
	//接口配置
	gpio_init(GPIOA,GPIO_MODE_AIN,GPIO_OSPEED_50MHZ,GPIO_PIN_4);
	gpio_init(GPIOA,GPIO_MODE_AIN,GPIO_OSPEED_50MHZ,GPIO_PIN_6);
	
	//ADC输入配置
	adc_special_function_config(ADC0,ADC_CONTINUOUS_MODE,ENABLE);
	adc_data_alignment_config(ADC0,ADC_DATAALIGN_RIGHT);
	adc_channel_length_config(ADC0,ADC_REGULAR_CHANNEL,1);
	adc_regular_channel_config(ADC0,0,ADC_CHANNEL_6,ADC_SAMPLETIME_55POINT5);
	adc_external_trigger_config(ADC0,ADC_REGULAR_CHANNEL,DISABLE);
	
	adc_enable(ADC0);
	delay_ms(1);
	adc_calibration_enable(ADC0);
	
	//DAC输出配置
	dac_trigger_source_config(DAC0,DAC_TRIGGER_SOFTWARE);
	dac_trigger_enable(DAC0);
	dac_wave_mode_config(DAC0,DAC_WAVE_DISABLE);
	dac_output_buffer_enable(DAC0);
	
	dac_enable(DAC0);
	dac_data_set(DAC0, DAC_ALIGN_12B_R, (u16)1024);
    dac_software_trigger_enable(DAC0);
	
	//Kalman初始化
	Kalman_Init(&k, 1024, 2, 1024, 2);
}

5、滤波处理

void dsp_test(void)
{
	u16 adc_get=0;
	
	adc_software_trigger_enable(ADC0,ADC_REGULAR_CHANNEL);
	if(adc_flag_get(ADC0,ADC_FLAG_EOC))
	{
		adc_flag_clear(ADC0,ADC_FLAG_EOC);
		adc_get = adc_regular_data_read(ADC0);
		Kalman_Update(&k,adc_get);
	}
	
	printf("%d	%f	%f\r\n",adc_get,k.x_mea,k.x_est);
}

6、主函数

#include "bitband.h"
#include "led.h"
#include "systick.h"
#include "print.h"
#include "dsp_test.h"

u16 mem_infor[2]={0};
u32 uid[3]={0};


int main(void)
{
    systick_set(96);
	led_init();
	
	print_config(9600);
	
	get_mem_infor(&mem_infor[0],&mem_infor[1]);
	printf("GD32F103VKT6 Flash Size=%dKB,Sram Size=%dKB...\r\n",mem_infor[1],mem_infor[0]);
	get_uid(uid);
	printf("GD32F103VKT6 UID=%d%d%d...\r\n",uid[0],uid[1],uid[2]);
	
	ad_da_init();
	
    while(1)
	{
		dsp_test();
		delay_ms(100);
    }
}

将数据打印输出，导入MATLAB，结果如图所示：