fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 20:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

LPE Variance Dependent Publication (#4914)

Browse Source 
* Use variance to control publishing for LPE.

* Don't stop publishing if we have gps/ baro.

* LPE tuning and cleanup.

* Added bias saturation to LPE.

* Added vector enabled low pass filter block.

* Added rk4 integration and pub lowpass to LPE.

* Fix std::abs issue on mac/ reset lowpass on state reset.

* Don't estimate gyro bias when rotating at high speed  att_est_q.

* Lowered low pass on position to 5 Hz for LPE.

* Streamline state space update for LPE.

* Added health flags to est2 log.

* Revert to old tuning, more conservative, less faults.

* Formatting.

* Fix for fault message on LPE.

* Added subscription throttling to LPE.

* Formatting.
This commit is contained in:
James Goppert
2016-07-01 11:43:09 -04:00
committed by GitHub
parent 2a395c3fec
commit 00dfc99e08
17 changed files with 417 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/params.c
+36 -20
View File
@@ -88,34 +88,32 @@ PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);
/**
* Accelerometer xy standard deviation
* Accelerometer xy noise density
*
* Data sheet sqrt(Noise power) = 150ug/sqrt(Hz)
* std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2
* Since accels sampled at 1000 Hz.
* Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz)
*
* should be 0.0464
* Larger than data sheet to account for tilt error.
*
* @group Local Position Estimator
* @unit m/s^2
* @unit m/s^2/srqt(Hz)
* @min 0.00001
* @max 2
* @decimal 4
*/
PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0015f);
/**
* Accelerometer z standard deviation
* Accelerometer z noise density
*
* (see Accel x comments)
* Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz)
*
* @group Local Position Estimator
* @unit m/s^2
* @unit m/s^2/srqt(Hz)
* @min 0.00001
* @max 2
* @decimal 4
*/
PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0015f);
/**
* Barometric presssure altitude z standard deviation.
@@ -145,11 +143,11 @@ PARAM_DEFINE_INT32(LPE_GPS_ON, 1);
* @max 0.4
* @decimal 2
*/
PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.29f);
/**
* GPS xy standard deviation.
* Minimum GPS xy standard deviation, uses reported EPH if greater.
*
* @group Local Position Estimator
* @unit m
@@ -157,10 +155,10 @@ PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
* @max 5
* @decimal 2
*/
PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
PARAM_DEFINE_FLOAT(LPE_GPS_XY, 1.0f);
/**
* GPS z standard deviation.
* Minimum GPS z standard deviation, uses reported EPV if greater.
*
* @group Local Position Estimator
* @unit m
@@ -168,10 +166,11 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
* @max 200
* @decimal 2
*/
PARAM_DEFINE_FLOAT(LPE_GPS_Z, 100.0f);
PARAM_DEFINE_FLOAT(LPE_GPS_Z, 3.0f);
/**
* GPS xy velocity standard deviation.
* EPV used if greater than this value.
*
* @group Local Position Estimator
* @unit m/s
@@ -193,7 +192,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.25f);
PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
/**
* GPS max eph
* Max EPH allowed for GPS initialization
*
* @group Local Position Estimator
* @unit m
@@ -204,7 +203,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
/**
* GPS max epv
* Max EPV allowed for GPS initialization
*
* @group Local Position Estimator
* @unit m
@@ -258,24 +257,30 @@ PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
/**
* Position propagation noise density
*
* Increase to trust measurements more.
* Decrease to trust model more.
*
* @group Local Position Estimator
* @unit m/s/sqrt(Hz)
* @min 0
* @max 1
* @decimal 8
*/
PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f);
/**
* Velocity propagation noise density
*
* Increase to trust measurements more.
* Decrease to trust model more.
*
* @group Local Position Estimator
* @unit (m/s)/s/sqrt(Hz)
* @min 0
* @max 1
* @decimal 8
*/
PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
/**
* Accel bias propagation noise density
@@ -332,3 +337,14 @@ PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
*/
PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
/**
* Cut frequency for state publication
*
* @group Local Position Estimator
* @unit Hz
* @min 5
* @max 1000
* @decimal 0
*/
PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0);