GD32F103卡尔曼滤波实现及效果

只看该作者 · 2022-11-25 20:53

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

测试方法：通过将DAC输出与ADC连接，，DAC输出一固定值
通过ADC采集数据，由于ADC精度受限以及一些干扰，ADC采集到
的数据在固定值附近波动，通过卡尔曼滤波估计一下ADC采集的真实值。

这个与之前的PID测试有点区别，PID是设定目标，自动调节DAC，使
ADC采集数据接近目标。

PID演示连接：

专栏：ARM DSP库 PID控制测试 - 哔哩哔哩 (bilibili.com)

视频：【ARM单片机PID控制测试——基于DSP库】 https://www.bilibili.com/video/BV1sd4y1w755/?share_source=copy_web&vd_source=7f0cb9c0f09b768583faf157910eb515

视频

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、端口设置

复制

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);

    }

}