Фильтр Калмана (ФК) является оптимальным линейным алгоритмом фильтрации параметров динамической линейной системы при наличии неполных и зашумленных наблюдений. Этот фильтр находит широкое применение в технических системах управления до оценок динамики изменения макроэкономических ситуаций или общественного мнения.
Данная статья ставит себе целью познакомить читателя со стандартным подходом к переходу от непрерывной модели динамической системы, описываемой системой произвольных линейных дифференциальных уравнений к дискретной модели.
Так же эта статья призвана сподвигнуть читателя на применение ФК в тех задачах, где на первый взгляд кажется что линейный ФК неприменим, а на самом деле это может быть не так.
Написать статью автора сподвиг тот факт, что несмотря на простоту последующих вещей в поисковой выдаче гугла как на русском так и на английском языке (по крайней мере на первой странице) автору найти их не удалось.
ФК может быть выполнен как в дискретном так и непрерывном виде. Наибольший интерес с точки зрения практической реализации на современных цифровых вычислителях представляет именно дискретный ФК на который будет сделан упор в данной статье.
Линейный дискретный ФК описывается следующими выражениями. Пусть модель системы может быть представлена следующим образом:
— матрица перехода,
— переходная матрица управления,
— переходная матрица возмущения,
,
,
— вектора состояния, управления и шумов (возмущения) системы на
-том шаге. Модель наблюдения:
,
— вектора наблюдения и шума наблюдения на
-том шаге. 5 уравнений работы ФК в данной статье интереса не представляют, поэтому на случай если они кому-либо нужны приводятся под спойлером.
Здесь и далее речь идет о стационарных (с постоянными коэффициентами) системах, для которых матрицы
,
и
не зависят от номера
.
В подавляющем большинстве практических приложений ФК осуществляет фильтрацию параметров непрерывных динамических систем, описываемых дифференциальными уравнениями для непрерывного времени. Обсчет ФК при этом происходит на цифровом вычислителе, что автоматически делает ФК дискретным и модель соответственно должна быть дискретной. Для получения дискретной модели этих непрерывных систем необходимо сначала составить сам вектор состояния (фазовый вектор), систему уравнения состояния, затем дискретизировать их, получив тем самым матрицы
,
и
.
Пусть поведение системы описывается набором из
дифференциальных уравнений первого порядка:
—
-мерный вектор состояния системы. Вектор состояния (он же фазовый вектор) это вектор, который содержит в себе переменные, описывающие систему и их производные вплоть до необходимого порядка.
—
-мерный вектор управления системы, описывающий оказываемое на систему контролируемое воздействие.
-мерный вектор, содержащий в себе случайное неконтролируемое воздействие на систему, или шумы.
— матрица состояния системы размером
.
— матрица управления размером
.
— матрица возмущения размером
. В этом выражении все произведения вычисляются по правилам матричного умножения. В общем случае элементы всех матриц являются функциями времени, однако в статье рассматриваются только стационарные системы, где элементы не зависят от времени.
Пример перехода от описания системы с помощью дифференциального уравнения высшего порядка к описанию через пространство состояний приведен ниже.
Для корректного перехода в дискретную область (другими словами дискретизации модели) нам потребуется ввести понятие матричной экспоненты. Матричной экспонентой называется матричная функция, полученная по аналогии с разложением экспоненциальной функции в ряд Тейлорана самом деле Маклорена:
подразумевается единичная матрица.
Точный переход от непрерывной модели в пространстве состояний к дискретной модели требует поиска решения однородной системы
, затем перехода к первоначальной системе, отыскания общего решения и интегрирования от начального момента
до некоторого
. Строгий вывод может быть найден в [1], здесь же приводится готовый результат.
В случае стационарности непрерывной динамической модели (не зависимости матриц
,
,
от времени) для получения дискретной модели можно ввести вспомогательную переходную матрицу системы
из момента
в момент
, где
:
и
подразумеваются матрицы из непрерывных уравнений, под
и
искомые матрицы дискретной модели.
Для иллюстрации вышеописанной математики рассмотрим два примера. Один из которых разминочный, а второй иллюстративный, для демонстрации возможностей описанного метода.
Пусть объект движется вдоль оси
с начальной скоростью
и постоянным ускорением
. Тогда его модель может быть представлена в виде:
, так как для вычисления требуется
. В то же время для вычисления
производная
не требуется, поэтому вносить производные порядка выше
в вектор состояния не имеет особого смысла.
Объединим три переменных в вектор состояния
и запишем систему уравнений в матричном виде для перехода к форме пространства состояния:

Читатель может сам убедиться в том, что
и выше представляет собой нулевую матрицу.
Таким образом получена известная всем матрица перехода, выведенная без применения каких-либо допущений.
Положим что наш объект движется в трехмерном пространстве с некой постоянной (по модулю) линейной скоростью и с угловой скоростью, представленной псевдовектором:
.
Распишем векторное произведение подробнее:
будет представлять собой:
Далее осуществим переход к матрице
по соответствующему выражению. Так как устно перемножать матрицы размером
по три раза довольно тяжело, вероятность ошибки велика, да и не царское это дело, то напишем скрипт с использованием библиотеки sympy языка Python:
И запустив его получим примерно вот это:
Что после обрамления соответствующими тэгами и вставки в исходный код статьи превращается в:
Таким образом может быть выведена матрица перехода фильтра Калмана для движения по окружности.
В отличии от предыдущего случая результат возведения
в степень выше 3 не является нулевой матрицей.
Поэтому представление такой матрицы возможно с конечной точностью. Однако при
ряды, получающиеся в элементах матрицы
сходятся довольно быстро. Для практического применения достаточно членов до второй степени, редко до третьей и тем более до четвертой.
Дополнительно проиллюстрируем работу матрицы
задав вектор
,
,
, и рекуррентную последовательность вида: 
В итоге задав некоторое начальное положение, скорость и угловую скорость можно получить такую траекторию

Точность совпадения первой и последних точек может быть получена как
При радиусе поворота порядка 150 единиц относительная погрешность не превышает величин порядка
. Этой точности вполне достаточно для модели ФК, следящего за поворачивающей целью.
Если раньше ФК применялся в основном для решения задач навигации, где применение линейных моделей движения давало неплохой результат, то с развитием таких современных приложений как робототехника, компьютерное зрение и прочее увеличилась надобность и в более сложных моделях движения объектов. При этом применение вышеописанного подхода позволяет без особых затрат синтезировать дискретную модель ФК, что позволят облегчить разработчикам задачу. Единственное ограничение такого подхода заключается в том, что непрерывная модель динамической системы должна описываться набором линейных, или хотя бы линеаризуемых, уравнений в пространстве состояния.
Резюмируя вышесказанное можно привести алгоритм синтеза переходной матрицы ФК:
Автором приветствуется конструктивная критика в отношении допущенных ошибок, неточностей, неверных формулировок, не упомянутых методов и прочего. Спасибо за внимание!
[1] Медич Дж. Статистически оптимальные линейные оценки и управление. Пер. с англ. Под ред. А.С. Шаталова Москва. Издательство «Энергия», 1973, 440 с.
[2] Матвеев В.В.Основы построения бесплатформенных инерциальных систем СПб.: ГНЦ РФ ОАО «Концерн „ЦНИИ Электроприбор“,2009. — 280с. ISBN 978-5-900180-73-3
Данная статья ставит себе целью познакомить читателя со стандартным подходом к переходу от непрерывной модели динамической системы, описываемой системой произвольных линейных дифференциальных уравнений к дискретной модели.
Скрытый текст
а так же сэкономить читателю время, избавляя того от попыток изобретения велосипеда и выставления себя перед коллегами в некрасивом свете. Не будьте как автор
Так же эта статья призвана сподвигнуть читателя на применение ФК в тех задачах, где на первый взгляд кажется что линейный ФК неприменим, а на самом деле это может быть не так.
Написать статью автора сподвиг тот факт, что несмотря на простоту последующих вещей в поисковой выдаче гугла как на русском так и на английском языке (по крайней мере на первой странице) автору найти их не удалось.
Динамическая модель для дискретного фильтра Калмана
Скрытый текст
В основном этот раздел нужен, для того чтобы познакомить читателя с системой принятых обозначений, которая очен сильно разнится от книги к книге и от статьи к статье. Объяснение смысла всех входящих в уравнения величин выходит за рамки данной статьи, при этом подразумевается что зашедший на огонек имеет об этом смысле некоторое представление. Если это не так, добро пожаловать сюда, сюда и сюда.
ФК может быть выполнен как в дискретном так и непрерывном виде. Наибольший интерес с точки зрения практической реализации на современных цифровых вычислителях представляет именно дискретный ФК на который будет сделан упор в данной статье.
Линейный дискретный ФК описывается следующими выражениями. Пусть модель системы может быть представлена следующим образом:
Скрытый текст
Первый этап, экстраполяция:
Здесь и далее речь идет о стационарных (с постоянными коэффициентами) системах, для которых матрицы
Непрерывная динамическая модель системы. Пространство состояний.
В подавляющем большинстве практических приложений ФК осуществляет фильтрацию параметров непрерывных динамических систем, описываемых дифференциальными уравнениями для непрерывного времени. Обсчет ФК при этом происходит на цифровом вычислителе, что автоматически делает ФК дискретным и модель соответственно должна быть дискретной. Для получения дискретной модели этих непрерывных систем необходимо сначала составить сам вектор состояния (фазовый вектор), систему уравнения состояния, затем дискретизировать их, получив тем самым матрицы
Пусть поведение системы описывается набором из
Пример перехода от описания системы с помощью дифференциального уравнения высшего порядка к описанию через пространство состояний приведен ниже.
Пример
Пусть движение точки вдоль некоторой оси
описывается дифференциальным уравнением второго порядка:
. Теперь имеем:
, матрица состояния окажется
играет роль скорости. Матрицы
и
в данном примере являются нулевыми, так как отсутствуют какие-либо управляющие и возмущающие воздействия.
Переход в дискретную область
Для корректного перехода в дискретную область (другими словами дискретизации модели) нам потребуется ввести понятие матричной экспоненты. Матричной экспонентой называется матричная функция, полученная по аналогии с разложением экспоненциальной функции в ряд Тейлора
Точный переход от непрерывной модели в пространстве состояний к дискретной модели требует поиска решения однородной системы
В случае стационарности непрерывной динамической модели (не зависимости матриц
Практические примеры
Скрытый текст
К сожалению в примерах будут только извращения с матрицей
, так как автору лень выдумывать примеры с управляющими воздействиями и вообще он в рамках диссертации занимается вопросами навигации где управляющих воздействий нет. Тем более что при зачаточных знаниях математического анализа после разбора примеров эти действия не должны вызвать проблем. За примерами с ненулевыми
и
можно сходить в [2].
Для иллюстрации вышеописанной математики рассмотрим два примера. Один из которых разминочный, а второй иллюстративный, для демонстрации возможностей описанного метода.
Тривиальный
Пусть объект движется вдоль оси
Объединим три переменных в вектор состояния
Читатель может сам убедиться в том, что
Таким образом получена известная всем матрица перехода, выведенная без применения каких-либо допущений.
Нетривиальный пример
Положим что наш объект движется в трехмерном пространстве с некой постоянной (по модулю) линейной скоростью и с угловой скоростью, представленной псевдовектором:
Распишем векторное произведение подробнее:
Далее осуществим переход к матрице
from sympy import symbols, Matrix, eye
x, y, z, T = symbols('x y z T')
vx, vy, vz = symbols('v_x v_y v_z')
wx, wy, wz = symbols('w_x w_y w_z')
A = Matrix([
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, -wz, wy],
[0, 0, 0, wz, 0, -wx],
[0, 0, 0, -wy, wx, 0]
])
F = eye(6) + A*T + A*A*T**2/2
from sympy import latex
print(latex(F))
И запустив его получим примерно вот это:
Скрытый текст
\left[\begin{matrix}1 & 0 & 0 & T & - \frac{T^{2} w_{z}}{2} & \frac{T^{2} w_{y}}{2}\\0 & 1 & 0 & \frac{T^{2} w_{z}}{2} & T & - \frac{T^{2} w_{x}}{2}\\0 & 0 & 1 & - \frac{T^{2} w_{y}}{2} & \frac{T^{2} w_{x}}{2} & T\\0 & 0 & 0 & \frac{T^{2} \left(- w_{y}^{2} - w_{z}^{2}\right)}{2} + 1 & \frac{T^{2} w_{x} w_{y}}{2} - T w_{z} & \frac{T^{2} w_{x} w_{z}}{2} + T w_{y}\\0 & 0 & 0 & \frac{T^{2} w_{x} w_{y}}{2} + T w_{z} & \frac{T^{2} \left(- w_{x}^{2} - w_{z}^{2}\right)}{2} + 1 & \frac{T^{2} w_{y} w_{z}}{2} - T w_{x}\\0 & 0 & 0 & \frac{T^{2} w_{x} w_{z}}{2} - T w_{y} & \frac{T^{2} w_{y} w_{z}}{2} + T w_{x} & \frac{T^{2} \left(- w_{x}^{2} - w_{y}^{2}\right)}{2} + 1\end{matrix}\right]
Что после обрамления соответствующими тэгами и вставки в исходный код статьи превращается в:
Таким образом может быть выведена матрица перехода фильтра Калмана для движения по окружности.
В отличии от предыдущего случая результат возведения
например <math>$inline$A^3$inline$</math>
или <math>$inline$A^4$inline$</math>
Поэтому представление такой матрицы возможно с конечной точностью. Однако при
Дополнительно проиллюстрируем работу матрицы
код на Python
Напомню, что для типа np.array символ "@" обозначает матричное перемножение. Расстояния и скорости измеряются в попугаях, угловая скорость в рад/с. Так же необходимо помнить, что для получения окружности надо чтобы вектора скорости и угловой скорости были перпендикулярны, иначе вместо окружности получится спираль.
import numpy as np
from numpy import pi
T = 1
wx, wy, wz = 0, 2*pi/100/2**.5, 2*pi/100/2**.5
vx0 = 10
A = np.array([
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, -wz, wy],
[0, 0, 0, wz, 0, -wx],
[0, 0, 0, -wy, wx, 0]
])
F = np.eye(6) + A * T + A @ A * T**2/2 + A @ A @ A * T**3/6
X = np.zeros((6, 101))
X[:, 0] = np.array([0, 0, 0, vx0, 0, 0])
for k in range(X.shape[1] - 1):
X[:, k + 1] = F @ X[:, k]
import matplotlib as mpl
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot(X[0, :], X[1, :], X[2, :])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.show()
Напомню, что для типа np.array символ "@" обозначает матричное перемножение. Расстояния и скорости измеряются в попугаях, угловая скорость в рад/с. Так же необходимо помнить, что для получения окружности надо чтобы вектора скорости и угловой скорости были перпендикулярны, иначе вместо окружности получится спираль.
В итоге задав некоторое начальное положение, скорость и угловую скорость можно получить такую траекторию

Точность совпадения первой и последних точек может быть получена как
>>> print(X[:3, 0] - X[:3,-1])
[-0.00051924 -0.0072984 0.0072984 ]
При радиусе поворота порядка 150 единиц относительная погрешность не превышает величин порядка
Заключение
Если раньше ФК применялся в основном для решения задач навигации, где применение линейных моделей движения давало неплохой результат, то с развитием таких современных приложений как робототехника, компьютерное зрение и прочее увеличилась надобность и в более сложных моделях движения объектов. При этом применение вышеописанного подхода позволяет без особых затрат синтезировать дискретную модель ФК, что позволят облегчить разработчикам задачу. Единственное ограничение такого подхода заключается в том, что непрерывная модель динамической системы должна описываться набором линейных, или хотя бы линеаризуемых, уравнений в пространстве состояния.
Резюмируя вышесказанное можно привести алгоритм синтеза переходной матрицы ФК:
- Запись дифференциального уравнения системы
- Переход к вектору состояния и к пространству состояний
- Линеаризация в случае необходимости
- Представление матрицы перехода в виде матричной экспоненты и усечение ряда при необходимости
- Вычисление остальных матриц с учетом матрицы перехода
Автором приветствуется конструктивная критика в отношении допущенных ошибок, неточностей, неверных формулировок, не упомянутых методов и прочего. Спасибо за внимание!
Использованная литература
[1] Медич Дж. Статистически оптимальные линейные оценки и управление. Пер. с англ. Под ред. А.С. Шаталова Москва. Издательство «Энергия», 1973, 440 с.
[2] Матвеев В.В.Основы построения бесплатформенных инерциальных систем СПб.: ГНЦ РФ ОАО «Концерн „ЦНИИ Электроприбор“,2009. — 280с. ISBN 978-5-900180-73-3