Struct PositionNoise

Source

pub struct PositionNoise<F: Frame> { /* private fields */ }

Expand description

Noise on a position measurement, R of equation (28), on the axes of F.

A GNSS receiver’s reported accuracy is bounded on both sides before it becomes this; see PositionNoise::clamped and Eskf::fuse_gnss_position.

Implementations§

Source §

impl<F: Frame> PositionNoise<F>

Source

pub fn from_sigma(x: f32, y: f32, z: f32) -> Self

From per-axis standard deviations in meters.

Source

pub fn from_variance(x: f32, y: f32, z: f32) -> Self

From per-axis variances in meters squared — the diagonal of a covariance, as ROS messages carry it.

Source

pub fn variance(self) -> [f32; 3]

The per-axis variances, in meters squared.

Source §

impl PositionNoise<Ned>

Source

pub fn horizontal_vertical(horizontal: f32, vertical: f32) -> Self

From a receiver’s horizontal and vertical accuracy, eph and epv — standard deviations in meters, as u-blox (hAcc, vAcc), MAVLink and PX4 report them.

Also how a two-dimensional fix is expressed: give the vertical axis a σ large enough that its Kalman gain is negligible against the height uncertainty the filter already holds, and say in the calling code which it is. A kilometer is comfortably that for any vehicle this filter runs on, and is a number a reader recognizes as deliberate where 1e6 reads as arbitrary.

Unbounded on purpose, which is the whole difference from clamped: a σ standing for an axis the receiver did not measure is a sentinel, not an accuracy, and a cap would quietly turn it back into a measurement.

Source

pub fn clamped( horizontal: f32, vertical: f32, horizontal_bounds: SigmaBounds, vertical_bounds: SigmaBounds, ) -> Self

The same, with each σ held within its own SigmaBounds in meters.

For the axes the receiver actually measured. Both are capped, so this is not the way to drop one: a vertical σ of 1000 m under a 100 m cap is a height measurement the filter will lean on rather than the one being declined. A two-dimensional fix goes through horizontal_vertical instead, bounding eph first if it came from the receiver.

A receiver’s accuracy estimate is its view of its own geometry and residuals, and under multipath it stays small while the fix is meters wrong, so both production estimators bound it rather than trusting it, each axis by its own rule:

let (eph, epv) = (0.05, 0.05); // an RTK fix claiming 5 cm

// PX4 floors horizontal at EKF2_GPS_P_NOISE and vertical at 1.5 times it, and caps
// both at EKF2_NOAID_NOISE while GNSS is the only aiding on that axis.
let (p, noaid) = (0.5, 10.0);
let px4 = PositionNoise::clamped(
    eph,
    epv,
    SigmaBounds::new(p, noaid),
    SigmaBounds::new(1.5 * p, noaid),
);
assert_eq!(px4, PositionNoise::horizontal_vertical(0.5, 0.75));

// ArduPilot floors horizontal at EK3_POSNE_M_NSE and vertical at 1.5 times it, and
// caps both at 100 m.
let posne = 0.5;
let ardupilot = PositionNoise::clamped(
    eph,
    epv,
    SigmaBounds::new(posne, 100.0),
    SigmaBounds::new(1.5 * posne, 100.0),
);
assert_eq!(ardupilot, px4);

PX4: EKF/aid_sources/gnss/gps_control.cpp:358-368 for horizontal, whose cap applies only while no other source aids horizontally and whose variance is floored again at 0.01² m², and gnss_height_control.cpp:62-69 for vertical, capped only while GNSS is not the sole height source. The cap is about what the filter can afford to lean on, not about the fix. ArduPilot: constrain_ftype(gpsPosAccuracy, _gpsHorizPosNoise, 100) at AP_NavEKF3_PosVelFusion.cpp:836, the ordinary GNSS branch rather than the identical line at :807, which is the synthetic-zero-velocity case opened at :789; vertical at :1402.

Keeping the bounds here rather than in Config keeps them traveling with the measurement they describe, which is the same reason R is a per-call argument at all.

Read at PX4-Autopilot c4e4ef98 (v1.18.0-beta1) and ardupilot 368dc0c4.