Фільтр Калмана - складно?

Технології 09 січня 2025

Нещодавно прочитав пост з «Доповненої реальності», в якому згадується Фільтр Калмана в порівнянні з більш простим «альфа-бета» фільтром. Давно збирався визнати щось на зразок сниппета за складанням ФК, і ось думаю саме час. У статті я вам розповім як на практиці можна скласти розширений ФК не особливо обтяжуючи себе високонауковими роздумами і глибокими теоретичними вишукуваннями.

Основні поняття
Завдання
Математична модель
Ув'язнення
Список літератури

Основні поняття

Рівняння «вхід-вихід» - це диференційне рівняння (наприклад, складене за рівняннями Лагранжа другого роду), у якого з лівого боку від знака рівності знаходяться додані, залежні від змінної виходу (узагальнені координати власного руху системи), а з правого - додані з вхідною змінною або вільні члени. Для простоти можна навести наступний приклад: для маятника в лівій частині рівняння «вхід-вихід» будуть стояти складаються з кутом відхилення вантажу від положення спокою, а в правій - складаються з силою тяжкості та іншими зовнішніми силами. Як виходять такі рівняння я описувати не буду - немає необхідності ускладнювати публікацію марними теоретичними викладками. Нижче покажу, що обійтися можна зручними для практики методами.
Передавальна функція (ПФ) - ставлення виходу до входу, вірніше вихідної змінної до вхідної. За правилом пропорції, після виносу мозку змінних за дужки, передавальна функція дорівнює відношенню лівої частини, записаної в операторній формі, до правої.
Операторна форма запису - в ній записується рівняння «вхід-вихід» після заміни виду: s = d/dt и x(t) -> x(s). У результаті рівняння вигляду:

a1*x''+a2*x'+a3*x = b1*u'+b2*u,

перетворюється на вигляд:

(a1*s^2+a2*s+a3)*x(s) = (b1*s+b2)*u(s).

При цьому передавальна функція матиме вигляд:

W(s) = x(s)/u(s) = (b1*s+b2)/(a1*s^2+a2*s+a3)

Завдання

У якості програми для ФК візьмемо завдання обробки інформації блоку датчиків (див. [1]). Ми маємо чотири одиниці двоосьових мікромеханічних акселерометрів (ММА). Всього 8 вимірювальних каналів, для кожного з яких можна скласти окреме рівняння типу «вхід-вихід». Тут слід зауважити, що деякі з виробників датчиків (наприклад, Analog Devices) надають наближену лінеаризовану (це важливо в нашому випадку) математичну модель. При роботі над дипломом я отримав від колег модель для MatLab. Але навіть, якщо виробник жмодиться завзято охороняє свої секрети, біда невелика. Якщо говорити про акселерометри від AD, то їх смуга пропускання для багатьох практичних завдань велика. Наприклад, у ADXL203 смуга, якщо не змінює пам'ять, 10кГц. Як акустичний датчик не прокотить (не дотягує до сотень кГц), а для цивільного застосування на рухомих об'єктах (машинах, яхтах, моторолерах, велосипедах, дитячих візках) або в ігровій індустрії (людині руками махати з частотою навіть 10 Гц навряд чи під силу) така смуга надлишкова.

Таким чином, ми самі можемо скласти потрібну нам модель датчиків. І таким шляхом ми вбиваємо відразу двох зайців: складаємо просту лінеаризовану модель і вводимо фільтрацію шумів вже на найнижчому рівні. Про простоту моделі потрібно зауважити, що при використанні рівняння, точно описує динаміку датчика, ми будемо змушені складати матриці фільтру Калмана з чисел, порядки яких сильно розрізняються. У моделі «від виробника» для згаданих акселерометрів відношення максимального коефіцієнта до мінімального - близько 1e + 10. Це призводить до жорстоко низької обумовленості обчислень, тобто мала похибка в завданні значення коефіцієнта може призвести до дуже великої обчислювальної похибки. Звужуючи смугу пропускання, закладену в математичну модель, ми по суті збільшуємо стабільність обчислень.

Так як скласти цю саму модель? Та дуже просто. Скориставшись пакетами MatLab або Octave і задавшись потрібною нам смугою пропускання (ФНЧ, ФВЧ або смугового фільтра), методом тику або за допомогою плагінів у пакетах синтезуємо ПФ. Тут потрібно зауважити, що в пакет MatLab входить візуальний засіб синтезу дискретного фільтра. В Octave поки я аналога не знайшов. В обох пакетах можна побудувати графіки АЧХ і ФЧХ, на яких потрібно звернути увагу на частоту (або частоти) зрізу, а також на коливання графіка АЧХ в і поза смугою пропускання. Загалом, шаманім з коефіцієнтами передавальної функції до тих пір, поки графіки АЧХ і ФЧХ нас не задовольнять. Я ж для себе вже намітив інший, більш ледачий простий шлях, а саме використовувати генетичні алгоритми для ідентифікації. Потрібно лише реалізувати еталонні вибірки «вхід - вихід» і організувати еволюційний процес. Але з наукової точки зору викладені «методики» невалидні.

Математична модель

І так для простоти умовимося, що є рівняння в операторній формі:

Це одне з найпоширеніших рівнянь динамічної ланки другого порядку. У цьому рівнянні а2 - відіграє роль коефіцієнта жорсткості пружини (сила жорсткості прямо пропорційна відстані положення чутливого елемента від положення спокою); а1 - коефіцієнт демпфування (сила, пропорційна швидкості руху; згадайте про мит - повільно рухати в ньому ложку легше, ніж швидко); а0 - характеризує інерцію. У коефіцієнти праворуч від знака рівності вкладається дещо інший зміст. Ці коефіцієнти характеризують ефективність впливу за різними порядками похідних. Тут важливо розуміти, що вимога фізичної реалізованості обмежує максимальний порядок похідної праворуч максимальним порядком похідної лівої частини.

Отримавши таке рівняння ми можемо починати підготовку до складання фільтра Калмана. Для цього нам спочатку потрібно перетворити рівняння «вхід-вихід» до моделі «простору станів». Це робиться за допомогою перетворення до «форми Коші».

Для початку слід перетворити на форму пропорцій і ввести нову (фіктивну) змінну:

У результаті отримаємо систему рівнянь:

У цій системі перше рівняння - рівняння динамічного процесу, а друге - рівняння спостереження.

Наведемо перше рівняння до форми Коші (до системи диференційних рівнянь першого порядку) заміною x1 = x:

Для зручності запису замін у цій системі було зроблено зворотне перетворення

Лапласа (перехід від операторної форми до диференційної). В результаті ми розбили

вихідне рівняння на два рівняння першого порядку. Введені змінні х1 і х2 називаються «змінними стану» або «фазовими координатами». Отримана система оперує «безперервним» часом. Ми її складали за вихідними диференційними рівняннями. Але нам потрібно побудувати різнісний алгоритм (алгоритм в дискретному часі) для обчислення значення фазових координат рекуррентним способом в ЕВМ. Тому перед використанням у фільтрі Калмана цю систему потрібно буде «дискретизувати». Але про це пізніше.

Зараз нам потрібно скласти рівняння спостереження для введених фазових координат. З урахуванням

введених замін і в диференційній формі отримаємо:

Залишається переписати отримані рівняння у векторно-матричній формі, попередньо розставивши додані відповідно до індексів фазових координат:

У цьому рівнянні

Для дискретизації цих рівнянь потрібно використовувати такі вирази:

де I - одинична матриця, F і R - шукані дискретизовані матриці моделі простору станів (різнісних рівнянь), ts - період квантування.

Період квантування - це ключовий елемент дискретизації. Чим він менший, тим точніше

повторює дискретну модель поведінку моделі з безперервним часом. До того ж, при

занадто великих значеннях ts через обчислювальні похибки дискретна модель стане

нестійкою. Якщо об'єкт не є високо динамічним (наприклад, фільтр низьких частот з

смугою пропускання до 10 Гц), то період квантування можна вибрати досить великим. Для

згаданого ФНЛ період квантування можна вибрати з великим запасом рівним 0.01 сек.

Отже, ми отримали дискретну модель виду:

Тепер все готове для складання моделі ФК. Ми повинні просто вибудувати по діагоналі отримані для кожного вимірювального каналу матриці F, R, і С. Для 8 вимірювальних каналів, для кожного з яких складена дискретна власна матриця F з розмірами 2х2 ми отримаємо власну матрицю A фільтра Калмана (див. зображення на початку публікації) з розмірами 16х16. Також надходимо з вектором-стовпчиком R: 2х1 * 8 каналів = 16х8 - матриця B в моделі ФК. З векторів-рядків C складається матриця H фільтра Калмана (8х16).

Готово. Далі потрібно це все запрограмувати. Особисто я вважаю за краще для цього використовувати Ruby + NArray - прототипувати з цією зв'язкою одне задоволення, але це моя особиста думка... кому як. Тільки врахуйте, що в лоб програмувати ці рівняння не варто - отримані матриці сильно розріджені нулями. Якщо подумати, то всі матричні вирази можна розбити на блочні операції. Оптимізувати тут багато чого потрібно на стадії реалізації.

Ув'язнення

Вибачте за деяку плутанину в позначеннях. Формули сіпалися з різних праць. Постараюся коротко роз'яснити що є що в рівняннях ФК на зображенні на початку посту.

Перше, що може кинутися в очі - матриці P, Q, R, K. Це, відповідно, матриця коваріацій помилок оцінювання (помилок вирішення завдання ФК), матриці коваріацій шумів динамічного процесу і вимірювальних шумів, а також матриця коефіцієнтів посилення фільтра Калмана. Вектори «х» і «z» - це фазовий вектор ФК (імітація фазового вектора блоку датчиків всередині моделі ФК) і вектор вихідних сигналів реальних датчиків (по суті це канал надходження інформації ззовні в ФК). З представленої діаграми легко зрозуміти принцип роботи ФК.

Начебто нічого складного. Однак у роботі ФК існує ряд важливих нюансів. Так ФК розроблений як алгоритм, що дає найменшу середньоквадратичну помилку при дотриманні декількох умов (гіпотез). Перша - шуми є білими (рівномірна спектральна характеристика) з гаусовим розподілом випадкових величин. Але на практиці ця гіпотеза не виконується, оскільки важко знайти джерело шумів з такими ідеальними параметрами. До того ж самі вимірювальні системи мають кінцеві смуги пропускання. У цьому випадку у фазовий вектор ФК потрібно включити ще й змінні для шумових процесів. Використовується штучний підхід: вважається, що у нас є джерело білого шуму, сигнал від якого проходить через фільтр. Таким чином в модель ФК додається модель ще двох динамічних об'єктів - фільтрів, які перетворюють пару білих шумових процесів на кольорові шум динамічного процесу і вимірювальний шум.

Друга гіпотеза - некорельованість шумів різних вимірювальних каналів. Вона також виконується рідко і знову ж таки є штучні методи обійти це обмеження. Всі ці методи призводять до збільшення розмірності ФК = > збільшують обчислювальне навантаження.

Тонкощі роботи ФК ускладнюють життя ботаніків вчених, але часто для практичних застосувань важливіше скоротити розмірність ФК ніж задовольнити якимось теоретичним вимогам. Ми шукаємо наближене рішення чисельними методами, а не точне аналітичне рішення з чітким теоретичним обґрунтуванням. Хоча кому як...