Архитектура (код 4.7)¶

Все доработки живут в AP_NavEKF3, AP_AHRS (включая AP_AHRS_DCM) и ArduPlane/ekf_check.cpp. Активны только при ExtNav-источнике / операторском фиксе — на GPS-сборках поведение стоковое.

1. Холодный старт без GPS (init-байпас)¶

NavEKF3_core::InitialiseFilterBootstrap() у стока отказывается стартовать для самолёта (assume_zero_sideslip()) без 3D-фикса GPS. Байпас: если EK3_SRC1_POSXY = ExtNav или Beacon, проверка GPS пропускается — фильтр поднимается, борт может армиться.

if (assume_zero_sideslip() && gps_status < FIX_3D) {
    if (posxy_source != EXTNAV && posxy_source != BEACON) { ...fail "No GPS lock"... }
}

2. Origin и операторский фикс¶

SET_GPS_GLOBAL_ORIGIN задаёт начало координат без GPS (setOriginLLH).
Операторский фикс CMD 43003 идёт через AP_AHRS::handle_external_position_estimate() в оба эстиматора: EKF3.setLatLng() и dcm.set_dead_reckon_position() — DCM всегда держит свежую позицию и готов принять управление.
Первый успешный setLatLng() ставит _has_forced_position = true.
readyToUseExtNav() = tiltAlignComplete && (extNavDataToFuse || _has_forced_position) — то есть одного operator-фикса достаточно, чтобы войти в AID_ABSOLUTE без непрерывного потока ExtNav.
moveEKFOrigin() управляет переносом origin; под операторской навигацией он не «рецентрится» на борт каждый кадр.
Время инъекции запоминается (get_last_ext_nav_injection_ms): AUTO-взлёт без GPS требует фикс свежее последнего disarm (ArduPlane/takeoff.cpp).

3. Гейт коаста¶

В setAidingMode() под _has_forced_position подавлены оба критических таймаута:

if (!attAiding && !_has_forced_position) { ...attAidLossCritical... }   // не валимся в AID_NONE
if (!posAiding && !_has_forced_position) { ...posAidLossCritical...  }   // не ставим posTimeout

Без этого после ~10 с без свежего фикса стоковая логика гасит horiz_pos_abs, AHRS откатывается на DCM (0,0). С гейтом позиция остаётся валидной, а растущая ковариация честно отражает падение точности. Между фиксами скорость держится синтетическим sideslip-фьюжном (ПВД + курс).

4. DCM-навигация: счисление и 3-state KF¶

DCM ведёт собственное полноценное счисление (AP_AHRS_DCM): якорь позиции (lat/lng последнего фикса) + интеграл скорости v = TAS·K_tas·(курс) + ветер. Поверх него работает дискретный рекурсивный фильтр Калмана с 3 состояниями:

W_N, W_E — компоненты ветра (м/с);
K_tas — масштабный коэффициент ПВД (засорение/обледенение трубки, ошибка ARSPD_RATIO).

Модель окна измерения. Между фиксами накапливаются интегралы: время окна dt, интегралы немасштабированной воздушной скорости S_N, S_E и длина воздушного пути. Невязка очередного фикса относительно счисленной позиции — это измерение z = H·x с H = [dt 0 S_N; 0 dt S_E]: ошибка ветра входит пропорционально времени, ошибка масштаба — пропорционально пройденному воздушному пути. Стартовая ковариация: ветер σ=5 м/с (свободно обучаем первыми фиксами), K_tas σ=3% с малым шумом процесса (медленный параметр среды).

Вес фикса. Шум измерения R = acc² берётся из param3 команды 43003 (зажим 2–100 м; не задан → 5 м): точный стартовый фикс тянет фильтр сильно, грубый операторский клик — слабо.

Компенсация задержки фикса. Счисление каждые ~0,2 с снимает снапшоты (позиция + интегралы окна) в кольцевой буфер глубиной ~10 с. Фикс с валидным timestamp (param1 43003, либо подставленный из AHRS_DR_FIXLAG) возрастом до 8 с сравнивается с оценкой на момент наблюдения, а якорь позиции проносится вперёд на счисленный с тех пор путь. Без timestamp (и при AHRS_DR_FIXLAG=0) фикс считается текущим. Повторная доставка того же фикса (ретрансмит MAVLink) распознаётся по timestamp и игнорируется; фикс старше буфера снапшотов отбрасывается с предупреждением DR fix too old.

Гейт выбросов (χ², 2 ст.св., ~99%). Фикс с махаланобисовым расстоянием > 9.21:

не обновляет состояния ветра/масштаба (печатается DR fix outlier), но пересаживает якорь позиции — оператор остаётся авторитетным источником позиции;
после 3 отказов подряд фикс принимается с ослабленным весом (R, раздутый в 4 раза, сообщение DR fix deweighted accept): устойчивое расхождение — это дивергенция фильтра, и оператор обязан мочь вытянуть его обратно;
гейт взводится только после 2 информативных окон (принятые обновления с dt ≥ 10 с) либо сразу после операторского ветра CMD 43004 — до сходимости большие невязки легитимны, и гейтить их нельзя.

Режимы обучения.

Ветер/масштаб обучаются только в полёте (armed, окно 1–600 с): на земле модель счисления невалидна (стоящий самолёт не сносит ветром, а ПВД на стоянке «дует»). Наземные фиксы только пересаживают якорь.
K_tas обучается только на манёвренных окнах: на прямом окне столбец K_tas в H параллелен ветровым (масштаб впитал бы продольный снос как ложное изменение шкалы), поэтому при «прямизне» окна > 0.92 и на этапе незрелого фильтра столбец замораживается. K_tas зажат в 0.5–1.5.
Выученный ветер клампится по модулю к AHRS_WIND_MAX (если задан).
CMD 43004 (операторский ветер) пишет ветер, сжимает его ковариацию до заданной точности и перезапускает окно (restart_dr_window()): часть окна, пролетевшая со старым ветром, исказила бы следующее обновление. Смена активного эстиматора (EKF3↔DCM) тоже перезапускает окно — буферы копятся в yaw-фрейме активного ядра, смешивать два фрейма нельзя.

Состояние фильтра видно в логе (DCM.VWN/VWE/Kts/KFa) и в GCS-сообщениях KF Update: W_N=... W_E=... K_tas=....

5. Ветер EKF3 (ADR-окно)¶

Заморозка: когда ветер ненаблюдаем (нет GPS/потока), treatWindStatesAsTruth зануляет ковариацию/шум ветровых состояний — записанный ветер «держится» между коррекциями.
ADR-оценка (air-data dead-reckoning). Окно оценки ветра расцеплено с каденсом коррекций: оно открывается на операторском фиксе (якорь _windAnchor*), непрерывно копит интеграл воздушного перемещения и закрывается по геометрическому условию (целое число витков / выпрямленный курс / потолок EK3_WIND_TWMAX), а не на каждом фиксе. На закрытии ветер вычисляется точно: wind = (groundDisp − airDisp)/win, где groundDisp — между двумя операторскими фиксами (скорости EKF не участвуют — оценка разомкнутая и не может прецессировать). Обновление вмешивается EMA с гейном, взвешенным качеством закрытия (EK3_WIND_EMA/QTOL/QFLR); закрытие с незамкнувшимся воздушным путём ветируется.
Bootstrap на наборе. Пока ветер ни разу не оценён, короткие окна ≥ 8 с обновляют его на каждом фиксе (окно при этом не пересаживается — каждое следующее обновление пересчитано по тому же растущему окну и замещает предыдущее): операторская серия фиксов на наборе высоты даёт грубый ветер уже в наборе, а не нулевой ветер в крейсере. Первое штатное закрытие окна завершает bootstrap.
Ре-арм в воздухе. Disarm гасит ветровые состояния; при ре-арме восстанавливается удержанная ADR-оценка — но только если она моложе 300 с. Более старая (посадка → долгая пауза → новый арм) отбрасывается целиком с сообщением EKF3 IMUx ADR wind stale, relearning: ветер мог смениться, и нулевой посев с повторным bootstrap безопаснее.
Ручная установка: CMD 43004 → set_external_wind_estimate() пишет прямо в stateStruct.wind_vel (на земле — отложенно, применится при активации ветровых состояний на взлёте) и синхронизирует ADR-оценку, чтобы дальнейшее обучение продолжалось от операторского значения.
Режим Hold: EK3_WIND_ADR=0 подавляет запись ADR-оценки в фильтр — ветер держится на текущем (например, операторском) значении, а вычисленная оценка публикуется в GCS как рекомендация (ADR wind rec: ...).

6. Деклинация из origin¶

checkUpdateEarthField(): если компас не scale-калиброван (have_scale_factor()==false) и есть операторский origin, таблица WMM засевается из него (GPS-denied эквивалент GPS-автосклонения) и запрашивается одиночный yaw-reset. Иначе курс был бы смещён на локальное склонение.

7. Монитор целостности `FS_EKF_FWINT`¶

ArduPlane/ekf_check.cpp, 10 Гц, полностью self-gated (opt-in, fixed-wing, armed, не фьюзит GPS):

сверяет EKF3 (крен/тангаж/курс, высота) против независимых DCM и сырого baro;
при грубом расхождении (> FS_EKF_FWANG° по углам или > FS_EKF_FWHGT м по высоте), удержанном ~1 с, форсит AHRS на DCM (независимый горизонт и счисление, засеянные из CMD 43003);
латчится до восстановления; на GPS-сборках не активируется никогда.