Краткая идея: использовать плотную (tightly‑coupled) интеграцию carrier‑phase GNSS + IMU в виде фактор‑графа с IMU‑прединтеграцией, целенаправленной обработкой помех (NLOS / мультипуть), адаптивным взвешиванием наблюдений и привязкой целевых ограничений (ZUPT, ДМВ/барометр/визуальные/Odometry) — в оффлайн/онлайн режимах применять скользящее-окно + периодическое полное сглаживание для восстановления неоднозначностей и снижения дрейфа.
Алгоритм (шаги и ключевые формулы):
1) Модель состояния
- узлы состояния: позиция ${\mathbf{p}}_k$, скорость ${\mathbf{v}}_k$, ориентация ${\mathbf{R}}_k$ (или кватернион), смещения ИМУ ${\mathbf{b}}_f,{\mathbf{b}}_{\omega }$, GNSS‑часы $\delta t_r$, целые неоднозначности $\mathbf{N}$.
- модель дрейфа биасов: ${\dot{\mathbf{b}}}_f={\mathbf{n}}_{bf},{\dot{\mathbf{b}}}_{\omega }={\mathbf{n}}_{b\omega }$.
2) IMU‑прединтеграция (Forster et al.) — представление связей между ключевыми кадрами:
- прединтегрированные измерения $\Delta {\tilde{\mathbf{R}}}_{ij},\Delta {\tilde{\mathbf{v}}}_{ij},\Delta {\tilde{\mathbf{p}}}_{ij}$.
- остатки для оптимизации:
$$r_{\Delta p}={\mathbf{R}}_i^{\top }({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)-\Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_f)$$ и аналогично $r_{\Delta v},r_{\Delta R}$.
3) GNSS‑модели (многочастотные, многоконстелляционные)
- псевдо-дальность:
$${\rho }_i=\Vert \mathbf{p}-{\mathbf{s}}_i\Vert +c\delta t_r+T_i+I_i+{ϵ}_{\rho }$$ - carrier‑phase:
$${\varphi }_i=\Vert \mathbf{p}-{\mathbf{s}}_i\Vert +c\delta t_r+T_i-I_i+\lambda N_i+{ϵ}_{\varphi }$$ где $T$ — тропосферный, $I$ — ионосферный (для двухчастот можно его устранить), $N_i$ — целая неоднозначность.
4) Интеграция в фактор‑графе
- факторы: IMU‑прединтеграция между кадрами; GNSS‑псевдо/фазовые факторы на узлах; факторы ZUPT (если есть остановки); баро/визуальные/odometry‑факторы при наличии.
- решатель: iSAM2 для онлайн или Ceres/GTSAM для пакетной/оффлайн оптимизации в скользящем окне.
5) Разрешение неоднозначностей и устойчивость в лесу
- использовать carrier‑phase и LAMBDA‑решатель для фиксации $\mathbf{N}$ внутри окна (или PPP‑AR при отсутствии базовой станции).
- детекция и коррекция cycle‑slips: Melbourne‑Wübbena и геометрические разности; при скольжке — маркировать и ресетать соответствующие неоднозначности.
- адаптивная оценка качества наблюдений: использовать $C/N_0$, угол подъёма, азимут и карта покрытия (если доступна) для вычисления весов; отброс NLOS через пороги $C/N_0$ и согласованность фазы/дальности.
- робастная функция потерь (Huber/Tukey) или переключаемые весовые переменные (switchable constraints) для подавления мультипута и аутлаеров.
6) Поведение при длительных потерях GNSS
- при коротких потерях (секунды) --- IMU‑прогноз с увеличением ковариации; при обнаружении стационарности применять ZUPT: позиция/скорость корректируется нулевыми скоростями.
- при длительных потерях — привлекать альтернативные датчики (визуальная одометрия, LIDAR SLAM, барометр) как факторы в графе; при восстановлении GNSS — повторная оценка окна и попытка фиксации неоднозначностей.
7) Дополнительные улучшения, специфичные для леса
- многоконстелляционная и многочастотная приемка (GPS+GLONASS+Galileo+BeiDou), антенна с гроздью/шумоподавлением, установка выше уровня крон для уменьшения мультипута.
- использование карты кроны/DEM (если есть): штраф высоты/маскирование спутников за деревьями.
- модель затухания сигналов по углу и $C/N_0$ для априорного взвешивания.
- постобработка: скользящее окно в реальном времени + периодическое многопроходное сглаживание (RTS или полное фактор‑графовое сглаживание) для устранения накопленного дрейфа и повышения точности.
8) Практические параметры и ожидаемый эффект
- окно оптимизации: $10\text{–}60$ секунд (зависит от вычислений); частота IMU $100\text{–}400$ Hz; GNSS‑эпохи 1 Hz или более.
- при корректной реализации: уменьшение погрешности позиционирования в лесу по сравнению с простым GNSS/INS‑фильтром в разы (зависит от условий: мультипуть, плотность кроны, доступность фазовых измерений).
- трейд‑офф: вычисления и сложность модели (неоднозначности, robust‑факторы) vs. точность.
Короткая сводка действий для реализации:
1. Собрать многочастотные многоконстелляционные наблюдения + высокочастотный IMU.
2. Предобработать GNSS: SNR/углы, детекция NLOS, cycle‑slip.
3. Построить фактор‑граф: IMU‑прединтеграция + псевдо/фазовые факторы + при необходимости ZUPT/odometry/баро.
4. Решать скользящее окно (iSAM2) онлайн, периодически делать пакетное сглаживание и фиксирование неоднозначностей (LAMBDA).
5. При длительных потерях GNSS — использовать доп. датчики; при восстановлении — повторно оптимизировать окно и фиксировать $\mathbf{N}$.
Эта схема даёт наиболее стойкий и точный трекинг в лесистой местности при условии наличия carrier‑phase данных, многоконстелляционной приёма и корректной обработки помех.