tapekhav / MLAT

Modeling of the aircraft coordinate determination system using MLAT technology

MLAT

— технология, позволяющая определять местоположение самолётов или иных воздушных судов (ВС) без использования дополнительного к имеющемуся оборудованию на ВС.

Зависимости

Для С++ присутствует единственная зависимость GTest - фрэймворк для юнит-тестов. В случае отсутствия данной зависимости библиотеки и тестовое приложение соберется, а unit-тесты будут пропущены. Для Python, который используется для визуализации данных требуется matplotlib.

Сборка

sudo apt-get install libgmock-dev
sudo apt-get install libgtest-dev
pip3 install matplotlib
mkdir build
cd build
> conan_paths.cmake
cmake ..

При сборке под windows, необходимо установить менеджер зависимостей conan версии 1.56.0

pip3 install matplotlib
mkdir build && cd build
conan install ..
cmake ..

Запуск

Для запуска необходимо запустить исполняемый файл simulator и выполнить run-plotter.sh, которому при необходимости выдать права на исполнение с помощью команды

chmod +x run-plotter.sh

При запуске под windows необходимо запустить simulator.exe и выполнить run-plotter.cmd

Описание модели.

Пусть самолет имеет координаты (x, y, z), а i-aя вышка имеет координаты (x_i, y_i, z_i). Всего пусть задано n > 3 вышек, тогда есть

$$ C_n^k = \frac{n(n-1)}{2} $$

способов выбрать две из них. Обозначим, расстояние от самолета до i-ой вышки как d_i. Расстояние от судна до j-ой вышки, соответственно, как d_j. Соответсвующие расстояния выражаются по формулам

$$ d_i = \sqrt{(x-x_i)^2 + (y-y_i)^2 + (z-z_i)^2}, \\ d_j = \sqrt{(x-x_j)^2 + (y-y_j)^2 + (z-z_j)^2}.\\ $$

Зададим функцию d_ij(x, y, z), как расстояние между вышками i и j, тогда

$$ d_{ij}(x,y,z) = |d_i - d_j| = \left | \sqrt{(x-x_i)^2 + (y-y_i)^2 + (z-z_i)^2} - \sqrt{(x-x_j)^2 + (y-y_j)^2 + (z-z_j)^2} \right |. $$

С другой стороны, нетрудно получить, что

$$ d_i = ct_i, $$

где t_i является временем, за которое сигнал доходит от самолета, до вышки, тогда обозначим

$$ d_{ij} = c|t_i - t_j| = c\Delta t_{ij}, $$

где c - скорость распространения сигнала, в нашем случае это скорость света, а разница времен есть ни что иное, как Time Difference of Arrival (TDOA), эта информация является известной нам, так как её мы получаем с вышек.

Постановка задачи

Реализовать описанную модель с переопределённой системой, решаемой с помощью нелинейного МНК с встроенным фильтром Калмана, определяющего параметры модели равноускоренного движения точки. Наблюдаемыми величинами в фильтре Калмана должны являться TDOA, а не положения точки.

Решение

Первоочередно перейдем от TOA, к TDOA. Далее, ясно, что

$$ d_{ij}(x,y,z) = d_{ij} \Leftrightarrow f_{ij}(x,y,z) \equiv d_{ij}(x,y,z) - d_{ij} = 0, $$

перебрав все возможные комбинации i и j, получим систему функций f(x, y, z) = (f₁₁, ..., f_n(n-1)). Нетрудно догадаться, что задача состоит в поиске таких параметров (x, y, z), что f(x, y, z) = 0. Построенная система уравнений является переопределенной. Она содержит всего 3 неизвестных, а вышек, как минимум 4, то есть минимум 6 уравнений.

Передадим фильтру Калмана TDOA. Там сначала применим метод наименьших квадратов (МНК):

$$ S(x,y,z) = \left | f(x,y,z) \right | 2^2 = \sum{i,j} f_{ij}^2(x,y,z). $$

Будем искать такие решения, которые будут минимизировать написанную сумму, для этого применим алгоритм Гаусса-Ньютона, обозначив за x вектор решения, получим:

$$ x^{(k+1)} = x^{(k)} - (J_f^TJ_f)^{-1}J_f^T \cdot x^{(k)}, $$

где J_f - матрица Якоби для системы f. А произведение матриц в вычитаемом есть псевдообратная матрица к матрице Якоби, которую можно вычислить с помощью QR разложения.

Далее начинается работа фильтра Калмана, в которой два этапа: предсказание и корректировка.

Предсказание состоит из:

1. Предсказания нового состояния системы $$ x_k = Fx_{k-1} $$
2. Предсказания ошибки ковариации $$ P_k = FP_{k-1}F^T $$ где $x_k$ - вектор состояния, $F$ - матрица эволюции(динамическая модель системы), $P_k$ - ковариационная матрица состояния.

Этап корректорировки состоит из:

1. Вычисление Kalman Gain $$ S = HP_kH^T + R $$ $$ K_k = P_k * H^T * S^{-1} $$
2. Корректировки вектора состояния с учетом пересчета ошибки с только что поданным TDOA $$ x_k = x_k + K_k z_k $$
3. Обновление ошибки ковариации $$ P_k = (I - K_kH)P_k $$ где $K_k$ - Kalman Gain, $H$ - матрица, отображающая отношение измерений и состояний, $R$ - ковариационная матрица шума, $z_k$ - пересчитанная ошибка с учетом только что поданных TDOA, $I$ - единичная матрица

Пример работы программы

Здесь отображены 3D модель движения самолета, проекции движения со скоростью и ускорением и модуль разности реальных значений и полученных после оценки фильтром Калмана

Литература

1. Вычисление сингулярного разложения матриц. Афанасьева А. А.
2. Gauss–Newton algorithm, wikipedia.
3. Moore–Penrose inverse, wikipedia, chapter Construction.
4. SVD, wikipedia.
5. A New Approach to Linear Filtering and Prediction Problems
6. Построение параметров траектории с использованием фильтра Калмана с шагом коррекции по всем измерениям в РЛС дальнего обнаружения. Бородавкин Л. В.