zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #794 from PX4/estimator_ram_fix 

Move Pauls EKF into a class and instantiate only when / if needed.
sbg
Lorenz Meier 11 years ago
parent
2aa9e3bd78 1e25ceb085
commit
89817d1366
3 changed files with 372 additions and 404 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 198
      src/modules/fw_att_pos_estimator/estimator.cpp
  2. 190
      src/modules/fw_att_pos_estimator/estimator.h
  3. 388
      src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp

198
src/modules/fw_att_pos_estimator/estimator.cpp
Unescape View File

@ -2,85 +2,6 @@ @@ -2,85 +2,6 @@
#include <string.h>
// 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
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; // 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 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
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 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 = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed
// GPS input data variables
float gpsCourse;
float gpsVelD;
float gpsLat;
float gpsLon;
float gpsHgt;
uint8_t GPSstatus;
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
// Baro input
float baroHgt;
bool statesInitialised = false;
bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
bool fuseMagData = false; // boolean true when magnetometer data is to be fused
bool fuseVtasData = false; // boolean true when airspeed data is to be fused
bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode = true; ///< boolean true if no position feedback is fused
bool useAirspeed = true; ///< boolean true if airspeed data is being used
bool useCompass = true; ///< boolean true if magnetometer data is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection = true;
static unsigned storeIndex = 0;
float Vector3f::length(void) const
{
return sqrt(x*x + y*y + z*z);
@ -185,7 +106,16 @@ void swap_var(float &d1, float &d2) @@ -185,7 +106,16 @@ void swap_var(float &d1, float &d2)
d2 = tmp;
}
void UpdateStrapdownEquationsNED()
AttPosEKF::AttPosEKF()
{
}
AttPosEKF::~AttPosEKF()
{
}
void AttPosEKF::UpdateStrapdownEquationsNED()
{
Vector3f delVelNav;
float q00;
@ -247,7 +177,7 @@ void UpdateStrapdownEquationsNED() @@ -247,7 +177,7 @@ void UpdateStrapdownEquationsNED()
qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
// Normalise the quaternions and update the quaternion states
quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
if (quatMag > 1e-16f)
{
float quatMagInv = 1.0f/quatMag;
@ -312,7 +242,7 @@ void UpdateStrapdownEquationsNED() @@ -312,7 +242,7 @@ void UpdateStrapdownEquationsNED()
ConstrainStates();
}
void CovariancePrediction(float dt)
void AttPosEKF::CovariancePrediction(float dt)
{
// scalars
float windVelSigma;
@ -953,7 +883,7 @@ void CovariancePrediction(float dt) @@ -953,7 +883,7 @@ void CovariancePrediction(float dt)
ConstrainVariances();
}
void FuseVelposNED()
void AttPosEKF::FuseVelposNED()
{
// declare variables used by fault isolation logic
@ -999,8 +929,8 @@ void FuseVelposNED() @@ -999,8 +929,8 @@ void FuseVelposNED()
observation[5] = -(hgtMea);
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring
posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring
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] = 0.04f + sq(velErr);
R_OBS[1] = R_OBS[0];
R_OBS[2] = 0.08f + sq(velErr);
@ -1026,7 +956,7 @@ void FuseVelposNED() @@ -1026,7 +956,7 @@ void FuseVelposNED()
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
}
// apply a 5-sigma threshold
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
{
@ -1175,7 +1105,7 @@ void FuseVelposNED() @@ -1175,7 +1105,7 @@ void FuseVelposNED()
//printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
}
void FuseMagnetometer()
void AttPosEKF::FuseMagnetometer()
{
uint8_t obsIndex;
uint8_t indexLimit;
@ -1482,7 +1412,7 @@ void FuseMagnetometer() @@ -1482,7 +1412,7 @@ void FuseMagnetometer()
ConstrainVariances();
}
void FuseAirspeed()
void AttPosEKF::FuseAirspeed()
{
float vn;
float ve;
@ -1503,14 +1433,14 @@ void FuseAirspeed() @@ -1503,14 +1433,14 @@ void FuseAirspeed()
// Need to check that it is flying before fusing airspeed data
// Calculate the predicted airspeed
VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
// Perform fusion of True Airspeed measurement
if (useAirspeed && fuseVtasData && (VtasPred > 1.0) && (VtasMeas > 8.0))
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
{
// Calculate observation jacobians
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
float H_TAS[21];
for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f;
@ -1611,7 +1541,7 @@ void FuseAirspeed() @@ -1611,7 +1541,7 @@ void FuseAirspeed()
ConstrainVariances();
}
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
{
uint8_t row;
uint8_t col;
@ -1624,7 +1554,7 @@ void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) @@ -1624,7 +1554,7 @@ 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 AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
{
uint8_t row;
uint8_t col;
@ -1637,13 +1567,13 @@ void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) @@ -1637,13 +1567,13 @@ void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
}
}
float sq(float valIn)
float AttPosEKF::sq(float valIn)
{
return valIn*valIn;
}
// Store states in a history array along with time stamp
void StoreStates(uint64_t timestamp_ms)
void AttPosEKF::StoreStates(uint64_t timestamp_ms)
{
for (unsigned i=0; i<n_states; i++)
storedStates[i][storeIndex] = states[i];
@ -1653,7 +1583,7 @@ void StoreStates(uint64_t timestamp_ms) @@ -1653,7 +1583,7 @@ void StoreStates(uint64_t timestamp_ms)
storeIndex = 0;
}
void ResetStoredStates()
void AttPosEKF::ResetStoredStates()
{
// reset all stored states
memset(&storedStates[0][0], 0, sizeof(storedStates));
@ -1674,7 +1604,7 @@ void ResetStoredStates() @@ -1674,7 +1604,7 @@ void ResetStoredStates()
}
// Output the state vector stored at the time that best matches that specified by msec
int RecallStates(float statesForFusion[n_states], uint64_t msec)
int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec)
{
int ret = 0;
@ -1723,7 +1653,7 @@ int RecallStates(float statesForFusion[n_states], uint64_t msec) @@ -1723,7 +1653,7 @@ int RecallStates(float statesForFusion[n_states], uint64_t msec)
return ret;
}
void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
{
// Calculate the nav to body cosine matrix
float q00 = sq(quat[0]);
@ -1748,7 +1678,7 @@ void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) @@ -1748,7 +1678,7 @@ void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
Tnb.y.z = 2*(q23 + q01);
}
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
{
// Calculate the body to nav cosine matrix
float q00 = sq(quat[0]);
@ -1773,7 +1703,7 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) @@ -1773,7 +1703,7 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
Tbn.z.y = 2*(q23 + q01);
}
void eul2quat(float (&quat)[4], const float (&eul)[3])
void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
{
float u1 = cos(0.5f*eul[0]);
float u2 = cos(0.5f*eul[1]);
@ -1787,35 +1717,35 @@ void eul2quat(float (&quat)[4], const float (&eul)[3]) @@ -1787,35 +1717,35 @@ void eul2quat(float (&quat)[4], const float (&eul)[3])
quat[3] = u1*u2*u6-u4*u5*u3;
}
void quat2eul(float (&y)[3], const float (&u)[4])
void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
{
y[0] = atan2((2.0*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
y[1] = -asin(2.0*(u[1]*u[3]-u[0]*u[2]));
y[2] = atan2((2.0*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
}
void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
{
velNED[0] = gpsGndSpd*cos(gpsCourse);
velNED[1] = gpsGndSpd*sin(gpsCourse);
velNED[0] = gpsGndSpd*cosf(gpsCourse);
velNED[1] = gpsGndSpd*sinf(gpsCourse);
velNED[2] = gpsVelD;
}
void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
{
posNED[0] = earthRadius * (lat - latRef);
posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
posNED[2] = -(hgt - hgtRef);
}
void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
{
lat = latRef + posNED[0] * earthRadiusInv;
lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
hgt = hgtRef - posNED[2];
}
void OnGroundCheck()
void AttPosEKF::OnGroundCheck()
{
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
if (staticMode) {
@ -1823,7 +1753,7 @@ void OnGroundCheck() @@ -1823,7 +1753,7 @@ void OnGroundCheck()
}
}
void calcEarthRateNED(Vector3f &omega, float latitude)
void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
{
//Define Earth rotation vector in the NED navigation frame
omega.x = earthRate*cosf(latitude);
@ -1831,13 +1761,13 @@ void calcEarthRateNED(Vector3f &omega, float latitude) @@ -1831,13 +1761,13 @@ void calcEarthRateNED(Vector3f &omega, float latitude)
omega.z = -earthRate*sinf(latitude);
}
void CovarianceInit()
void AttPosEKF::CovarianceInit()
{
// Calculate the initial covariance matrix P
P[0][0] = 0.25f*sq(1.0f*deg2rad);
P[1][1] = 0.25f*sq(1.0f*deg2rad);
P[2][2] = 0.25f*sq(1.0f*deg2rad);
P[3][3] = 0.25f*sq(10.0f*deg2rad);
P[0][0] = 0.25f * sq(1.0f*deg2rad);
P[1][1] = 0.25f * sq(1.0f*deg2rad);
P[2][2] = 0.25f * sq(1.0f*deg2rad);
P[3][3] = 0.25f * sq(10.0f*deg2rad);
P[4][4] = sq(0.7);
P[5][5] = P[4][4];
P[6][6] = sq(0.7);
@ -1857,12 +1787,12 @@ void CovarianceInit() @@ -1857,12 +1787,12 @@ void CovarianceInit()
P[20][20] = P[18][18];
}
float ConstrainFloat(float val, float min, float max)
float AttPosEKF::ConstrainFloat(float val, float min, float max)
{
return (val > max) ? max : ((val < min) ? min : val);
}
void ConstrainVariances()
void AttPosEKF::ConstrainVariances()
{
if (!numericalProtection) {
return;
@ -1914,7 +1844,7 @@ void ConstrainVariances() @@ -1914,7 +1844,7 @@ void ConstrainVariances()
}
void ConstrainStates()
void AttPosEKF::ConstrainStates()
{
if (!numericalProtection) {
return;
@ -1971,7 +1901,7 @@ void ConstrainStates() @@ -1971,7 +1901,7 @@ void ConstrainStates()
}
void ForceSymmetry()
void AttPosEKF::ForceSymmetry()
{
if (!numericalProtection) {
return;
@ -1989,7 +1919,7 @@ void ForceSymmetry() @@ -1989,7 +1919,7 @@ void ForceSymmetry()
}
}
bool FilterHealthy()
bool AttPosEKF::FilterHealthy()
{
if (!statesInitialised) {
return false;
@ -2012,7 +1942,7 @@ bool FilterHealthy() @@ -2012,7 +1942,7 @@ bool FilterHealthy()
* This resets the position to the last GPS measurement
* or to zero in case of static position.
*/
void ResetPosition(void)
void AttPosEKF::ResetPosition(void)
{
if (staticMode) {
states[7] = 0;
@ -2030,7 +1960,7 @@ void ResetPosition(void) @@ -2030,7 +1960,7 @@ void ResetPosition(void)
*
* This resets the height state with the last altitude measurements
*/
void ResetHeight(void)
void AttPosEKF::ResetHeight(void)
{
// write to the state vector
states[9] = -hgtMea;
@ -2039,7 +1969,7 @@ void ResetHeight(void) @@ -2039,7 +1969,7 @@ void ResetHeight(void)
/**
* Reset the velocity state.
*/
void ResetVelocity(void)
void AttPosEKF::ResetVelocity(void)
{
if (staticMode) {
states[4] = 0.0f;
@ -2054,7 +1984,7 @@ void ResetVelocity(void) @@ -2054,7 +1984,7 @@ void ResetVelocity(void)
}
void FillErrorReport(struct ekf_status_report *err)
void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
{
for (int i = 0; i < n_states; i++)
{
@ -2069,7 +1999,7 @@ void FillErrorReport(struct ekf_status_report *err) @@ -2069,7 +1999,7 @@ void FillErrorReport(struct ekf_status_report *err)
err->hgtTimeout = current_ekf_state.hgtTimeout;
}
bool StatesNaN(struct ekf_status_report *err_report) {
bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
bool err = false;
// check all states and covariance matrices
@ -2122,7 +2052,7 @@ bool StatesNaN(struct ekf_status_report *err_report) { @@ -2122,7 +2052,7 @@ bool StatesNaN(struct ekf_status_report *err_report) {
* updated, but before any of the fusion steps are
* executed.
*/
int CheckAndBound()
int AttPosEKF::CheckAndBound()
{
// Store the old filter state
@ -2164,7 +2094,7 @@ int CheckAndBound() @@ -2164,7 +2094,7 @@ int CheckAndBound()
return 0;
}
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
@ -2200,7 +2130,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl @@ -2200,7 +2130,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
}
void InitializeDynamic(float (&initvelNED)[3])
void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
{
// Clear the init flag
@ -2254,7 +2184,7 @@ void InitializeDynamic(float (&initvelNED)[3]) @@ -2254,7 +2184,7 @@ void InitializeDynamic(float (&initvelNED)[3])
summedDelVel.z = 0.0f;
}
void InitialiseFilter(float (&initvelNED)[3])
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3])
{
//store initial lat,long and height
latRef = gpsLat;
@ -2266,7 +2196,7 @@ void InitialiseFilter(float (&initvelNED)[3]) @@ -2266,7 +2196,7 @@ void InitialiseFilter(float (&initvelNED)[3])
InitializeDynamic(initvelNED);
}
void ZeroVariables()
void AttPosEKF::ZeroVariables()
{
// Do the data structure init
for (unsigned i = 0; i < n_states; i++) {
@ -2292,12 +2222,12 @@ void ZeroVariables() @@ -2292,12 +2222,12 @@ void ZeroVariables()
memset(&current_ekf_state, 0, sizeof(current_ekf_state));
}
void GetFilterState(struct ekf_status_report *state)
void AttPosEKF::GetFilterState(struct ekf_status_report *state)
{
memcpy(state, &current_ekf_state, sizeof(state));
}
void GetLastErrorState(struct ekf_status_report *last_error)
void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
{
memcpy(last_error, &last_ekf_error, sizeof(last_error));
}

190
src/modules/fw_att_pos_estimator/estimator.h
Unescape View File

@ -48,85 +48,10 @@ void swap_var(float &d1, float &d2); @@ -48,85 +48,10 @@ void swap_var(float &d1, float &d2);
const unsigned int n_states = 21;
const unsigned int data_buffer_size = 50;
extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
extern Vector3f dVelIMU;
extern Vector3f dAngIMU;
extern float P[n_states][n_states]; // covariance matrix
extern float Kfusion[n_states]; // Kalman gains
extern float states[n_states]; // state matrix
extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
extern bool useAirspeed; // boolean true if airspeed data is being used
extern bool useCompass; // boolean true if magnetometer data is being used
extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
extern float innovVelPos[6]; // innovation output
extern float varInnovVelPos[6]; // innovation variance output
extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
extern bool fuseMagData; // boolean true when magnetometer data is to be fused
extern float velNED[3]; // North, East, Down velocity obs (m/s)
extern float posNE[2]; // North, East position obs (m)
extern float hgtMea; // measured height (m)
extern float posNED[3]; // North, East Down position (m)
extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
extern float innovMag[3]; // innovation output
extern float varInnovMag[3]; // innovation variance output
extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
extern float innovVtas; // innovation output
extern float varInnovVtas; // innovation variance output
extern bool fuseVtasData; // boolean true when airspeed data is to be fused
extern float VtasMeas; // true airspeed measurement (m/s)
extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
extern float latRef; // WGS-84 latitude of reference point (rad)
extern float lonRef; // WGS-84 longitude of reference point (rad)
extern float hgtRef; // WGS-84 height of reference point (m)
extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
extern float EAS2TAS; // ratio f true to equivalent airspeed
// GPS input data variables
extern float gpsCourse;
extern float gpsVelD;
extern float gpsLat;
extern float gpsLon;
extern float gpsHgt;
extern uint8_t GPSstatus;
// Baro input
extern float baroHgt;
extern bool statesInitialised;
extern bool numericalProtection;
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
extern bool staticMode;
// extern bool staticMode;
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
@ -150,6 +75,93 @@ struct ekf_status_report { @@ -150,6 +75,93 @@ struct ekf_status_report {
bool kalmanGainsNaN;
};
class AttPosEKF {
public:
AttPosEKF();
~AttPosEKF();
// 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; // 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 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 = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed
// GPS input data variables
float gpsCourse;
float gpsVelD;
float gpsLat;
float gpsLon;
float gpsHgt;
uint8_t GPSstatus;
// Baro input
float baroHgt;
bool statesInitialised = false;
bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
bool fuseMagData = false; // boolean true when magnetometer data is to be fused
bool fuseVtasData = false; // boolean true when airspeed data is to be fused
bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode = true; ///< boolean true if no position feedback is fused
bool useAirspeed = true; ///< boolean true if airspeed data is being used
bool useCompass = true; ///< boolean true if magnetometer data is being used
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection = true;
unsigned storeIndex = 0;
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
@ -164,8 +176,6 @@ void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); @@ -164,8 +176,6 @@ 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);
float sq(float valIn);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
@ -190,15 +200,19 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); @@ -190,15 +200,19 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
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);
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);
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();
@ -231,5 +245,15 @@ void FillErrorReport(struct ekf_status_report *err); @@ -231,5 +245,15 @@ void FillErrorReport(struct ekf_status_report *err);
void InitializeDynamic(float (&initvelNED)[3]);
protected:
bool FilterHealthy();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
};
uint32_t millis();

388
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
Unescape View File

@ -124,6 +124,16 @@ public: @@ -124,6 +124,16 @@ public:
*/
int start();
/**
* Print the current status.
*/
void print_status();
/**
* Trip the filter by feeding it NaN values.
*/
int trip_nan();
private:
bool _task_should_exit; /**< if true, sensor task should exit */
@ -199,6 +209,7 @@ private: @@ -199,6 +209,7 @@ private:
param_t tas_delay_ms;
} _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf;
/**
* Update our local parameter cache.
@ -280,7 +291,8 @@ FixedwingEstimator::FixedwingEstimator() : @@ -280,7 +291,8 @@ FixedwingEstimator::FixedwingEstimator() :
/* states */
_initialized(false),
_gps_initialized(false),
_mavlink_fd(-1)
_mavlink_fd(-1),
_ekf(nullptr)
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
@ -384,6 +396,12 @@ void @@ -384,6 +396,12 @@ void
FixedwingEstimator::task_main()
{
_ekf = new AttPosEKF();
if (!_ekf) {
errx(1, "failed allocating EKF filter - out of RAM!");
}
/*
* do subscriptions
*/
@ -414,23 +432,23 @@ FixedwingEstimator::task_main() @@ -414,23 +432,23 @@ FixedwingEstimator::task_main()
parameters_update();
/* set initial filter state */
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
fuseMagData = false;
fuseVtasData = false;
statesInitialised = false;
_ekf->fuseVelData = false;
_ekf->fusePosData = false;
_ekf->fuseHgtData = false;
_ekf->fuseMagData = false;
_ekf->fuseVtasData = false;
_ekf->statesInitialised = false;
/* initialize measurement data */
VtasMeas = 0.0f;
_ekf->VtasMeas = 0.0f;
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
Vector3f lastAccel = {0.0f, 0.0f, -9.81f};
dVelIMU.x = 0.0f;
dVelIMU.y = 0.0f;
dVelIMU.z = 0.0f;
dAngIMU.x = 0.0f;
dAngIMU.y = 0.0f;
dAngIMU.z = 0.0f;
_ekf->dVelIMU.x = 0.0f;
_ekf->dVelIMU.y = 0.0f;
_ekf->dVelIMU.z = 0.0f;
_ekf->dAngIMU.x = 0.0f;
_ekf->dAngIMU.y = 0.0f;
_ekf->dAngIMU.z = 0.0f;
/* wakeup source(s) */
struct pollfd fds[2];
@ -509,7 +527,7 @@ FixedwingEstimator::task_main() @@ -509,7 +527,7 @@ FixedwingEstimator::task_main()
}
last_sensor_timestamp = _gyro.timestamp;
IMUmsec = _gyro.timestamp / 1e3f;
_ekf.IMUmsec = _gyro.timestamp / 1e3f;
float deltaT = (_gyro.timestamp - last_run) / 1e6f;
last_run = _gyro.timestamp;
@ -521,20 +539,20 @@ FixedwingEstimator::task_main() @@ -521,20 +539,20 @@ FixedwingEstimator::task_main()
// Always store data, independent of init status
/* fill in last data set */
dtIMU = deltaT;
_ekf->dtIMU = deltaT;
angRate.x = _gyro.x;
angRate.y = _gyro.y;
angRate.z = _gyro.z;
_ekf->angRate.x = _gyro.x;
_ekf->angRate.y = _gyro.y;
_ekf->angRate.z = _gyro.z;
accel.x = _accel.x;
accel.y = _accel.y;
accel.z = _accel.z;
_ekf->accel.x = _accel.x;
_ekf->accel.y = _accel.y;
_ekf->accel.z = _accel.z;
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
lastAngRate = angRate;
dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
lastAccel = accel;
_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
_ekf->lastAngRate = angRate;
_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
_ekf->lastAccel = accel;
#else
@ -563,20 +581,20 @@ FixedwingEstimator::task_main() @@ -563,20 +581,20 @@ FixedwingEstimator::task_main()
// Always store data, independent of init status
/* fill in last data set */
dtIMU = deltaT;
_ekf->dtIMU = deltaT;
angRate.x = _sensor_combined.gyro_rad_s[0];
angRate.y = _sensor_combined.gyro_rad_s[1];
angRate.z = _sensor_combined.gyro_rad_s[2];
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
accel.x = _sensor_combined.accelerometer_m_s2[0];
accel.y = _sensor_combined.accelerometer_m_s2[1];
accel.z = _sensor_combined.accelerometer_m_s2[2];
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
lastAngRate = angRate;
dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
lastAccel = accel;
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
lastAccel = _ekf->accel;
if (last_mag != _sensor_combined.magnetometer_timestamp) {
mag_updated = true;
@ -597,7 +615,7 @@ FixedwingEstimator::task_main() @@ -597,7 +615,7 @@ FixedwingEstimator::task_main()
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
VtasMeas = _airspeed.true_airspeed_m_s;
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
newAdsData = true;
} else {
@ -622,24 +640,24 @@ FixedwingEstimator::task_main() @@ -622,24 +640,24 @@ FixedwingEstimator::task_main()
/* check if we had a GPS outage for a long time */
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
ResetPosition();
ResetVelocity();
ResetStoredStates();
_ekf->ResetPosition();
_ekf->ResetVelocity();
_ekf->ResetStoredStates();
}
/* fuse GPS updates */
//_gps.timestamp / 1e3;
GPSstatus = _gps.fix_type;
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
_ekf->GPSstatus = _gps.fix_type;
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
gpsLat = math::radians(_gps.lat / (double)1e7);
gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
gpsHgt = _gps.alt / 1e3f;
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f;
newDataGps = true;
}
@ -652,10 +670,10 @@ FixedwingEstimator::task_main() @@ -652,10 +670,10 @@ FixedwingEstimator::task_main()
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
baroHgt = _baro.altitude - _baro_ref;
_ekf->baroHgt = _baro.altitude - _baro_ref;
// Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
}
#ifndef SENSOR_COMBINED_SUB
@ -671,27 +689,27 @@ FixedwingEstimator::task_main() @@ -671,27 +689,27 @@ FixedwingEstimator::task_main()
// XXX we compensate the offsets upfront - should be close to zero.
// 0.001f
magData.x = _mag.x;
magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.x = _mag.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
magData.y = _mag.y;
magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.y = _mag.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
magData.z = _mag.z;
magBias.z = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _mag.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
#else
// XXX we compensate the offsets upfront - should be close to zero.
// 0.001f
magData.x = _sensor_combined.magnetometer_ga[0];
magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
magData.y = _sensor_combined.magnetometer_ga[1];
magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
magData.z = _sensor_combined.magnetometer_ga[2];
magBias.z = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
#endif
@ -705,7 +723,7 @@ FixedwingEstimator::task_main() @@ -705,7 +723,7 @@ FixedwingEstimator::task_main()
/**
* CHECK IF THE INPUT DATA IS SANE
*/
int check = CheckAndBound();
int check = _ekf->CheckAndBound();
switch (check) {
case 0:
@ -739,7 +757,7 @@ FixedwingEstimator::task_main() @@ -739,7 +757,7 @@ FixedwingEstimator::task_main()
struct ekf_status_report ekf_report;
GetLastErrorState(&ekf_report);
_ekf->GetLastErrorState(&ekf_report);
struct estimator_status_report rep;
memset(&rep, 0, sizeof(rep));
@ -779,16 +797,16 @@ FixedwingEstimator::task_main() @@ -779,16 +797,16 @@ FixedwingEstimator::task_main()
if (hrt_elapsed_time(&start_time) > 100000) {
if (!_gps_initialized && (GPSstatus == 3)) {
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
if (!_gps_initialized && (_ekf->GPSstatus == 3)) {
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
double lat = _gps.lat * 1e-7;
double lon = _gps.lon * 1e-7;
float alt = _gps.alt * 1e-3;
InitialiseFilter(velNED);
_ekf->InitialiseFilter(_ekf->velNED);
// Initialize projection
_local_pos.ref_lat = _gps.lat;
@ -799,7 +817,7 @@ FixedwingEstimator::task_main() @@ -799,7 +817,7 @@ FixedwingEstimator::task_main()
// Store
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref = _baro.altitude;
baroHgt = _baro.altitude - _baro_ref;
_ekf->baroHgt = _baro.altitude - _baro_ref;
_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
// XXX this is not multithreading safe
@ -808,24 +826,24 @@ FixedwingEstimator::task_main() @@ -808,24 +826,24 @@ FixedwingEstimator::task_main()
_gps_initialized = true;
} else if (!statesInitialised) {
velNED[0] = 0.0f;
velNED[1] = 0.0f;
velNED[2] = 0.0f;
posNED[0] = 0.0f;
posNED[1] = 0.0f;
posNED[2] = 0.0f;
posNE[0] = posNED[0];
posNE[1] = posNED[1];
InitialiseFilter(velNED);
} else if (!_ekf->statesInitialised) {
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
_ekf->velNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f;
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_ekf->InitialiseFilter(_ekf->velNED);
}
}
// If valid IMU data and states initialised, predict states and covariances
if (statesInitialised) {
if (_ekf->statesInitialised) {
// Run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED();
_ekf->UpdateStrapdownEquationsNED();
#if 0
// debug code - could be tunred into a filter mnitoring/watchdog function
float tempQuat[4];
@ -842,20 +860,20 @@ FixedwingEstimator::task_main() @@ -842,20 +860,20 @@ FixedwingEstimator::task_main()
#endif
// store the predicted states for subsequent use by measurement fusion
StoreStates(IMUmsec);
_ekf->StoreStates(IMUmsec);
// Check if on ground - status is used by covariance prediction
OnGroundCheck();
_ekf->OnGroundCheck();
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + dVelIMU;
dt += dtIMU;
_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
dt += _ekf->dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
CovariancePrediction(dt);
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) {
_ekf->CovariancePrediction(dt);
_ekf->summedDelAng = _ekf->summedDelAng.zero();
_ekf->summedDelVel = _ekf->summedDelVel.zero();
dt = 0.0f;
}
@ -865,79 +883,79 @@ FixedwingEstimator::task_main() @@ -865,79 +883,79 @@ FixedwingEstimator::task_main()
// Fuse GPS Measurements
if (newDataGps && _gps_initialized) {
// Convert GPS measurements to Pos NE, hgt and Vel NED
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_m_s;
velNED[2] = _gps.vel_d_m_s;
calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
_ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
posNE[0] = posNED[0];
posNE[1] = posNED[1];
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
// set fusion flags
fuseVelData = true;
fusePosData = true;
_ekf->fuseVelData = true;
_ekf->fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
// run the fusion step
FuseVelposNED();
_ekf->FuseVelposNED();
} else if (statesInitialised) {
} else if (_ekf->statesInitialised) {
// Convert GPS measurements to Pos NE, hgt and Vel NED
velNED[0] = 0.0f;
velNED[1] = 0.0f;
velNED[2] = 0.0f;
posNED[0] = 0.0f;
posNED[1] = 0.0f;
posNED[2] = 0.0f;
posNE[0] = posNED[0];
posNE[1] = posNED[1];
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
_ekf->velNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f;
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
// set fusion flags
fuseVelData = true;
fusePosData = true;
_ekf->fuseVelData = true;
_ekf->fusePosData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
// run the fusion step
FuseVelposNED();
_ekf->FuseVelposNED();
} else {
fuseVelData = false;
fusePosData = false;
_ekf->fuseVelData = false;
_ekf->fusePosData = false;
}
if (newAdsData && statesInitialised) {
if (newAdsData && _ekf->statesInitialised) {
// Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
fuseHgtData = true;
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
_ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
// run the fusion step
FuseVelposNED();
_ekf->FuseVelposNED();
} else {
fuseHgtData = false;
_ekf->fuseHgtData = false;
}
// Fuse Magnetometer Measurements
if (newDataMag && statesInitialised) {
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
if (newDataMag && _ekf->statesInitialised) {
_ekf->fuseMagData = true;
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
} else {
fuseMagData = false;
_ekf->fuseMagData = false;
}
if (statesInitialised) FuseMagnetometer();
if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
// Fuse Airspeed Measurements
if (newAdsData && statesInitialised && VtasMeas > 8.0f) {
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
_ekf->fuseVtasData = true;
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
_ekf->FuseAirspeed();
} else {
fuseVtasData = false;
_ekf->fuseVtasData = false;
}
// Publish results
@ -954,7 +972,7 @@ FixedwingEstimator::task_main() @@ -954,7 +972,7 @@ FixedwingEstimator::task_main()
// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
math::Quaternion q(states[0], states[1], states[2], states[3]);
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
@ -962,10 +980,10 @@ FixedwingEstimator::task_main() @@ -962,10 +980,10 @@ FixedwingEstimator::task_main()
_att.R[i][j] = R(i, j);
_att.timestamp = last_sensor_timestamp;
_att.q[0] = states[0];
_att.q[1] = states[1];
_att.q[2] = states[2];
_att.q[3] = states[3];
_att.q[0] = _ekf->states[0];
_att.q[1] = _ekf->states[1];
_att.q[2] = _ekf->states[2];
_att.q[3] = _ekf->states[3];
_att.q_valid = true;
_att.R_valid = true;
@ -974,13 +992,13 @@ FixedwingEstimator::task_main() @@ -974,13 +992,13 @@ FixedwingEstimator::task_main()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = angRate.x - states[10];
_att.pitchspeed = angRate.y - states[11];
_att.yawspeed = angRate.z - states[12];
_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
// gyro offsets
_att.rate_offsets[0] = states[10];
_att.rate_offsets[1] = states[11];
_att.rate_offsets[2] = states[12];
_att.rate_offsets[0] = _ekf->states[10];
_att.rate_offsets[1] = _ekf->states[11];
_att.rate_offsets[2] = _ekf->states[12];
/* lazily publish the attitude only once available */
if (_att_pub > 0) {
@ -993,20 +1011,15 @@ FixedwingEstimator::task_main() @@ -993,20 +1011,15 @@ FixedwingEstimator::task_main()
}
}
if (!isfinite(states[0])) {
print_status();
_exit(0);
}
if (_gps_initialized) {
_local_pos.timestamp = last_sensor_timestamp;
_local_pos.x = states[7];
_local_pos.y = states[8];
_local_pos.z = states[9];
_local_pos.x = _ekf->states[7];
_local_pos.y = _ekf->states[8];
_local_pos.z = _ekf->states[9];
_local_pos.vx = states[4];
_local_pos.vy = states[5];
_local_pos.vz = states[6];
_local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5];
_local_pos.vz = _ekf->states[6];
_local_pos.xy_valid = _gps_initialized;
_local_pos.z_valid = true;
@ -1107,9 +1120,10 @@ FixedwingEstimator::start() @@ -1107,9 +1120,10 @@ FixedwingEstimator::start()
return OK;
}
void print_status()
void
FixedwingEstimator::print_status()
{
math::Quaternion q(states[0], states[1], states[2], states[3]);
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
@ -1125,30 +1139,30 @@ void print_status() @@ -1125,30 +1139,30 @@ void print_status()
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z);
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)dAngIMU.x, (double)dAngIMU.y, (double)dAngIMU.z, (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]);
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]);
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]);
printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]);
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]);
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]);
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]);
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
printf("states: %s %s %s %s %s %s %s %s %s %s\n",
(statesInitialised) ? "INITIALIZED" : "NON_INIT",
(onGround) ? "ON_GROUND" : "AIRBORNE",
(fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(fusePosData) ? "FUSE_POS" : "INH_POS",
(fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
(fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
(_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
(_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
(_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
(_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
}
int trip_nan() {
int FixedwingEstimator::trip_nan() {
int ret = 0;
@ -1166,7 +1180,7 @@ int trip_nan() { @@ -1166,7 +1180,7 @@ int trip_nan() {
float nan_val = 0.0f / 0.0f;
warnx("system not armed, tripping state vector with NaN values");
states[5] = nan_val;
_ekf->states[5] = nan_val;
usleep(100000);
// warnx("tripping covariance #1 with NaN values");
@ -1178,15 +1192,15 @@ int trip_nan() { @@ -1178,15 +1192,15 @@ int trip_nan() {
// usleep(100000);
warnx("tripping covariance #3 with NaN values");
P[3][3] = nan_val; // covariance matrix
_ekf->P[3][3] = nan_val; // covariance matrix
usleep(100000);
warnx("tripping Kalman gains with NaN values");
Kfusion[0] = nan_val; // Kalman gains
_ekf->Kfusion[0] = nan_val; // Kalman gains
usleep(100000);
warnx("tripping stored states[0] with NaN values");
storedStates[0][0] = nan_val;
_ekf->storedStates[0][0] = nan_val;
usleep(100000);
warnx("\nDONE - FILTER STATE:");
@ -1234,7 +1248,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) @@ -1234,7 +1248,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
if (estimator::g_estimator) {
warnx("running");
print_status();
estimator::g_estimator->print_status();
exit(0);
@ -1245,7 +1259,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) @@ -1245,7 +1259,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
if (!strcmp(argv[1], "trip")) {
if (estimator::g_estimator) {
int ret = trip_nan();
int ret = estimator::g_estimator->trip_nan();
exit(ret);

Write Preview
Loading…
Cancel
Save