Indoor altitude: why EKF height sits below the rangefinder
Three logs from the same aircraft (DAKEFPVH743, board ID 002B0019 35335116 33333431):
| log |
firmware |
flight |
guidetest.bin |
ArduCopter 4.7.0 (1511f271) |
Loiter, then a Stabilize takeoff attempt |
guidetest2.bin |
ArduCopter 4.7.1 (dbe79216) |
Guided, 1.0 m takeoff, 5 min hover |
guidetest3.bin |
ArduCopter 4.7.1 (dbe79216) |
Guided, 1.2 m takeoff, 2.5 min hover |
Relevant setup, identical across all three: RC7_OPTION=90 with RCIN.C7=1500 throughout, so the EKF is on source set 2 for the whole of every log. Set 2 is POSXY=None, VELXY=OpticalFlow, POSZ=RangeFinder, VELZ=None. Rangefinder and flow both arrive over MAVLink (RNGFND1_TYPE=10, FLOW_TYPE=5), with RNGFND1_MAX=3.0 and RNGFND1_GNDCLR=0.10.
The rangefinder is not being ignored
The EKF is on the rangefinder and fusing it continuously in all three logs. In steady hover it tracks with a fixed offset:
| log |
EKF alt - RFND.Dist, steady hover |
samples |
| guidetest |
-0.141 m (sd 0.099) |
4521 |
| guidetest2 |
-0.079 m (sd 0.053) |
5916 |
| guidetest3 |
-0.104 m (sd 0.062) |
2832 |
That is RNGFND1_GNDCLR = 0.10. EKF altitude is height above the takeoff point; the rangefinder measures sensor to floor. This part is correct behaviour and is not the problem.
The failure is dynamic, and the driver is the accelerometer.
The accelerometer under-reads gravity whenever the motors run
Specific force is |a - g|, so any real acceleration can only make the measured magnitude larger than g. A measured magnitude below g is therefore an unambiguous sensor under-read, and the size of it is a lower bound. Comparing the aircraft sitting still with props stopped against steady hover (airborne, rangefinder height steady):
| log |
props off and static |
steady hover |
motors-on component |
| guidetest |
-0.082 (n=1456) |
-0.165 (n=5839) |
+0.083 |
| guidetest2 |
-0.120 (n=458) |
-0.178 (n=7425) |
+0.059 |
| guidetest3 |
-0.088 (n=321) |
-0.215 (n=3627) |
+0.127 |
Values are |acc| - 9.807 in m/s2, IMU0. The static column is the ordinary accelerometer calibration bias, which the EKF learns and removes. The third column is the part that only exists while the props turn, and it comes back to the static value after disarm.
That is vibration rectification: a DC offset in AccZ caused by motor vibration. VIBE peaks at 27.7 (X) and 13.9 (Z) with zero clip events, and no harmonic notch is configured (INS_HNTCH_ENABLE=0, INS_HNTC2_ENABLE=0).
The EKF reads that offset as a real downward acceleration.
Nothing in this configuration can reject it
Source set 2 has EK3_SRC2_VELZ=0 and VELXY=5, so there is no vertical velocity observation at all. XKF4.TS=50 (velocity timeout bit set) for the whole of every log confirms it. velD is pure accelerometer integration, and the only thing opposing it is the rangefinder height fusion.
That fusion is weak. With EK3_RNG_M_NSE=0.5 the observation variance is 0.25 m2 against a position-down state variance (XKV1.V09) of about 0.018 m2, so the Kalman gain is roughly 7 percent per update. It cannot outrun the accelerometer error.
Worst case, and why nothing is flagged
guidetest.bin, t=439 to 443.2 s. The aircraft is sitting on the ground at 1.4 to 8 percent throttle (MOT_THST_HOVER=0.12), rangefinder pinned at its 0.02 m floor:
| t (s) |
RFND.Dist |
EKF alt |
XKF1.VD |
XKF3.IPD |
XKF4.SH |
BARO.Alt |
| 438.1 |
0.02 |
-0.01 |
0.00 |
0.00 |
0.00 |
1.32 |
| 440.1 |
0.02 |
-0.24 |
0.44 |
0.17 |
0.06 |
-3.66 |
| 441.1 |
0.02 |
-0.67 |
0.88 |
0.49 |
0.19 |
-5.65 |
| 442.1 |
0.02 |
-1.17 |
1.21 |
0.97 |
0.37 |
-7.98 |
| 443.1 |
0.02 |
-1.59 |
1.29 |
1.32 |
0.51 |
-4.95 |
Fitted over that window, the EKF’s velD ramps at +0.345 m/s2 and the altitude falls 1.58 m in 4.1 s, while the aircraft is parked on the floor. That ramp is the net rate after the height fusion has already clawed some of it back.
The rangefinder innovation reaches 1.32 m and still passes its gate (XKF4.SH=0.51 against the 5 sigma EK3_HGT_I_GATE=500), so there is no EKF failsafe, no lane switch, no status message. Nothing in the log announces this.
The phantom sink also fooled the land detector, which logged NOT_LANDED at 439.7 s at 1.4 percent throttle.
What the pilot sees
Both Guided takeoffs overshoot by roughly 2x, because the EKF enters the climb already low and lagging:
| log |
commanded |
true peak (RFND) |
EKF reported peak |
| guidetest2 |
1.0 m |
1.91 m |
1.29 m |
| guidetest3 |
1.2 m |
2.04 m |
1.47 m |
The barometer, separately
The baro is unusable in this space. Spool-up drives it to -11.5 m (guidetest), -11.2 m (guidetest2) and -10.7 m (guidetest3), and it drifts about +2 m high in hover. It stays out of the height solution while on source set 2, but it will be the height source on set 1 or set 3 (EK3_SRC1_POSZ=1), and on any rangefinder dropout the EKF falls back to it. EK3_GND_EFF_DZ=+4 currently makes the filter trust the baro more, which is the wrong direction for a baro this dirty.
What to change now, on stock 4.7.x
Lower EK3_RNG_M_NSE from 0.5 to about 0.15. That takes the height fusion gain from roughly 7 percent to roughly 45 percent per update, so the rangefinder can outrun the accelerometer error. It is the cheapest change and it targets the symptom directly. This rangefinder is already smooth and filtered, so the usual objection (chasing range noise) is weak here.
Then deal with the vibration, which is the actual source. ESC RPM telemetry is working (hover around 12,400 RPM, so a 207 Hz fundamental) and no notch is configured, so an RPM-tracked harmonic notch will clean up the control loops. The rectification itself is mechanical, so flight controller soft-mounting and prop balance are what move the numbers in the accelerometer table above.
Which fixes address this
These are open ArduPilot PRs, carried in the Small Fast Drone branch:
- #32471, hover Z-bias learning for vibration rectification. This is the primary one. It learns
INS_ACC_VRFB_Z per IMU during hover and applies it only while the motors are running, which is exactly the shape of the error measured above. It ships a SITL model of the fault (SIM_ACC_VRF, a motors-on-only accel offset) and autotests.
- #33478, fuse AGL KF velocity as a velD observation (
EK3_OPTIONS bit 5). This supplies the observation that source set 2 is missing. It anchors velD to the rangefinder and makes the Z accel bias observable.
- #33507, estimate accel-Z bias in the AGL KF. Estimates the bias inside the rangefinder-aided filter rather than waiting for the main filter’s slow bias state.
- #33359, use the AGL KF for the rangefinder height switch and observation (
EK3_OPTIONS bit 4). Fuses a de-glitched, IMU-aided AGL height instead of raw range, which removes the climb lag visible in the takeoff table.
- #32972, protect height fusion from baro ground effect at takeoff. Suppresses the height reset during ground effect, adds a negative
EK3_GND_EFF_DZ as a baro noise floor, and holds the pre-takeoff height through spool-up. Needed for the -11 m excursions if this aircraft ever flies on set 1 or 3.
- #32232, ground clearance fusion fix (
EK3_OPTIONS bit 3). Holds the on-ground rangefinder assumption until takeoff is detected rather than until onGround clears, which matters here because the land detector was fooled.
Caveats
The mechanism is measured rather than inferred: three logs, an attitude-independent accelerometer check, and the innovation arithmetic reproduces the logged XKF4.SH to two decimal places. But two things are worth stating plainly.
The motors-on accelerometer error is measured cleanly in hover (+0.06 to +0.13 m/s2). During the spool-up window it appears larger, but the aircraft is rocking on its legs there, so |acc| = g no longer holds and that larger figure is not trustworthy. The event-level number to rely on is the measured velD ramp of +0.345 m/s2, which needs no assumption about the accelerometer at all.
That #32471 removes the divergence on this airframe is a prediction, not a result. It has not been A/B tested against these conditions. Doing that in SITL with SIM_ACC_VRF set to the measured 0.06 to 0.13 m/s2 is the obvious next step.
Reproducing the checks
Note that the logs do not share a time base: guidetest.bin and guidetest2.bin report raw TimeUS, while guidetest3.bin is offset by 57.6 s. The checks below either state which log they apply to or select their own windows from the data.
# the divergence, guidetest.bin
log_extract.py compare guidetest.bin \
--sources "RFND.Dist,-XKF1.PD,XKF1.VD,XKF3.IPD,XKF4.SH,BARO.Alt" \
--interval 0.5 --from-time 436 --to-time 456
# the accelerometer check, window-free: compare |acc| where the props are stopped
# and the aircraft is still (gyro < 0.01 rad/s, rangefinder at its floor) against
# steady hover (airborne, rangefinder height steady). In both states the true
# acceleration is zero, so |acc| must equal 9.807.
# confirms there is no velocity observation: XKF4.TS bit 1 set throughout
log_extract.py extract guidetest.bin --types XKF4 --fields C,TS,SS --condition "XKF4.C==0"
