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