Custom EKF3 Position/Wind Reset for GPS-Denied Flight (ArduPlane 4.4.3)
Hi everyone,
I’ve been working on GPS-denied flight for a VTOL QuadPlane (~15 kg, CUAV V5+) and wanted to share a custom EKF3 modification that solved the problem for us. The code is open, links at the bottom.
The problem
Stock EKF3 can’t initialize position without GPS. If there’s no GPS from boot, origin never gets set, the filter stays in AID_NONE, and you get:
- No position reporting through AHRS
- No wind state estimation (inhibitWindStates = true)
- Zero-velocity fusion fighting actual flight velocity
- No auto/guided modes
We tried feeding coordinates through MAVLink GPS input first, but that brought its own set of problems: EKF warmup conflicts, core disagreements, inability to initialize cleanly without a real GPS fix at boot. So we went deeper and modified EKF3 directly.
What we added
Two new functions in the EKF3 core, called through custom MAVLink COMMAND_INT commands:
forcePositionReset() (command 43210)
Params: lat (degE7), lon (degE7), accuracy (meters)
What it does internally:
- Sets origin if
validOriginis false (critical for cold start) - Converts lat/lon to NE offset, calls
ResetPositionNE() - Updates
lastKnownPositionNEso AID_NONE doesn’t snap back to (0,0) - Sets position covariance from accuracy parameter:
P[7][7] = sq(posAccuracy) - Switches
PV_AidingModetoAID_ABSOLUTE - Sets
_has_forced_positionflag (used in Outputs and PosVelFusion)
Key implementation in AP_NavEKF3_PosVelFusion.cpp:
bool NavEKF3_core::forcePositionReset(const Location &loc, float posAccuracy)
{
if (!validOrigin) {
if (!setOrigin(loc)) return false;
}
Vector2F posNE = EKF_origin.get_distance_NE_ftype(loc);
ResetPositionNE(posNE.x, posNE.y);
lastKnownPositionNE.x = posNE.x;
lastKnownPositionNE.y = posNE.y;
P[7][7] = sq(ftype(posAccuracy));
P[8][8] = sq(ftype(posAccuracy));
PV_AidingMode = AID_ABSOLUTE;
// First call: velocity is zero, open covariance for TAS fusion
// Repeat calls: velocity already correct, don't touch
const bool firstForce = !_has_forced_position;
_has_forced_position = true;
if (firstForce) {
zeroRows(P,4,5);
zeroCols(P,4,5);
P[4][4] = sq(ftype(25.0f));
P[5][5] = sq(ftype(25.0f));
}
posTimeout = false;
velTimeout = false;
lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;
return true;
}
Applied to all cores through NavEKF3::forcePositionReset() which iterates over core[i].
forceWindReset() (command 43211)
Params: windN (m/s), windE (m/s), accuracy (m/s)
bool NavEKF3_core::forceWindReset(float windN, float windE, float windAccuracy)
{
stateStruct.wind_vel.x = windN;
stateStruct.wind_vel.y = windE;
P[22][22] = sq(ftype(windAccuracy));
P[23][23] = sq(ftype(windAccuracy));
windStatesAligned = true;
return true;
}
Optional, but improves dead reckoning accuracy in the first few minutes after position reset. EKF will estimate wind on its own once TAS fusion kicks in.
Why AID_ABSOLUTE instead of staying in AID_NONE
This was the hardest part to get right.
In AID_NONE, the filter runs zero-velocity fusion every cycle. On the ground that’s fine, it constrains tilt drift. But in flight at 20 m/s, the filter tries to reconcile “velocity = 0” with IMU accelerations and starts corrupting pitch/roll estimates. Position goes haywire.
Switching to AID_ABSOLUTE enables wind estimation, proper TAS fusion, and dead reckoning on IMU + airspeed + compass. Without GPS data arriving, the filter dead-reckons from the forced position.
The _has_forced_position flag blocks zero-velocity fusion if the filter ever falls back into AID_NONE logic:
// AP_NavEKF3_PosVelFusion.cpp
if (fuseHgtData && PV_AidingMode == AID_NONE) {
if (assume_zero_sideslip() && tiltAlignComplete && motorsArmed) {
fusePosData = false;
if (_has_forced_position) {
// Don't fuse zero velocity after force reset.
// Let IMU + airspeed dead-reckon.
fuseVelData = false;
} else {
// Standard AID_NONE: fuse zero velocity
velPosObs[0] = 0.0f;
velPosObs[1] = 0.0f;
// ...
}
}
}
The same flag is checked in AP_NavEKF3_Outputs.cpp to ensure AHRS reports position, wind, and correct status flags even without GPS:
Outputs.cpp:182 if (PV_AidingMode == AID_NONE && !_has_forced_position)
Outputs.cpp:186 if (!inhibitWindStates || _has_forced_position)
Outputs.cpp:223 if (PV_AidingMode != AID_NONE || _has_forced_position)
Outputs.cpp:312 if (getPosD_local(posD) && (PV_AidingMode != AID_NONE || _has_forced_position))
MAVLink handler (ArduPlane side)
In ArduPlane/GCS_Mavlink.cpp, handler for COMMAND_INT:
case 43210: {
// Custom: Force EKF position reset
// param1: accuracy (m), 0 = default 5.0
// x: lat (degE7), y: lon (degE7)
Location loc;
loc.lat = packet.x;
loc.lng = packet.y;
float accuracy = packet.param1;
if (accuracy <= 0) accuracy = 5.0f;
if (AP::ahrs().EKF3.forcePositionReset(loc, accuracy)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
}
case 43211: {
// Custom: Force EKF wind state
// param1: windN (m/s), param2: windE (m/s), param3: accuracy
float windN = packet.param1;
float windE = packet.param2;
float accuracy = packet.param3;
if (accuracy <= 0) accuracy = 2.0f;
if (AP::ahrs().EKF3.forceWindReset(windN, windE, accuracy)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
}
GCS side
We use a custom Qt6/MAVSDK GCS (vtol-co-pilot). Operator clicks “Position Correction” button, clicks on map, coordinates go through sendCommandInt(). Wind override takes direction in degrees + speed, GCS converts to NED components.
The interface is simple: any companion computer or GCS can call COMMAND_INT 43210 with lat/lon/accuracy. If you have automated positioning (visual odometry, SLAM, UWB, radar beacons), you can call this automatically instead of manually.
EKF3 boot change
One more small modification: in AP_NavEKF3_core.cpp, the GPS check that prevents EKF initialization without GPS was removed for planes:
// GPS check removed for planes — allows cold start without GPS.
// EKF initialises in AID_NONE; position can be set later via
// forcePositionReset (MAVLink command 43210).
Without this, EKF3 won’t even start its state initialization without a GPS fix.
First call vs repeat calls
This caught me off guard during testing. On the first forcePositionReset(), velocity is still zero from initialization, so we open velocity covariance wide (25 m/s) to let TAS fusion build it up from scratch. On repeat calls in flight, velocity is already correct from dead reckoning, and resetting covariance would destabilize the filter for several minutes until TAS fusion recovers.
Flight test results
Tested on a ~15 kg VTOL QuadPlane, CUAV V5+, custom ArduPlane 4.4.3.
- Wind: 7-11 m/s
- Altitudes: 100-1500 m
- Distances: 1-23 km
- Position correction: manual by operator using camera payload
Dead reckoning drift between corrections: 50-150 m over 5-7 km. Drift depends mainly on wind stability. Steady wind gives much better results than gusty conditions, which makes sense since airspeed + compass dead reckoning assumes relatively stable wind.
Auto missions work after forcePositionReset(). Guided and auto modes become available, waypoint navigation functions. Operator periodically corrects position, aircraft continues the mission.
Modified files summary
AP_NavEKF3_PosVelFusion.cpp: forcePositionReset(), forceWindReset(), zero-velocity fusion blocking
AP_NavEKF3_core.h: _has_forced_position flag, function declarations
AP_NavEKF3_core.cpp: GPS boot check removal, flag initialization
AP_NavEKF3.cpp: wrapper functions iterating over all cores
AP_NavEKF3_Outputs.cpp: position/wind/status reporting with _has_forced_position
AP_NavEKF3.h: public API declarations
ArduPlane/GCS_Mavlink.cpp: COMMAND_INT handler for 43210/43211
Code
- Firmware: github.com/HubMan17/ardupilot-custom (branch custom-v2.0)
- GCS: github.com/HubMan17/vtol-co-pilot
Total modification is roughly 120 lines in the EKF3 core plus the MAVLink handler. The firmware is based on ArduPlane 4.4.3.
Has anyone else tackled GPS-denied flight on fixed-wing ArduPilot? Curious how others have approached it.
