GD32F103卡尔曼滤波实现及效果

[复制链接]
1413|9
手机看帖
扫描二维码
随时随地手机跟帖
zeshoufx|  楼主 | 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);
    }
}
演示视频:
GD32F103卡尔曼滤波实现
【GD32F103卡尔曼滤波测试】 https://www.bilibili.com/video/BV1bR4y1o7TQ/?share_source=copy_web&vd_source=7f0cb9c0f09b768583faf157910eb515
GD32F103卡尔曼滤波测试效果
【GD32F103卡尔曼滤波测试效果】 https://www.bilibili.com/video/BV1u14y1n7KD/?share_source=copy_web&vd_source=7f0cb9c0f09b768583faf157910eb515
7、将数据打印输出,导入MATLAB,结果如图所示:
可以看到整体上实际的采集值在波动,而卡尔曼滤波估计的真实值却基本很平稳

结果1

结果1

结果2

结果2

使用特权

评论回复
呐咯密密| | 2022-11-26 16:58 | 显示全部楼层
这个卡尔曼滤波执行一次时间多久,会不会影响系统的时效性

使用特权

评论回复
zeshoufx|  楼主 | 2022-11-26 20:00 | 显示全部楼层
呐咯密密 发表于 2022-11-26 16:58
这个卡尔曼滤波执行一次时间多久,会不会影响系统的时效性

还没具体测试过,,,我觉得跟多采集几次然后取平均这种暴力滤波方式差不多,,明天测试后补充,,,

使用特权

评论回复
OKAKAKO| | 2022-12-2 17:08 | 显示全部楼层
zeshoufx 发表于 2022-11-26 20:00
还没具体测试过,,,我觉得跟多采集几次然后取平均这种暴力滤波方式差不多,,明天测试后补充 ...

估计跟直接取平均还是有区别的,大概误差应该在0.2左右

使用特权

评论回复
zeshoufx|  楼主 | 2022-12-2 20:08 | 显示全部楼层
OKAKAKO 发表于 2022-12-2 17:08
估计跟直接取平均还是有区别的,大概误差应该在0.2左右

0.2秒吗

使用特权

评论回复
Jacquetry| | 2022-12-21 17:35 | 显示全部楼层
这个应该会有一点误差的

使用特权

评论回复
卡尔曼滤波确实通过MATLAB分析后的结果很稳定,果然滤波能消除干扰

使用特权

评论回复
jf101| | 2022-12-24 13:26 | 显示全部楼层
zeshoufx 发表于 2022-11-26 20:00
还没具体测试过,,,我觉得跟多采集几次然后取平均这种暴力滤波方式差不多,,明天测试后补充 ...

楼主,平均算法滤波测试了嘛?结果跟卡尔曼滤波差距怎么样?

使用特权

评论回复
dspmana| | 2023-1-9 17:42 | 显示全部楼层
这个卡尔曼可以做数据融合的吗?              

使用特权

评论回复
pentruman| | 2023-1-9 17:51 | 显示全部楼层
这个的计算量应该不会很小了。              

使用特权

评论回复
发新帖 我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则

66

主题

1945

帖子

14

粉丝