Skip to content

AP_NavEKF3: average the flow samples between measurements - #35308

Open
andyp1per wants to merge 2 commits into
ArduPilot:masterfrom
andyp1per:pr-ekf3-flow-average
Open

andyp1per wants to merge 2 commits into
ArduPilot:masterfrom
andyp1per:pr-ekf3-flow-average

Conversation

@andyp1per

Copy link
Copy Markdown
Contributor

Summary

If your optical flow sensor runs faster than 20 Hz, as many current ones do (50-66 Hz over DroneCAN, 100 Hz over MAVLink, around 200 Hz on some), EKF3 keeps one sample in each 50 ms and discards the rest. It treats the sample it keeps as the rate averaged since the last one, but it carries one sample's noise, so a 100 Hz sensor's noise reaches the filter undiminished and the velocity estimate wanders with it.

This averages the usable samples received between measurements, with the sensor's body rates over the same samples. Discarding four samples in five is the defect, so it is on for every sensor that gives more than one sample per measurement; a 10 Hz sensor gives one and is unchanged.

Classification & Testing (check all that apply and add your own)

  • Checked by a human programmer
  • Automated test(s) verify changes (e.g. unit test, autotest)
  • Tested manually, description below (e.g. SITL)
  • Logs available on request

EK3_FlowSamplesAveraged hovers on flow at 10 Hz and at 100 Hz. Over three runs the 100 Hz flow innovation spread is 24.1-27.6 mrad/s, where five samples averaged predicts 25.8 and a single sample gives 57.7; at 10 Hz it stays at 56.5-58.1. Without the change the 100 Hz half reads 57.7 and fails.

In a SITL flow-only hover with 0.05 rad/s of sensor noise, velocity error falls from 0.057 to 0.038 m/s rms at 100 Hz and the drift from 0.27 to 0.08 m; at 66 Hz from 0.051 to 0.038 m/s; at 10 Hz nothing changes.

423 real flight logs replayed (through #35306, so that logs from before 4.7 replay their range finder): the flow innovation spread falls on 270 lanes and rises on 79 (median ratio 0.87), and the time the flow disagrees with the velocity by more than 1 m/s falls by 16%.

No parameters change, and EK3_FLOW_M_NSE keeps its default: it covers the flow's scale, height and timing errors as well as the sensor's noise, and averaging reduces only the noise. Replaying the same logs with it lowered to 0.15 gives slightly lower GPS error (p95 summed over 255 cores falls by 17 m/s, lower on 84 and higher on 51) but 26% more single-axis lockouts, and at 0.10 both are worse.

Description

Detail for reviewers, not needed to review the change in outline.

  • The sum starts again after a gap longer than a measurement interval, so samples from before a dropout are not averaged in. Samples failing the existing quality and rate checks stay out of the average, and an interval with none writes nothing, as a rejected sample did before; its body rates still feed the gyro bias estimate, clamped as before.
  • Samples are weighted equally: the EKF knows when each one arrived, not the interval the sensor integrated it over, and the arrival times carry the scheduler's jitter rather than the sensor's.
  • The sensor's body rates are averaged over the same samples, so the gyro bias estimate compares them with the IMU's rate over the same window (delAngBodyOF) rather than one sample with a 50 ms average.
  • The time stamp keeps its fixed back-dating of flowTimeDeltaAvg_ms/2, 50 ms, set for a 10 Hz PX4Flow. A single sample from a 100 Hz sensor centres about 5 ms before it arrives, so it was already stamped about 45 ms too old, which EK3_FLOW_DELAY cannot take back as it does not go negative; an average of its samples centres about 25 ms back, so the change halves that error. Making the back-dating follow the sensor's rate is a separate change.
  • A sensor near 20 Hz whose samples jitter across the 50 ms gate now averages the one held with the next where it used to drop it.
  • The samples are averaged in the sensor frame without rotating them to a common one. Across the logs the vehicle yaws at most 5.3 deg in a 50 ms window (p99 2-3.6 deg), and the plain average differs from one rotated into the window's centre frame by p99 0.2-2% of the flow (max 5.5%); roll and pitch rates are removed by the body-rate compensation, which is linear and averages exactly. A prototype that rotates each sample by the IMU's yaw since the last measurement, replayed over the same logs, leaves the flow innovation spread where it was (median ratio 1.000, 0.996-1.005 for 80% of lanes), so the plain average stays.
  • Every sample reaches the EKF through the DAL, and ROFH changes with each one (it carries the sample's time), so Replay sees and averages the same samples. The flow calibrator reads the samples the EKF fuses (flowCalSample), so it now sees the averages, on both axes alike.
  • PX4Flow calls into the EKF from its I2C thread on master, so the sums share that thread with the rest of writeOptFlowMeas()'s state; AP_OpticalFlow: publish PX4Flow frames from the main loop #34783 moves it to the main loop.
  • The one large regression in the replays is a flow lane that lands with a residual velocity and then rejects all flow on the ground, where the height is clamped to the range finder's ground clearance and a few cm/s of error predicts several rad/s of flow. That happens on 122 lanes with or without this change, which flips 14 of them, 9 one way and 5 the other.

EKF3 takes a flow measurement at most every 50 ms and treats it as the
rate averaged since the last one, but a sensor faster than that had all
but one sample of each interval discarded, so its noise reached the
filter undiminished. Average the usable samples instead, with the flow
sensor's body rates over the same samples. A 10 Hz sensor gives one
sample per measurement and is unchanged.
A 100 Hz SITL flow sensor's innovations in a hover show the noise of
its samples averaged, under 0.6 of one sample's, and a 10 Hz sensor's
show one sample's, which the limit is set against. Over three runs
24.1-27.6 mrad/s at 100 Hz and 56.5-58.1 at 10 Hz against a single
sample's 57.7; without the averaging 57.7 at 100 Hz.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant