mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-07-27 00:48:23 +08:00
3ebc6d6a40
* fix(ekf2): report HAGL variance for dist_bottom_var dist_bottom_var was published as P(terrain, terrain) only. When the range finder is the EKF height reference (EKF2_HGT_REF=2), the terrain state is reset to 0 and not directly observed, so this variance does not represent the actual uncertainty of dist_bottom (HAGL). Use ComputeHaglInnovVar(P, 0) to report the proper HAGL variance, which accounts for terrain state, vertical position state, and their covariance. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * fix(ekf2): clamp HAGL variance to non-negative H*P*H^T should be >= 0 for a valid covariance, but float drift or mildly non-PSD P can produce a tiny negative result. Clamp at the accessor so consumers of lpos.dist_bottom_var never see a negative variance. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> --------- Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>
89 lines
4.7 KiB
Plaintext
89 lines
4.7 KiB
Plaintext
# Fused local position in NED.
|
|
# The coordinate system origin is the vehicle position at the time when the EKF2-module was started.
|
|
|
|
uint32 MESSAGE_VERSION = 1
|
|
|
|
uint64 timestamp # time since system start (microseconds)
|
|
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
|
|
|
bool xy_valid # true if x and y are valid
|
|
bool z_valid # true if z is valid
|
|
bool v_xy_valid # true if vx and vy are valid
|
|
bool v_z_valid # true if vz is valid
|
|
|
|
# Position in local NED frame
|
|
float32 x # North position in NED earth-fixed frame, (metres)
|
|
float32 y # East position in NED earth-fixed frame, (metres)
|
|
float32 z # Down position (negative altitude) in NED earth-fixed frame, (metres)
|
|
|
|
# Position reset delta
|
|
float32[2] delta_xy # Amount of lateral shift of position estimate in latest reset (in x and y) [m]
|
|
uint8 xy_reset_counter # Index of latest lateral position estimate reset
|
|
float32 delta_z # Amount of vertical shift of position estimate in latest reset [m]
|
|
uint8 z_reset_counter # Index of latest vertical position estimate reset
|
|
|
|
# Velocity in NED frame
|
|
float32 vx # North velocity in NED earth-fixed frame, (metres/sec)
|
|
float32 vy # East velocity in NED earth-fixed frame, (metres/sec)
|
|
float32 vz # Down velocity in NED earth-fixed frame, (metres/sec)
|
|
float32 z_deriv # Down position time derivative in NED earth-fixed frame, (metres/sec)
|
|
|
|
# Velocity reset delta
|
|
float32[2] delta_vxy # Amount of lateral shift of velocity estimate in latest reset (in x and y) [m/s]
|
|
uint8 vxy_reset_counter # Index of latest vertical velocity estimate reset
|
|
float32 delta_vz # Amount of vertical shift of velocity estimate in latest reset [m/s]
|
|
uint8 vz_reset_counter # Index of latest vertical velocity estimate reset
|
|
|
|
# Acceleration in NED frame
|
|
float32 ax # North velocity derivative in NED earth-fixed frame, (metres/sec^2)
|
|
float32 ay # East velocity derivative in NED earth-fixed frame, (metres/sec^2)
|
|
float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2)
|
|
|
|
float32 heading # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians)
|
|
float32 heading_var
|
|
float32 unaided_heading # Same as heading but generated by integrating corrected gyro data only
|
|
float32 delta_heading # Heading delta caused by latest heading reset [rad]
|
|
uint8 heading_reset_counter # Index of latest heading reset
|
|
bool heading_good_for_control
|
|
|
|
float32 tilt_var
|
|
|
|
# Position of reference point (local NED frame origin) in global (GPS / WGS84) frame
|
|
bool xy_global # true if position (x, y) has a valid global reference (ref_lat, ref_lon)
|
|
bool z_global # true if z has a valid global reference (ref_alt)
|
|
uint64 ref_timestamp # Time when reference position was set since system start, (microseconds)
|
|
float64 ref_lat # Reference point latitude, (degrees)
|
|
float64 ref_lon # Reference point longitude, (degrees)
|
|
float32 ref_alt # Reference altitude AMSL, (metres)
|
|
|
|
# Distance to surface
|
|
bool dist_bottom_valid # true if distance to bottom surface is valid
|
|
float32 dist_bottom # Distance from from bottom surface to ground, (metres)
|
|
float32 dist_bottom_var # height above ground estimate variance (m^2)
|
|
|
|
float32 delta_dist_bottom # Amount of vertical shift of dist bottom estimate in latest reset [m]
|
|
uint8 dist_bottom_reset_counter # Index of latest dist bottom estimate reset
|
|
|
|
uint8 dist_bottom_sensor_bitfield # bitfield indicating what type of sensor is used to estimate dist_bottom
|
|
uint8 DIST_BOTTOM_SENSOR_NONE = 0
|
|
uint8 DIST_BOTTOM_SENSOR_RANGE = 1 # (1 << 0) a range sensor is used to estimate dist_bottom field
|
|
uint8 DIST_BOTTOM_SENSOR_FLOW = 2 # (1 << 1) a flow sensor is used to estimate dist_bottom field (mostly fixed-wing use case)
|
|
|
|
float32 eph # Standard deviation of horizontal position error, (metres)
|
|
float32 epv # Standard deviation of vertical position error, (metres)
|
|
float32 evh # Standard deviation of horizontal velocity error, (metres/sec)
|
|
float32 evv # Standard deviation of vertical velocity error, (metres/sec)
|
|
|
|
bool dead_reckoning # True if this position is estimated through dead-reckoning
|
|
|
|
# estimator specified vehicle limits
|
|
# set to INFINITY when limiting not required
|
|
float32 vxy_max # maximum horizontal speed (meters/sec)
|
|
float32 vz_max # maximum vertical speed (meters/sec)
|
|
float32 hagl_min # minimum height above ground level (meters)
|
|
float32 hagl_max_z # maximum height above ground level for z-control (meters)
|
|
float32 hagl_max_xy # maximum height above ground level for xy-control (meters)
|
|
|
|
# TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position
|
|
# TOPICS estimator_local_position
|