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

Rewrote the filter mainloop to match the order in the offboard simulator, added a number of scaling fixes, initializing all structs correctly

sbg
Lorenz Meier 11 years ago
parent
dba229ffa5
commit
70ffa27acd
3 changed files with 163 additions and 122 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 49
      src/modules/fw_att_pos_estimator/estimator.cpp
  2. 7
      src/modules/fw_att_pos_estimator/estimator.h
  3. 229
      src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp

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

@ -1,5 +1,8 @@
#include "estimator.h" #include "estimator.h"
// For debugging only
#include <stdio.h>
// Global variables // Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates 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 KHP[n_states][n_states]; // intermediate result used for covariance updates
@ -32,14 +35,15 @@ float posNED[3]; // North, East Down position (m)
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements 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 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 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 innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float innovVtas; // innovation output float innovVtas; // innovation output
float varInnovVtas; // innovation variance output float varInnovVtas; // innovation variance output
float VtasMeas; // true airspeed measurement (m/s) float VtasMeas; // true airspeed measurement (m/s)
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
float latRef; // WGS-84 latitude of reference point (rad) float latRef; // WGS-84 latitude of reference point (rad)
float lonRef; // WGS-84 longitude of reference point (rad) float lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m) float hgtRef; // WGS-84 height of reference point (m)
@ -1125,6 +1129,8 @@ void FuseVelposNED()
} }
} }
} }
//printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
} }
void FuseMagnetometer() void FuseMagnetometer()
@ -1586,13 +1592,15 @@ float sq(float valIn)
} }
// Store states in a history array along with time stamp // Store states in a history array along with time stamp
void StoreStates() void StoreStates(uint64_t timestamp_ms)
{ {
static uint8_t storeIndex = 0; static uint8_t storeIndex = 0;
if (storeIndex == data_buffer_size) storeIndex = 0; for (unsigned i=0; i<n_states; i++)
for (uint8_t i=0; i<=n_states; i++) storedStates[i][storeIndex] = states[i]; storedStates[i][storeIndex] = states[i];
statetimeStamp[storeIndex] = millis(); statetimeStamp[storeIndex] = timestamp_ms;
storeIndex = storeIndex + 1; storeIndex++;
if (storeIndex == data_buffer_size)
storeIndex = 0;
} }
// Output the state vector stored at the time that best matches that specified by msec // Output the state vector stored at the time that best matches that specified by msec
@ -1791,8 +1799,29 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
} }
void InitialiseFilter() void InitialiseFilter(float initvelNED[3])
{ {
// Do the data structure init
for (unsigned i = 0; i < n_states; i++) {
for (unsigned j = 0; j < n_states; j++) {
KH[i][j] = 0.0f; // intermediate result used for covariance updates
KHP[i][j] = 0.0f; // intermediate result used for covariance updates
P[i][j] = 0.0f; // covariance matrix
}
Kfusion[i] = 0.0f; // Kalman gains
states[i] = 0.0f; // state matrix
}
for (unsigned i = 0; i < data_buffer_size; i++) {
for (unsigned j = 0; j < n_states; j++) {
}
statetimeStamp[i] = 0;
}
// Calculate initial filter quaternion states from raw measurements // Calculate initial filter quaternion states from raw measurements
float initQuat[4]; float initQuat[4];
Vector3f initMagXYZ; Vector3f initMagXYZ;
@ -1809,10 +1838,6 @@ void InitialiseFilter()
initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
// calculate initial velocities
float initvelNED[3];
calcvelNED(initvelNED, gpsCourse, gpsGndSpd, gpsVelD);
//store initial lat,long and height //store initial lat,long and height
latRef = gpsLat; latRef = gpsLat;
lonRef = gpsLon; lonRef = gpsLon;

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

@ -110,7 +110,6 @@ extern float EAS2TAS; // ratio f true to equivalent airspeed
// GPS input data variables // GPS input data variables
extern float gpsCourse; extern float gpsCourse;
extern float gpsGndSpd;
extern float gpsVelD; extern float gpsVelD;
extern float gpsLat; extern float gpsLat;
extern float gpsLon; extern float gpsLon;
@ -122,7 +121,7 @@ extern float baroHgt;
extern bool statesInitialised; extern bool statesInitialised;
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions const float covTimeStepMax = 0.02f; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions
void UpdateStrapdownEquationsNED(); void UpdateStrapdownEquationsNED();
@ -144,7 +143,7 @@ float sq(float valIn);
void quatNorm(float quatOut[4], float quatIn[4]); void quatNorm(float quatOut[4], float quatIn[4]);
// store staes along with system time stamp in msces // store staes along with system time stamp in msces
void StoreStates(); void StoreStates(uint64_t timestamp_ms);
// recall stste vector stored at closest time to the one specified by msec // recall stste vector stored at closest time to the one specified by msec
void RecallStates(float statesForFusion[n_states], uint32_t msec); void RecallStates(float statesForFusion[n_states], uint32_t msec);
@ -169,7 +168,7 @@ void OnGroundCheck();
void CovarianceInit(); void CovarianceInit();
void InitialiseFilter(); void InitialiseFilter(float initvelNED[3]);
uint32_t millis(); uint32_t millis();

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

@ -92,7 +92,7 @@ extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]);
__EXPORT uint32_t millis(); __EXPORT uint32_t millis();
static uint64_t last_run = 0; static uint64_t last_run = 0;
static uint32_t IMUmsec = 0; static uint64_t IMUmsec = 0;
uint32_t millis() uint32_t millis()
{ {
@ -383,11 +383,11 @@ FixedwingEstimator::task_main()
/* rate limit gyro updates to 50 Hz */ /* rate limit gyro updates to 50 Hz */
/* XXX remove this!, BUT increase the data buffer size! */ /* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_gyro_sub, 17); orb_set_interval(_gyro_sub, 6);
#else #else
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
/* XXX remove this!, BUT increase the data buffer size! */ /* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_sensor_combined_sub, 17); orb_set_interval(_sensor_combined_sub, 6);
#endif #endif
parameters_update(); parameters_update();
@ -459,6 +459,10 @@ FixedwingEstimator::task_main()
perf_count(_perf_gyro); perf_count(_perf_gyro);
/**
* PART ONE: COLLECT ALL DATA
**/
hrt_abstime last_sensor_timestamp; hrt_abstime last_sensor_timestamp;
/* load local copies */ /* load local copies */
@ -498,7 +502,10 @@ FixedwingEstimator::task_main()
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
lastAngRate = angRate; lastAngRate = angRate;
dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; // dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
dVelIMU.x = 0.0f;
dVelIMU.y = 0.0f;
dVelIMU.z = 0.0f;
lastAccel = accel; lastAccel = accel;
@ -522,7 +529,7 @@ FixedwingEstimator::task_main()
last_run = _sensor_combined.timestamp; last_run = _sensor_combined.timestamp;
/* guard against too large deltaT's */ /* guard against too large deltaT's */
if (deltaT > 1.0f) if (deltaT > 1.0f || deltaT < 0.000001f)
deltaT = 0.01f; deltaT = 0.01f;
// Always store data, independent of init status // Always store data, independent of init status
@ -549,41 +556,13 @@ FixedwingEstimator::task_main()
#endif #endif
if (_initialized) { bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
/* predict states and covariances */ if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
/* run the strapdown INS every sensor update */ perf_count(_perf_airspeed);
UpdateStrapdownEquationsNED();
/* store the predictions */
StoreStates();
/* evaluate if on ground */
OnGroundCheck();
/* prepare the delta angles and time used by the covariance prediction */
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + correctedDelVel;
dt += dtIMU;
/* predict the covairance if the total delta angle has exceeded the threshold
* or the time limit will be exceeded on the next measurement update
*/
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
CovariancePrediction();
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
dt = 0.0f;
}
}
bool baro_updated;
orb_check(_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
baroHgt = _baro.altitude; VtasMeas = _airspeed.true_airspeed_m_s;
} }
bool gps_updated; bool gps_updated;
@ -592,65 +571,36 @@ FixedwingEstimator::task_main()
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps); perf_count(_perf_gps);
if (_gps.fix_type > 2) { if (_gps.fix_type < 3) {
gps_updated = false;
} else {
/* fuse GPS updates */ /* fuse GPS updates */
//_gps.timestamp / 1e3; //_gps.timestamp / 1e3;
GPSstatus = _gps.fix_type; GPSstatus = _gps.fix_type;
gpsCourse = _gps.cog_rad; velNED[0] = _gps.vel_n_m_s;
gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s); velNED[1] = _gps.vel_e_m_s;
gpsVelD = _gps.vel_d_m_s; 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); gpsLat = math::radians(_gps.lat / (double)1e7);
gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
gpsHgt = _gps.alt / 1e3f; gpsHgt = _gps.alt / 1e3f;
if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { }
InitialiseFilter();
_initialized = true;
warnx("init done.");
continue;
}
if (_initialized) {
/* convert GPS measurements to horizontal NE, altitude and 3D velocity 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);
posNE[0] = posNED[0];
posNE[1] = posNED[1];
hgtMea = -posNED[2];
// set flags for further processing
fuseVelData = true;
fusePosData = true;
fuseHgtData = true;
/* recall states after adjusting for delays */
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
/* run the actual fusion */ }
FuseVelposNED();
}
} else { bool baro_updated;
orb_check(_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
/* do not fuse anything, we got no position / vel update */ baroHgt = _baro.altitude;
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
}
} else { // Could use a blend of GPS and baro alt data if desired
/* do not fuse anything, we got no position / vel update */ hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt;
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
} }
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
@ -689,41 +639,107 @@ FixedwingEstimator::task_main()
#endif #endif
if (_initialized) { }
/**
* PART TWO: EXECUTE THE FILTER
**/
if (hrt_elapsed_time(&start_time) > 500000 && !_initialized && (GPSstatus == 3)) {
InitialiseFilter(velNED);
_initialized = true;
warnx("init done.");
}
if (_initialized) {
/* predict states and covariances */
/* run the strapdown INS every sensor update */
UpdateStrapdownEquationsNED();
/* store the predictions */
StoreStates(IMUmsec);
/* evaluate if on ground */
OnGroundCheck();
fuseMagData = true; /* prepare the delta angles and time used by the covariance prediction */
RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); summedDelAng = summedDelAng + correctedDelAng;
FuseMagnetometer(); summedDelVel = summedDelVel + correctedDelVel;
dt += dtIMU;
/* predict the covairance if the total delta angle has exceeded the threshold
* or the time limit will be exceeded on the next measurement update
*/
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
CovariancePrediction();
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
dt = 0.0f;
} }
}
if (gps_updated && _initialized) {
/* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */
calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
posNE[0] = posNED[0];
posNE[1] = posNED[1];
// set flags for further processing
fuseVelData = true;
fusePosData = true;
/* recall states after adjusting for delays */
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
/* run the actual fusion */
FuseVelposNED();
} else { } else {
fuseMagData = false; fuseVelData = false;
fusePosData = false;
} }
bool airspeed_updated; if (baro_updated && _initialized) {
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated && _initialized) { fuseHgtData = true;
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); // recall states stored at time of measurement after adjusting for delays
perf_count(_perf_airspeed); RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
// run the fusion step
FuseVelposNED();
} else {
fuseHgtData = false;
}
if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { if (mag_updated && _initialized) {
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms));
VtasMeas = _airspeed.true_airspeed_m_s; } else {
fuseMagData = false;
}
if (_initialized) { if (_initialized) {
FuseMagnetometer();
}
fuseVtasData = true; if (airspeed_updated && _initialized
RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) {
FuseAirspeed();
}
} else {
fuseVtasData = false;
}
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
} else { } else {
fuseVtasData = false; fuseVtasData = false;
} }
// Publish results
if (_initialized) { if (_initialized) {
// State vector: // State vector:
@ -894,6 +910,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
printf("dt: %8.6f\n", dtIMU);
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 (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 (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 (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]);

Write Preview
Loading…
Cancel
Save