Unscented Kalman Filter

Реализация сигма-точечного фильтра Калмана на основании алгоритма, описанного в статье :

• Численные методы нелинейной фильтрации для обработки сигналов и измерений.

В качестве примера приведена реализация UKF для оценки курсового угла автомобиля во время движения на основе установленного на борту MEMS IMU и Спутниковой навигационной системы (СНС/SNS). Модель наблюдателя курсового угла представляет собой интеграл истинной скорости рыскания, которую можно представить как сумму скорости рыскания, измеренной датчиком угловых скоростей (ДУС), входящим в состав IMU, и белого гауссовского шума. $$ψ_{i+1} = ψ_i + ω_{z_i} * dt + ξ_{ω_z} * dt$$ В качестве измерения используются измерения курсового угла СНС c добавлением белого гауссовского шума. $$ψ_i + ζ_{ψ_i}$$ Вектор состояния включает в себя оценку курсового угла $$x=[\hat{ψ}]$$ Таким образом получается скалярный вариант ФК: $$Q = [ σ_{w_z}^2 ], R=[σ_{sns}^2 ]$$ где Q – матрица шума процесса, R – матрица шума измерений, σ – соответствующие среднеквадартические отклонения. Матрица шума измерений состоит из дисперсий шума измерений, значения которых передаются приемником СНС наряду с измерениями курсового угла. Значения этой матрицы изменяются на каждом такте работы системы в зависимости от условий приема спутниковых сигналов. Матрица шума процесса Q настраивается эмпирически. Представленный пример имеет смысл демонстрации работы алгоритма. По желанию ползователя размер вектора состояния может быть увеличен, а модель процесса представлена нелинейной функцией.

Для запуска примера достаточно запустить выполнение файла example.py и установить все зависимости из файла requirements.txt. На рисунке ниже показана работа алгоритма при работе с реальными данными с автомобиля, содержащимися в файле test.txt.