Кратко и по делу — геометрия, модель, погрешности, численные методы и критерии устойчивости.
1) Базовая геометрическая модель (TOA / псевдодальность как в GPS)
- Приёмники (известные координаты) ${\mathbf{s}}_i=(x_i,y_i,z_i)$. Источник ищем в точке $\mathbf{x}=(x,y,z)$.
- Идеальное измерение расстояния (TOA при синхронизованных часах): $r_i=\Vert \mathbf{x}-{\mathbf{s}}_i\Vert =\sqrt{(x-x_i{)}^2+(y-y_i{)}^2+(z-z_i{)}^2}$.
- Множества: каждое уравнение $\Vert \mathbf{x}-{\mathbf{s}}_i\Vert =r_i$ задаёт сферу с центром в ${\mathbf{s}}_i$ и радиусом $r_i$.
- Пересечение двух сфер — окружность (если непусто).
- Пересечение трёх сфер в общем случае даёт до двух точек (в 3D — обычно 0,1 или 2).
- Четвёртая независимая сфера даёт однозначное решение (если совместно не вырождено).
- В GPS используются псевдо-расстояния (псевдодовжины) ${\rho }_i=r_i+c\cdot b+{\epsilon }_i$, где $b$ — смещение часов приёмника (неизвестно), $c$ — скорость света, ${\epsilon }_i$ — шум/ошибка. Это добавляет четвёртый неизвестный $b$, поэтому для 3D требуются минимум 4 измерения (спутника).
2) TDOA / разности времен
- Разности ${\rho }_i-{\rho }_j=r_i-r_j+{\eta }_{ij}$ задают двухлопастные поверхности — двуполостные гиперболоиды. Каждая разность даёт одну уравнение-гиперболоид. Для локализации в 3D нужно по сути 3 независимых разностных уравнения (т.е. обычно 4 приёмника с опорным каналом).
3) Модель с шумом и линейризация
- Нелинейная модель псевдодлин: ${\rho }_i=\sqrt{(x-x_i{)}^2+(y-y_i{)}^2+(z-z_i{)}^2}+cb+{\epsilon }_i.$ - Линейризация вокруг приближённого решения ${\mathbf{x}}_0,b_0$ даёт модель вида
$$\Delta \mathbf{\rho }\approx H\Delta \mathbf{\theta }+\mathbf{\epsilon },$$ где $\Delta \mathbf{\theta }=[\Delta x,\Delta y,\Delta z,\Delta b{]}^T$, а строка якобиана для приёма $i$ $$h_i=\left[-\frac{x_0-x_i}{r_i},-\frac{y_0-y_i}{r_i},-\frac{z_0-z_i}{r_i},1\right].$$ - Итоговая поправка по методу наименьших квадратов (с весами $W$):
$$\Delta \mathbf{\theta }=(H^{T}WH{)}^{-1}{H}^{T}W\Delta \mathbf{\rho }.$$
4) Численные методы решения
- Итерационные нелинейные: Gauss–Newton, Levenberg–Marquardt (усиление устойчивости при плохой обусловленности). Шаги: начальное приближение → линейная поправка → обновление → пока схождение.
- Прямые (алгебраические) методы: методы дифференцирования уравнений (вычитание одного уравнения из остальных) приводят к линейной системе, или алгоритм Bancroft (альгебраическое замыкание для GPS) — дают начальное приближение или быстрый вычислимый результат, но чувствительны к шуму.
- Численная стабильность: решать систему через QR или SVD, не через нормальные уравнения напрямую, чтобы избежать усугубления ошибок.
- Для последовательной/трековой оценки: Калман/расширенный Калман (EKF) или частичный фильтр (UKF) с динамической моделью источника.
- Для выбросов: робастные M-оценки, RANSAC.
5) Учет погрешностей и систематических ошибок
- Шум: предполагается гауссовский $\epsilon$ с ковариацией $\Sigma$. Тогда ковариация оценок:
$$\mathrm{Cov}(\hat{\mathbf{\theta }})\approx (H^T{\Sigma }^{-1}H{)}^{-1}.$$ - Систематические ошибки: ионосфера/тропосфера, мультипуть, несинхронизированные часы — моделируются как дополнительные параметры или корректируются априорно; в GPS — поправки и двухчастотные измерения для удаления ионосферного вклада.
- В практических решениях используют взвешивание измерений по их дисперсиям и модельным коррекциям.
6) Геометрические критерии устойчивости и применимости
- Прерывность и уникальность решения: чтобы решение было устойчивым и однозначным, опоры (приёмники/спутники) должны ненулевым образом «окружать» объект в трёхмерном объёме — не находиться в одной плоскости и не быть скомпонованы близко друг к другу.
- Условность матрицы якобиана: плохая геометрия проявляется через большую обусловленность $\kappa (H)$ или малого наименьшего собственного значения $H^{T}H$. Практический критерий: если $\kappa (H)\gg 1$ — решение чувствительно к шуму.
- DOP (Dilution of Precision): удобная скалярная оценка. Для ковариации при $\Sigma ={\sigma }^{2}I$ $$\mathrm{Cov}(\hat{\mathbf{\theta }})={\sigma }^2(H^{T}H{)}^{-1},$$ и
$$\text{GDOP}=\sqrt{\mathrm{trace}((H^{T}H{)}^{-1})}.$$ Частные: PDOP (пространственная), HDOP/VDOP (гор./верт.). Практические пороги: GDOP 20 — плохая (ориентировочно).
- Тетраэдрический критерий: объём тетраэдра с вершинами в опорах (нормирован к масштабам) — маленький объём → плохая вертикальная обусловленность.
- Угловое покрытие: большие угловые расстояния опор вокруг цели уменьшают погрешность; близкое расположение опор (малые углы) ухудшает устойчивость.
- Минимальное число измерений: при неизвестном смещении часов — минимум 4 независимых измерения; если синхронизированное время — 3 достаточно (в идеальных условиях).
- Наличие зеркальных решений: при пересечении трёх сфер возможны две точки; дополнительная четвертая сфера, физические ограничения (например, земля) или знаковые условия устраняют зеркальную неоднозначность.
7) Практические рекомендации
- Начальное приближение берите из алгебраического метода (Bancroft) или предыдущей оценки (в треке).
- Используйте взвешенный LM (Levenberg–Marquardt) + SVD/QR при решении линейных шагов.
- Оцените DOP и условность $H$ до/после решения: если GDOP высок, увеличьте число измерений, измените геометрию опор или примените более надёжную модель/фильтр.
- Обрабатывайте выбросы и мультипуть робастно; учитывайте коррекции атмосферы для высокой точности.
Итого: геометрия — сферы (TOA) или гиперболоиды (TDOA); пересечения дают окружности/точки; шум превращает сферы в сферические слои, их пересечение — область неопределённости, аппроксимируемая эллипсоидом с ковариацией ${\sigma }^2(H^{T}H{)}^{-1}$. Устойчивость определяется укладкой опор (неплоскостность, большой объём тетраэдра, малый GDOP/малое condition number). Численно решают через итеративную линейризацию (Gauss–Newton / LM) с SVD/QR и взвешиванием; для инициализации — алгебраические методы (Bancroft) или разностные схемы.