Фильтр Калмана в мироконтроллерах

При разработке устройств на микроконтроллере неминуемо сталкиваемся с проблемой измерения и ужасных помех. Использование сырых данных становится невозможным и на выручку приходит фильтр Калмана.

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

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

Если двигаться на автомобиле и получая текущие координаты по GPS, мы сможем увеличить точность определения положения, если будем учитывать скорость и направление движения автомобиля, предугадывая где будет находится автомобиль в следующий момент времени. Скорость зависит от степени нажатия на педаль газа. И это все можно заложить в алгоритм фильтра, в конечном итоге получая более точные координаты, нежели полученные от одного GPS приемника.

В чистом фиде данный фильт редко используется. Обычно принимается его упрощенная версия без матриц и многоэтажных уравнений. Будет всего одна формула, которая выводится из всей горы матриц, при условии если мы пренебрежем расчетом управляющего воздействия. Формула в конечном итоге имеет следующий вид:

где    -  результирующее значение текущего вычисления, 

  -  коэффициент стабилизации,

  -  исходное значение текущего измерения,

  -  результирующее значение предыдущего вычисления.

Потребуется подобрать это коэффициент стабилизации, он должен быть больше 0 и не больше 1. Чем меньше коэффициент, тем сильнее сглаживаются данные, но при этом увеличивается время стабилизации. Вот пример, показывающий поведение результирующей кривой, при разных коэффициентах:

Для испытания я взял аналоговый акселерометр ADXL335 и подключил его к микроконтроллеру ATMega8, считывание данных идет через АЦП.

Код для Bascom-AVR>

$regfile = "m8def.dat"          'микроконтроллер ATmega8
$crystal = 8000000              'частота работы 8МГц
$baud = 9600                    'скорость передачи 9600 бод
$hwstack = 32
$swstack = 32
$framesize = 32


'конфигурируем АЦП
Config Adc = Single , Prescaler = 128 , Reference = Internal

Dim R As Word                  'переменная R в которую будем записывать показания с АЦП

declare sub kalman             'объявляем подпрограмму фильтрации

'переменные для фильтра
dim Mn as single     'результирующее значение
dim An as Single     'исходное значение
dim Mn1 as Single    'результат вычисления в предыдущей интерации
dim k as single      'коэффициент стабилизации
k = 0.1              'устанавливаем коэффициент

Start Adc

Do

R = Getadc(0)               'считывание данных с акселерометра

An = R
call Kalman                 'вызываем подпрограмму фильтрации

Print R ; ";" ; round (Mn)  'отправляем данные в терминал

Waitms 50

Loop

End

'подпрограмма фильтрации
sub Kalman
 Mn = k * An
 An = 1 - k
 Mn1 = Mn1 * An
 Mn = Mn + Mn1
 Mn1 = Mn
end sub

Данные от микроконтроллера AVR передаются через UART на компьютер и записываются в EXEL. Для увеличения количества шума датчик закреплен на электродвигателе.

Вибрация от двигателя передалась акселерометру и вот что получилось в результате: синий - исходный сигнал, красный - после фильтрации

Метод очень даже хорош. Пример аналогичного фильтра низких частот для Arduino:

#include <SerialFlow.h>

SerialFlow rd;

#define BAUD_RATE 9600

const byte led_pin = P1_0;
const byte acc_x_pin = A5;;

short acc_x_raw, acc_x, acc_xf;

float FK = 0.1;

void setup() {
    pinMode(led_pin, OUTPUT);    
    rd.setPacketFormat(SerialFlow::COMPLEX_1, 2, 2);
    rd.begin(BAUD_RATE);
    acc_xf = 0;
}

void loop() {
    acc_x_raw = analogRead(acc_x_pin);
    acc_x = acc_x_raw - 1023/2.0;
    acc_xf = (1-FK)*acc_xf + FK*acc_x;

    rd.setPacketValue(acc_x);
    rd.setPacketValue(acc_xf);
    rd.sendPacket();
    delay(20);
}