Перейти к содержанию

Zholobov latch (soft-correction)

Кастомный патч в EKF3, который меняет реакцию на CMD 43003: первый вызов после boot — жёсткий snap, последующие — мягкая коррекция без перерасчёта velocity/attitude.

Источник: коммит d04f9d1, файлы:

  • libraries/AP_NavEKF3/AP_NavEKF3_core.h
  • libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
  • libraries/AP_NavEKF3/AP_NavEKF3.cpp (новый параметр)
  • libraries/AP_NavEKF3/AP_NavEKF3.h

Назван в честь ArtemZholobov на форуме ArduPilot, где впервые описана field-validated реализация для ArduPlane 4.4.3.

Зачем нужен

Default-поведение setLatLng — жёсткий reset позиции (ResetPositionNE). При:

  • Скорости 200 km/h (≈55 m/s)
  • Drift'е за время coast'а в 60–120 м

…это будет резкий step в state, который сольётся в step управляющего сигнала. Контроллер увидит «телепорт самолёта» и ответит резким крылом / тангажом.

Zholobov field-validated паттерн: первый snap делать только на земле (где скорость = 0, step безопасен), а в воздухе использовать мягкую коррекцию через стандартный FuseVelPosNED, но с искусственно завышенным R_OBS — чтобы Kalman gain был мал и тянул state плавно.

Жизненный цикл

boot → InitialiseVariables:  _has_forced_position = false

  ↓ первая CMD 43003 (на земле, скорость ≈ 0)

applyExtNavSoftCorrection:
  if (!_has_forced_position):
      ResetPositionNE(newPosNE)           # ← жёсткий snap
      P[7][7] = P[8][8] = sq(pos_err)     # ковариация по pos_err
      _has_forced_position = true
      _last_forced_position_ms = now

  ↓ полёт, через секунды/минуты вторая CMD 43003

applyExtNavSoftCorrection:
  else:  # _has_forced_position уже true
      drift_floor = (now - _last_forced_position_ms) * EK3_EXTNAV_DRIFT
      R_OBS[pos] = sq(MAX(pos_err, drift_floor))   # широкая R
      FuseVelPosNED(fusePosData=true, fuseVelData=false)
        # стандартный путь, но с большой R → малый Kalman gain
        # ResetPositionNE НЕ вызывается
      _last_forced_position_ms = now

Параметр EK3_EXTNAV_DRIFT

Новый параметр, добавлен в libraries/AP_NavEKF3/AP_NavEKF3.cpp (idx 12):

Поле Значение
Тип float
Default 1.0
Range 0.1 .. 5.0
Единицы m/s
Описание Expected NE position drift rate, used to floor R_OBS on repeat setLatLng calls

Физический смысл: «насколько метров за секунду я ожидаю что борт мог уйти от истины». Используется как floor для R на повторных коррекциях:

R_floor = (time_since_last_correction_seconds) × EK3_EXTNAV_DRIFT
R_OBS  = MAX(pos_err_from_command, R_floor)²

Что это значит:

  • 1.0 (default) — консервативно, основано на оценке Tridge для среднекалиброванного airspeed. Безопасно по умолчанию.
  • 0.5 — если ARSPD_RATIO откалиброван хорошо (≤ 0.5 m/s drift).
  • 2.0+ — если ARSPD откалиброван плохо или ожидается сильный ветер.

Чем больше EK3_EXTNAV_DRIFT → тем шире R → тем меньше Kalman gain → тем мягче коррекция. Чрезмерно большое значение приводит к тому что коррекция почти не двигает state.

Source-гейт

// AP_NavEKF3_PosVelFusion.cpp:209-217
#if EK3_FEATURE_EXTERNAL_NAV
    {
        const AP_NavEKF_Source::SourceXY posxy_source = frontend->sources.getPosXYSource();
        if (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV ||
            posxy_source == AP_NavEKF_Source::SourceXY::BEACON) {
            return applyExtNavSoftCorrection(loc, posAccuracy, timestamp_ms);
        }
    }
#endif

Soft-correction путь активен только при SRC1_POSXY ∈ {4, 6}. На GPS-конфигурации (SRC=3) выполняется legacy hard-snap с V1.2.0-восстановленным AID_NONE guard'ом. Это защита defence-in-depth: случайная CMD 43003 на GPS-самолёте не повредит healthy fix.

Что значит «не трогает velocity/attitude»

В soft-correction пути:

  • FuseVelPosNED(fusePosData=true, fuseVelData=false) — фьюзится только позиция, не скорость.
  • Reset-trigger'ы (posTimeout, posVarianceIsTooLarge, velTimeout → ResetVelocity) отключены в этой ветке кода.
  • Wind states treatWindStatesAsTruth=true сохраняется (если он был — см. common/wind).

Эффект: позиция плавно подтягивается к новой коррекции, velocity и attitude остаются как были. Контроллер не видит step.

Telemetry/Debug

Латч прозрачен в логах — applyExtNavSoftCorrection использует тот же FuseVelPosNED, что и стандартный путь. В .bin логе:

  • XKF1.PN/PE — позиция: на первом вызове увидишь step, на последующих — плавную кривую.
  • XKF1.PNV/PEV — variance: на первом вызове падает до sq(pos_err), на последующих — на короткое время повышается (через R_OBS_floor).
  • Сообщения DAL — каждый успешный setLatLng логируется.

В коде нет специального счётчика _has_forced_position который пишется в лог — это только в-памяти state. Если нужен post-flight анализ числа soft-correction'ов — считать по DAL-записям setLatLng (первая = snap, остальные = soft).

Известные ограничения

  1. _has_forced_position сбрасывается только в InitialiseVariables() (init или re-init core). Это значит:
  2. Если core лопнет и переинициализируется — следующая CMD 43003 снова сделает hard snap. Это редкий путь, но возможен.

  3. В нормальной работе (boot → flight → land) state живёт от boot до boot.

  4. R_OBS floor расширяется через GPS-accuracy ветку (clamp [_gpsHorizPosNoise, 100m]), а не через ExtNav-ветку (clamp 10m). Это намеренно: 10m clamp слишком мал для legitimate drift'а после 120s coast'а (1 m/s × 120 = 120 m). См. длинный комментарий в коде о routing-note.

  5. Без treatWindStatesAsTruth (если ветер не заморозился) soft-correction может медленно мигрировать в ветер вместо позиции. На практике treatWindStatesAsTruth латчится автоматически когда dead_reckoning=true — это покрывает Scenario B (long coast).