РадиоЛоцман - Все об электронике

Применение фильтра Калмана на примере STM32F103C8

STMicroelectronics STM32F103C8

Захаров Денис, Украина

При измерении аналоговых величин, возникает погрешность, которая в той или иной мере оказывает негативное влияние на результат измерения. Избавиться от воздействия шумов можно с помощью усреднения результатов. Такой метод требует большого количества измерений и подходит для относительно инерционных процессов. Одним из простых методов подавления шумов, позволяющих свести к минимуму потери полезного сигнала, является применение фильтра Калмана. Этот метод носит имя американского математика Рудольфа Калмана и способен давать значение, максимально приближенное к реальному. Такие результаты достигаются благодаря тому, что система может учитывать управляющее воздействие на систему, если известны свойства этого воздействия.

Для исследований возьмем микроконтроллер (МК) STM32F103C8 (Рисунок 1). Программу будем писать на языке C++ в среде Keil и STM32CubeMX.

Применение фильтра Калмана на примере STM32F103C8
Рисунок 1.

С помощью STM32CubeMX проведем инициализацию микроконтроллера и настроим работу с UART (Рисунок 2).

Применение фильтра Калмана на примере STM32F103C8
Рисунок 2.

Смоделируем синусоидальный сигнал, и после его зашумления будем исследовать работу фильтра. В сети Интернет, я нашел бесплатную программу SerialPortPlotter, с помощью которой можно строить графики по данным, принятым с МК.

Построение чистой синусоиды

В качестве примера будем использовать 8-битные числа с диапазоном 0-255. Точкой пересечения полуволн будет значение 127, длина волны 255 единиц. Для наших целей этих параметров будет достаточно.

Формула синусоиды выглядит следующим образом:

Y= a+b×Sin(c×X),

где

а – характеризует сдвиг по оси Y (а=127),
b – характеризует растяжение по оси Y (b=127),
c – характеризует растяжение по оси X.

Поскольку длина периода равняется 2π, и он состоит из 255 шагов, очевидно, что

с= 2π(1/255).

Следовательно,

Y=127+127×Sin(π×X×0.007843).

Выполним математические операции и отправим полученные значения (Листинг 1) в SerialPortPlotter на скорости 115200 бод.

Листинг 1.

for (int i=0;i<256;i++)
{
int out_sin = (127+(127*sin(3.14*i*0.007843)));
char char_sin[4];
a2str(out_sin, char_sin);

HAL_UART_Transmit(&huart1,(uint8_t *) str_$, strlen (str_$), 100);  // Заголовок в виде значка "$"
HAL_UART_Transmit(&huart1,(uint8_t *) char_sin, strlen (char_sin), 100); // Данные
HAL_UART_Transmit(&huart1,(uint8_t *) str_t, strlen (str_t), 100);  // Конец сообщения
}

На Рисунке 3 видим полученный результат.

Применение фильтра Калмана на примере STM32F103C8
Рисунок 3.

Построение искаженной синусоиды

Возьмем полученные данные с чистой синусоиды и исказим результат (Листинг 2).

Листинг 2.

unsigned char sin_tab_shum [256] = {
127,130,133,136,139,142,145,148,151,154,
180,222,240,235,230,172,175,178,181,
184,187,189,192,195,197,200,202,205,207,210,212,214,217,219,221,223,225,227,
229,
200,222,234,236,237,239,240,242,243,244,245,180,155,100,120,155,180,222,
252,252,253,253,253,253,253,254,253,253,253,253,252,252,251,251,250,249,249,
248,247,246,245,243,242,241,239,238,236,235,233,231,230,228,226,224,222,220,
218,215,213,211,209,206,204,201,199,196,193,191,188,185,182,180,177,174,171,
168,165,162,159,156,153,150,147,144,141,137,134,131,128,125,122,119,116,112,
109,106,103,100,97,94,91,88,85,82,7 ,76,73,71,68,65,62,60,57,54,52,49,47,
80,
90,100,60
,35,33,31,29,27,25,23,22,20,18,17,15,14,12,11,10,8,7,6,5,4,4,3,2,2,
1,1,0,0,0,0,0,0,0,0,0,0,1,1,2,2,3,4,5,6,7,8,9,10,11,13,14,16,17,19,21,22,24,
26,28,30,32,34,36,39,41,43,46,48,5 ,53,56,58,61,64,66,69,72,75,78,81,84,87,
89,93,96,99,102,105,108,111,114,117,120,123,127};

Красным выделены принудительно измененные значения. Как видим, на графике должно появиться несколько резких скачков. Перепишем программу и посмотрим результат (Листинг 3).

Листинг 3.

for (int i=0;i<256;i++)
{
char str[10];
a2str(sin_tab_shum[i], str); //char* a2str(uint_fast16_t d, char* out)
strcat(str, str_ln);

HAL_UART_Transmit(&huart1,(uint8_t *) str_$, strlen (str_$), 100);
HAL_UART_Transmit(&huart1,(uint8_t *) str, strlen (str), 100);
HAL_UART_Transmit(&huart1,(uint8_t *) str_t, strlen (str_t), 100);
}

Загрузим программу в МК и отследим изменения на графике( Рисунок 4). Результат оправдал ожидания – на графике появились явные скачки.

Применение фильтра Калмана на примере STM32F103C8
Рисунок 4.

Применяем алгоритм фильтра Калмана

Алгоритм упрощенного фильтра Калмана я взял с сайта Алекс Гувера. С моей стороны был доработан автоматический расчет переменной значения среднего отклонения.

На Листинге 4 показан алгоритм расчета фильтра Калмана.

Листинг 4.

// переменные для калмана
float varVolt = 0; // среднее отклонение (расчет в программе)
float varProcess = 2; // скорость реакции на изменение (подбирается вручную)
float Pc = 0.0;
float G = 0.0;
float P = 1.0;
float Xp = 0.0;
float Zp = 0.0;
float Xe = 0.0;

float filter(float val) { //функция фильтрации
Pc = P + varProcess;
G = Pc/(Pc + varVolt);
P = (1-G)*Pc;
Xp = Xe;
Zp = Xp;
Xe = G*(val-Zp)+Xp; // "фильтрованное" значение
return(Xe);
}

В основном цикле добавляем сигнал с фильтрацией (Листинг 5).

Листинг 5.

for (int i=0;i<256;i++)
{
char str[10];
a2str(sin_tab_shum[i], str);
strcat(str, str_ln);
HAL_UART_Transmit(&huart1,(uint8_t *) str_$, strlen (str_$), 100);
HAL_UART_Transmit(&huart1,(uint8_t *) str, strlen (str), 100);

int fil_var0 = sin_tab_shum[i];
int fil_var = filter(fil_var0 );
char buffer[4];
sprintf(buffer, "%u", fil_var );
strcat(buffer, str_ln);

HAL_UART_Transmit(&huart1,(uint8_t *) str_sp, strlen (str_sp), 100);
HAL_UART_Transmit(&huart1,(uint8_t *) buffer, strlen (buffer), 100);
HAL_UART_Transmit(&huart1,(uint8_t *) str_t, strlen (str_t), 100);
}

Полученные данные отправляем в SerialPortPlotter и получаем следующий результат (Рисунок 5). Красной линией обозначен первоначальный сигнал, а желтой линией –отфильтрованный.

Применение фильтра Калмана на примере STM32F103C8
Рисунок 5.

Данный метод поможет правильно считывать показания с аналоговых датчиков температуры, давления, освещенности и т.д.

Ссылки

  1. Исходный код Keil uVision5 и STM32CubeMX
Для комментирования материалов с сайта и получения полного доступа к нашему форуму Вам необходимо зарегистрироваться.
Имя