mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-29 09:44:07 +08:00
Update EKF estimator to most recent version from Paul Riseborough
This commit is contained in:
Lorenz Meier
parent
4942883ddc
commit
c82996850b
@ -84,7 +84,7 @@
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "estimator_23states.h"
|
||||
#include "estimator_22states.h"
|
||||
|
||||
|
||||
|
||||
@ -1371,10 +1371,15 @@ FixedwingEstimator::task_main()
|
||||
}
|
||||
|
||||
if (newRangeData) {
|
||||
_ekf->fuseRngData = true;
|
||||
_ekf->useRangeFinder = true;
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
|
||||
_ekf->GroundEKF();
|
||||
|
||||
if (_ekf->Tnb.z.z > 0.9f) {
|
||||
// _ekf->rngMea is set in sensor readout already
|
||||
_ekf->fuseRngData = true;
|
||||
_ekf->fuseOptFlowData = false;
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
|
||||
_ekf->OpticalFlowEKF();
|
||||
_ekf->fuseRngData = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -1514,8 +1519,8 @@ FixedwingEstimator::task_main()
|
||||
|
||||
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
|
||||
_wind.timestamp = _global_pos.timestamp;
|
||||
_wind.windspeed_north = _ekf->windSpdFiltNorth;
|
||||
_wind.windspeed_east = _ekf->windSpdFiltEast;
|
||||
_wind.windspeed_north = _ekf->states[14];
|
||||
_wind.windspeed_east = _ekf->states[15];
|
||||
// XXX we need to do something smart about the covariance here
|
||||
// but we default to the estimate covariance for now
|
||||
_wind.covariance_north = _ekf->P[14][14];
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,247 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "estimator_utilities.h"
|
||||
|
||||
class AttPosEKF {
|
||||
|
||||
public:
|
||||
|
||||
AttPosEKF();
|
||||
~AttPosEKF();
|
||||
|
||||
/* ##############################################
|
||||
*
|
||||
* M A I N F I L T E R P A R A M E T E R S
|
||||
*
|
||||
* ########################################### */
|
||||
|
||||
/*
|
||||
* parameters are defined here and initialised in
|
||||
* the InitialiseParameters() (which is just 20 lines down)
|
||||
*/
|
||||
|
||||
float covTimeStepMax; // maximum time allowed between covariance predictions
|
||||
float covDelAngMax; // maximum delta angle between covariance predictions
|
||||
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
|
||||
float yawVarScale;
|
||||
float windVelSigma;
|
||||
float dAngBiasSigma;
|
||||
float dVelBiasSigma;
|
||||
float magEarthSigma;
|
||||
float magBodySigma;
|
||||
float gndHgtSigma;
|
||||
|
||||
float vneSigma;
|
||||
float vdSigma;
|
||||
float posNeSigma;
|
||||
float posDSigma;
|
||||
float magMeasurementSigma;
|
||||
float airspeedMeasurementSigma;
|
||||
|
||||
float gyroProcessNoise;
|
||||
float accelProcessNoise;
|
||||
|
||||
float EAS2TAS; // ratio f true to equivalent airspeed
|
||||
|
||||
void InitialiseParameters()
|
||||
{
|
||||
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
|
||||
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
|
||||
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
EAS2TAS = 1.0f;
|
||||
|
||||
yawVarScale = 1.0f;
|
||||
windVelSigma = 0.1f;
|
||||
dAngBiasSigma = 5.0e-7f;
|
||||
dVelBiasSigma = 1e-4f;
|
||||
magEarthSigma = 3.0e-4f;
|
||||
magBodySigma = 3.0e-4f;
|
||||
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
|
||||
|
||||
vneSigma = 0.2f;
|
||||
vdSigma = 0.3f;
|
||||
posNeSigma = 2.0f;
|
||||
posDSigma = 2.0f;
|
||||
|
||||
magMeasurementSigma = 0.05;
|
||||
airspeedMeasurementSigma = 1.4f;
|
||||
gyroProcessNoise = 1.4544411e-2f;
|
||||
accelProcessNoise = 0.5f;
|
||||
}
|
||||
|
||||
// Global variables
|
||||
float KH[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float KHP[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float P[n_states][n_states]; // covariance matrix
|
||||
float Kfusion[n_states]; // Kalman gains
|
||||
float states[n_states]; // state matrix
|
||||
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
|
||||
|
||||
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
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
|
||||
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
|
||||
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)
|
||||
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
|
||||
|
||||
float velNED[3]; // North, East, Down velocity obs (m/s)
|
||||
float posNE[2]; // North, East position obs (m)
|
||||
float hgtMea; // measured height (m)
|
||||
float posNED[3]; // North, East Down position (m)
|
||||
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float innovVtas; // innovation output
|
||||
float varInnovVtas; // innovation variance output
|
||||
float VtasMeas; // true airspeed measurement (m/s)
|
||||
float magDeclination;
|
||||
float latRef; // WGS-84 latitude of reference point (rad)
|
||||
float lonRef; // WGS-84 longitude of reference point (rad)
|
||||
float hgtRef; // WGS-84 height of reference point (m)
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
|
||||
// GPS input data variables
|
||||
float gpsCourse;
|
||||
float gpsVelD;
|
||||
float gpsLat;
|
||||
float gpsLon;
|
||||
float gpsHgt;
|
||||
uint8_t GPSstatus;
|
||||
|
||||
// Baro input
|
||||
float baroHgt;
|
||||
|
||||
bool statesInitialised;
|
||||
|
||||
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
|
||||
bool fuseMagData; // boolean true when magnetometer data is to be fused
|
||||
bool fuseVtasData; // boolean true when airspeed data is to be fused
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
bool useAirspeed; ///< boolean true if airspeed data is being used
|
||||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
|
||||
struct ekf_status_report current_ekf_state;
|
||||
struct ekf_status_report last_ekf_error;
|
||||
|
||||
bool numericalProtection;
|
||||
|
||||
unsigned storeIndex;
|
||||
|
||||
|
||||
void UpdateStrapdownEquationsNED();
|
||||
|
||||
void CovariancePrediction(float dt);
|
||||
|
||||
void FuseVelposNED();
|
||||
|
||||
void FuseMagnetometer();
|
||||
|
||||
void FuseAirspeed();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
|
||||
|
||||
// store staes along with system time stamp in msces
|
||||
void StoreStates(uint64_t timestamp_ms);
|
||||
|
||||
/**
|
||||
* Recall the state vector.
|
||||
*
|
||||
* Recalls the vector stored at closest time to the one specified by msec
|
||||
*
|
||||
* @return zero on success, integer indicating the number of invalid states on failure.
|
||||
* Does only copy valid states, if the statesForFusion vector was initialized
|
||||
* correctly by the caller, the result can be safely used, but is a mixture
|
||||
* time-wise where valid states were updated and invalid remained at the old
|
||||
* value.
|
||||
*/
|
||||
int RecallStates(float statesForFusion[n_states], uint64_t msec);
|
||||
|
||||
void ResetStoredStates();
|
||||
|
||||
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
|
||||
|
||||
void calcEarthRateNED(Vector3f &omega, float latitude);
|
||||
|
||||
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
|
||||
|
||||
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
|
||||
|
||||
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
|
||||
|
||||
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
|
||||
|
||||
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
|
||||
|
||||
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
||||
|
||||
static float sq(float valIn);
|
||||
|
||||
void OnGroundCheck();
|
||||
|
||||
void CovarianceInit();
|
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
|
||||
|
||||
float ConstrainFloat(float val, float min, float max);
|
||||
|
||||
void ConstrainVariances();
|
||||
|
||||
void ConstrainStates();
|
||||
|
||||
void ForceSymmetry();
|
||||
|
||||
int CheckAndBound();
|
||||
|
||||
void ResetPosition();
|
||||
|
||||
void ResetVelocity();
|
||||
|
||||
void ZeroVariables();
|
||||
|
||||
void GetFilterState(struct ekf_status_report *state);
|
||||
|
||||
void GetLastErrorState(struct ekf_status_report *last_error);
|
||||
|
||||
bool StatesNaN(struct ekf_status_report *err_report);
|
||||
void FillErrorReport(struct ekf_status_report *err);
|
||||
|
||||
void InitializeDynamic(float (&initvelNED)[3], float declination);
|
||||
|
||||
protected:
|
||||
|
||||
bool FilterHealthy();
|
||||
|
||||
void ResetHeight(void);
|
||||
|
||||
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
|
||||
|
||||
};
|
||||
|
||||
uint32_t millis();
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -2,7 +2,7 @@
|
||||
|
||||
#include "estimator_utilities.h"
|
||||
|
||||
const unsigned int n_states = 23;
|
||||
const unsigned int n_states = 22;
|
||||
const unsigned int data_buffer_size = 50;
|
||||
|
||||
class AttPosEKF {
|
||||
@ -29,10 +29,6 @@ public:
|
||||
float covDelAngMax; // maximum delta angle between covariance predictions
|
||||
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
|
||||
float a1; // optical flow sensor misalgnment angle about X axis (rad)
|
||||
float a2; // optical flow sensor misalgnment angle about Y axis (rad)
|
||||
float a3; // optical flow sensor misalgnment angle about Z axis (rad)
|
||||
|
||||
float yawVarScale;
|
||||
float windVelSigma;
|
||||
float dAngBiasSigma;
|
||||
@ -59,9 +55,6 @@ public:
|
||||
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
|
||||
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
EAS2TAS = 1.0f;
|
||||
a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad)
|
||||
a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad)
|
||||
a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad)
|
||||
|
||||
yawVarScale = 1.0f;
|
||||
windVelSigma = 0.1f;
|
||||
@ -69,7 +62,6 @@ public:
|
||||
dVelBiasSigma = 1e-4f;
|
||||
magEarthSigma = 3.0e-4f;
|
||||
magBodySigma = 3.0e-4f;
|
||||
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
|
||||
|
||||
vneSigma = 0.2f;
|
||||
vdSigma = 0.3f;
|
||||
@ -82,12 +74,13 @@ public:
|
||||
accelProcessNoise = 0.5f;
|
||||
|
||||
gndHgtSigma = 0.1f; // terrain gradient 1-sigma
|
||||
R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2
|
||||
R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2
|
||||
flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
|
||||
auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
|
||||
rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check
|
||||
minFlowRng = 0.01f; //minimum range between ground and flow sensor
|
||||
moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check
|
||||
minFlowRng = 0.3f; //minimum range between ground and flow sensor
|
||||
moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
|
||||
}
|
||||
|
||||
struct mag_state_struct {
|
||||
@ -134,6 +127,7 @@ public:
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtRngTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
|
||||
float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
@ -145,7 +139,7 @@ public:
|
||||
Vector3f lastGyroOffset; // Last gyro offset
|
||||
Vector3f delAngTotal;
|
||||
|
||||
Mat3f Tbn; // transformation matrix from body to NED coordinates
|
||||
Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow
|
||||
Mat3f Tnb; // transformation amtrix from NED to body coordinates
|
||||
|
||||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
@ -157,10 +151,6 @@ public:
|
||||
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
|
||||
float windSpdFiltNorth; // average wind speed north component
|
||||
float windSpdFiltEast; // average wind speed east component
|
||||
float windSpdFiltAltitude; // the last altitude used to filter wind speed
|
||||
float windSpdFiltClimb; // filtered climb rate
|
||||
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
|
||||
@ -175,7 +165,8 @@ public:
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float losData[2]; // optical flow LOS rate measurements (rad/sec)
|
||||
float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec)
|
||||
float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec)
|
||||
float innovVtas; // innovation output
|
||||
float innovRng; ///< Range finder innovation
|
||||
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
|
||||
@ -190,6 +181,8 @@ public:
|
||||
bool refSet; ///< flag to indicate if the reference position has been set
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used
|
||||
uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle
|
||||
|
||||
// GPS input data variables
|
||||
double gpsLat;
|
||||
@ -217,6 +210,7 @@ public:
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
bool useGPS; // boolean true if GPS data is being used
|
||||
bool useAirspeed; ///< boolean true if airspeed data is being used
|
||||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
bool useRangeFinder; ///< true when rangefinder is being used
|
||||
@ -267,11 +261,9 @@ void FuseMagnetometer();
|
||||
|
||||
void FuseAirspeed();
|
||||
|
||||
void FuseRangeFinder();
|
||||
|
||||
void FuseOptFlow();
|
||||
|
||||
void GroundEKF();
|
||||
void OpticalFlowEKF();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
@ -286,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms);
|
||||
* Recall the state vector.
|
||||
*
|
||||
* Recalls the vector stored at closest time to the one specified by msec
|
||||
*
|
||||
*FuseOptFlow
|
||||
* @return zero on success, integer indicating the number of invalid states on failure.
|
||||
* Does only copy valid states, if the statesForFusion vector was initialized
|
||||
* correctly by the caller, the result can be safely used, but is a mixture
|
||||
@ -295,6 +287,8 @@ void StoreStates(uint64_t timestamp_ms);
|
||||
*/
|
||||
int RecallStates(float *statesForFusion, uint64_t msec);
|
||||
|
||||
void RecallOmega(float *omegaForFusion, uint64_t msec);
|
||||
|
||||
void ResetStoredStates();
|
||||
|
||||
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
|
||||
@ -325,7 +319,7 @@ void CovarianceInit();
|
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
|
||||
|
||||
float ConstrainFloat(float val, float min, float max);
|
||||
float ConstrainFloat(float val, float min, float maxf);
|
||||
|
||||
void ConstrainVariances();
|
||||
|
||||
@ -39,7 +39,7 @@ MODULE_COMMAND = ekf_att_pos_estimator
|
||||
|
||||
SRCS = ekf_att_pos_estimator_main.cpp \
|
||||
ekf_att_pos_estimator_params.c \
|
||||
estimator_23states.cpp \
|
||||
estimator_22states.cpp \
|
||||
estimator_utilities.cpp
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user