卡尔曼滤波及效果

只看该作者 · 2024-10-15 12:00

卡尔曼滤波结构体：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);
}
}