fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 16:20:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF filter update. Now correctly scaling variance (well, if you could call this ever "correct") for repeated fusion of the same data quantity.

Browse Source
This commit is contained in:
Lorenz Meier
2014-08-17 20:49:46 +02:00
parent 60706500df
commit 171857811f
3 changed files with 104 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+28 -5
View File
@@ -96,7 +96,10 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
__EXPORT uint32_t millis();
__EXPORT uint64_t getMicros();
static uint64_t IMUmsec = 0;
static uint64_t IMUusec = 0;
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
uint32_t millis()
@@ -104,6 +107,11 @@ uint32_t millis()
return IMUmsec;
}
uint64_t getMicros()
{
return IMUusec;
}
class FixedwingEstimator
{
public:
@@ -850,7 +858,8 @@ FixedwingEstimator::task_main()
}
_last_sensor_timestamp = _gyro.timestamp;
IMUmsec = _gyro.timestamp / 1e3f;
IMUmsec = _gyro.timestamp / 1e3;
IMUusec = _gyro.timestamp;
float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
_last_run = _gyro.timestamp;
@@ -914,7 +923,8 @@ FixedwingEstimator::task_main()
// Copy gyro and accel
_last_sensor_timestamp = _sensor_combined.timestamp;
IMUmsec = _sensor_combined.timestamp / 1e3f;
IMUmsec = _sensor_combined.timestamp / 1e3;
IMUusec = _sensor_combined.timestamp;
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
@@ -994,8 +1004,6 @@ FixedwingEstimator::task_main()
if (gps_updated) {
last_gps = _gps.timestamp_position;
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps);
@@ -1008,11 +1016,17 @@ FixedwingEstimator::task_main()
_gps_start_time = hrt_absolute_time();
/* check if we had a GPS outage for a long time */
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f;
const float pos_reset_threshold = 5.0f; // seconds
/* timeout of 5 seconds */
if (gps_elapsed > pos_reset_threshold) {
_ekf->ResetPosition();
_ekf->ResetVelocity();
_ekf->ResetStoredStates();
}
_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold));
/* fuse GPS updates */
@@ -1044,6 +1058,8 @@ FixedwingEstimator::task_main()
newDataGps = true;
last_gps = _gps.timestamp_position;
}
}
@@ -1052,8 +1068,15 @@ FixedwingEstimator::task_main()
orb_check(_baro_sub, &baro_updated);
if (baro_updated) {
hrt_abstime baro_last = _baro.timestamp;
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1));
_ekf->baroHgt = _baro.altitude;
if (!_baro_init) {
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+60 -12
View File
@@ -2,6 +2,11 @@
#include <string.h>
#include <stdio.h>
#include <stdarg.h>
#include <math.h>
#ifndef M_PI_F
#define M_PI_F ((float)M_PI)
#endif
#define EKF_COVARIANCE_DIVERGED 1.0e8f
@@ -38,6 +43,7 @@ AttPosEKF::AttPosEKF() :
resetStates{},
storedStates{},
statetimeStamp{},
lastVelPosFusion(millis()),
statesAtVelTime{},
statesAtPosTime{},
statesAtHgtTime{},
@@ -59,7 +65,12 @@ AttPosEKF::AttPosEKF() :
accel(),
dVelIMU(),
dAngIMU(),
dtIMU(0),
dtIMU(0.005f),
dtIMUfilt(0.005f),
dtVelPos(0.01f),
dtVelPosFilt(0.01f),
dtHgtFilt(0.01f),
dtGpsFilt(0.1f),
fusionModeGPS(0),
innovVelPos{},
varInnovVelPos{},
@@ -260,6 +271,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
// Constrain states (to protect against filter divergence)
ConstrainStates();
// update filtered IMU time step length
dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU;
}
void AttPosEKF::CovariancePrediction(float dt)
@@ -962,6 +976,21 @@ void AttPosEKF::CovariancePrediction(float dt)
ConstrainVariances();
}
void AttPosEKF::updateDtGpsFilt(float dt)
{
dtGpsFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtGpsFilt * 0.95f;
}
void AttPosEKF::updateDtHgtFilt(float dt)
{
dtHgtFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtHgtFilt * 0.95f;
}
void AttPosEKF::updateDtVelPosFilt(float dt)
{
dtVelPosFilt = ConstrainFloat(dt, 0.0005f, 2.0f) * 0.05f + dtVelPosFilt * 0.95f;
}
void AttPosEKF::FuseVelposNED()
{
@@ -998,6 +1027,18 @@ void AttPosEKF::FuseVelposNED()
// associated with sequential fusion
if (fuseVelData || fusePosData || fuseHgtData)
{
uint64_t tNow = getMicros();
updateDtVelPosFilt((tNow - lastVelPosFusion) / 1e6f);
lastVelPosFusion = tNow;
// scaler according to the number of repetitions of the
// same measurement in one fusion step
float gpsVarianceScaler = dtGpsFilt / dtVelPosFilt;
// scaler according to the number of repetitions of the
// same measurement in one fusion step
float hgtVarianceScaler = dtHgtFilt / dtVelPosFilt;
// set the GPS data timeout depending on whether airspeed data is present
if (useAirspeed) horizRetryTime = gpsRetryTime;
else horizRetryTime = gpsRetryTimeNoTAS;
@@ -1010,12 +1051,12 @@ void AttPosEKF::FuseVelposNED()
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
R_OBS[0] = sq(vneSigma) + sq(velErr);
R_OBS[0] = gpsVarianceScaler * sq(vneSigma) + sq(velErr);
R_OBS[1] = R_OBS[0];
R_OBS[2] = sq(vdSigma) + sq(velErr);
R_OBS[3] = sq(posNeSigma) + sq(posErr);
R_OBS[2] = gpsVarianceScaler * sq(vdSigma) + sq(velErr);
R_OBS[3] = gpsVarianceScaler * sq(posNeSigma) + sq(posErr);
R_OBS[4] = R_OBS[3];
R_OBS[5] = sq(posDSigma) + sq(posErr);
R_OBS[5] = hgtVarianceScaler * sq(posDSigma) + sq(posErr);
// calculate innovations and check GPS data validity using an innovation consistency check
if (fuseVelData)
@@ -1995,9 +2036,8 @@ void AttPosEKF::FuseOptFlow()
varInnovOptFlow[0] = 1.0f/SK_LOS[0];
innovOptFlow[0] = losPred[0] - losData[0];
// reset the observation index to 0 (we start by fusing the X
// measurement)
obsIndex = 0;
// set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag
obsIndex = 1;
fuseOptFlowData = false;
}
else if (obsIndex == 1) // we are now fusing the Y measurement
@@ -2041,6 +2081,10 @@ void AttPosEKF::FuseOptFlow()
Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
varInnovOptFlow[1] = 1.0f/SK_LOS[1];
innovOptFlow[1] = losPred[1] - losData[1];
// reset the observation index
obsIndex = 0;
fuseOptFlowData = false;
}
// Check the innovation for consistency and don't fuse if > 3Sigma
@@ -2102,10 +2146,9 @@ void AttPosEKF::FuseOptFlow()
P[i][j] = P[i][j] - KHP[i][j];
}
}
ForceSymmetry();
ConstrainVariances();
}
obsIndex = obsIndex + 1;
ForceSymmetry();
ConstrainVariances();
}
void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
@@ -3006,9 +3049,14 @@ void AttPosEKF::ZeroVariables()
{
// Initialize on-init initialized variables
dtIMUfilt = ConstrainFloat(dtIMU, 0.001f, 0.02f);
dtVelPosFilt = ConstrainFloat(dtVelPos, 0.04f, 0.5f);
dtGpsFilt = 1.0f / 5.0f;
dtHgtFilt = 1.0f / 100.0f;
storeIndex = 0;
lastVelPosFusion = millis();
// Do the data structure init
for (unsigned i = 0; i < n_states; i++) {
for (unsigned j = 0; j < n_states; j++) {
src/modules/ekf_att_pos_estimator/estimator_23states.h
+16 -1
View File
@@ -116,6 +116,9 @@ public:
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
// Times
uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
@@ -140,7 +143,12 @@ public:
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
float dtIMUfilt; // average time between IMU measurements (sec)
float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
float dtVelPosFilt; // average time between position / velocity fusion steps
float dtHgtFilt; // average time between height measurement updates
float dtGpsFilt; // average time between gps measurement updates
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
@@ -211,6 +219,9 @@ public:
unsigned storeIndex;
void updateDtGpsFilt(float dt);
void updateDtHgtFilt(float dt);
void UpdateStrapdownEquationsNED();
@@ -300,6 +311,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination);
protected:
void updateDtVelPosFilt(float dt);
bool FilterHealthy();
bool GyroOffsetsDiverged();
@@ -314,3 +327,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
uint32_t millis();
uint64_t getMicros();