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
Split 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 @@ @@ -1,5 +1,8 @@
#include "estimator.h"
// For debugging only
#include <stdio.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
@ -32,14 +35,15 @@ float posNED[3]; // North, East Down position (m) @@ -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 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 statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float innovVtas; // innovation output
float varInnovVtas; // innovation variance output
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 lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
@ -1125,6 +1129,8 @@ void FuseVelposNED() @@ -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()
@ -1586,13 +1592,15 @@ float sq(float valIn) @@ -1586,13 +1592,15 @@ float sq(float valIn)
}
// Store states in a history array along with time stamp
void StoreStates()
void StoreStates(uint64_t timestamp_ms)
{
static uint8_t storeIndex = 0;
if (storeIndex == data_buffer_size) storeIndex = 0;
for (uint8_t i=0; i<=n_states; i++) storedStates[i][storeIndex] = states[i];
statetimeStamp[storeIndex] = millis();
storeIndex = storeIndex + 1;
for (unsigned i=0; i<n_states; i++)
storedStates[i][storeIndex] = states[i];
statetimeStamp[storeIndex] = timestamp_ms;
storeIndex++;
if (storeIndex == data_buffer_size)
storeIndex = 0;
}
// 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 @@ -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;
}
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
float initQuat[4];
Vector3f initMagXYZ;
@ -1809,10 +1838,6 @@ void InitialiseFilter() @@ -1809,10 +1838,6 @@ void InitialiseFilter()
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;
// calculate initial velocities
float initvelNED[3];
calcvelNED(initvelNED, gpsCourse, gpsGndSpd, gpsVelD);
//store initial lat,long and height
latRef = gpsLat;
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 @@ -110,7 +110,6 @@ extern float EAS2TAS; // ratio f true to equivalent airspeed
// GPS input data variables
extern float gpsCourse;
extern float gpsGndSpd;
extern float gpsVelD;
extern float gpsLat;
extern float gpsLon;
@ -122,7 +121,7 @@ extern float baroHgt; @@ -122,7 +121,7 @@ extern float baroHgt;
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
void UpdateStrapdownEquationsNED();
@ -144,7 +143,7 @@ float sq(float valIn); @@ -144,7 +143,7 @@ float sq(float valIn);
void quatNorm(float quatOut[4], float quatIn[4]);
// 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
void RecallStates(float statesForFusion[n_states], uint32_t msec);
@ -169,7 +168,7 @@ void OnGroundCheck(); @@ -169,7 +168,7 @@ void OnGroundCheck();
void CovarianceInit();
void InitialiseFilter();
void InitialiseFilter(float initvelNED[3]);
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[]); @@ -92,7 +92,7 @@ extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]);
__EXPORT uint32_t millis();
static uint64_t last_run = 0;
static uint32_t IMUmsec = 0;
static uint64_t IMUmsec = 0;
uint32_t millis()
{
@ -383,11 +383,11 @@ FixedwingEstimator::task_main() @@ -383,11 +383,11 @@ FixedwingEstimator::task_main()
/* rate limit gyro updates to 50 Hz */
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_gyro_sub, 17);
orb_set_interval(_gyro_sub, 6);
#else
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_sensor_combined_sub, 17);
orb_set_interval(_sensor_combined_sub, 6);
#endif
parameters_update();
@ -459,6 +459,10 @@ FixedwingEstimator::task_main() @@ -459,6 +459,10 @@ FixedwingEstimator::task_main()
perf_count(_perf_gyro);
/**
* PART ONE: COLLECT ALL DATA
**/
hrt_abstime last_sensor_timestamp;
/* load local copies */
@ -498,7 +502,10 @@ FixedwingEstimator::task_main() @@ -498,7 +502,10 @@ FixedwingEstimator::task_main()
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
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;
@ -522,7 +529,7 @@ FixedwingEstimator::task_main() @@ -522,7 +529,7 @@ FixedwingEstimator::task_main()
last_run = _sensor_combined.timestamp;
/* guard against too large deltaT's */
if (deltaT > 1.0f)
if (deltaT > 1.0f || deltaT < 0.000001f)
deltaT = 0.01f;
// Always store data, independent of init status
@ -549,41 +556,13 @@ FixedwingEstimator::task_main() @@ -549,41 +556,13 @@ FixedwingEstimator::task_main()
#endif
if (_initialized) {
/* predict states and covariances */
/* run the strapdown INS every sensor update */
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);
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
baroHgt = _baro.altitude;
VtasMeas = _airspeed.true_airspeed_m_s;
}
bool gps_updated;
@ -592,65 +571,36 @@ FixedwingEstimator::task_main() @@ -592,65 +571,36 @@ FixedwingEstimator::task_main()
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps);
if (_gps.fix_type > 2) {
if (_gps.fix_type < 3) {
gps_updated = false;
} else {
/* fuse GPS updates */
//_gps.timestamp / 1e3;
GPSstatus = _gps.fix_type;
gpsCourse = _gps.cog_rad;
gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s);
gpsVelD = _gps.vel_d_m_s;
velNED[0] = _gps.vel_n_m_s;
velNED[1] = _gps.vel_e_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);
gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
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 */
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
}
baroHgt = _baro.altitude;
} else {
/* do not fuse anything, we got no position / vel update */
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
// Could use a blend of GPS and baro alt data if desired
hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt;
}
#ifndef SENSOR_COMBINED_SUB
@ -689,41 +639,107 @@ FixedwingEstimator::task_main() @@ -689,41 +639,107 @@ FixedwingEstimator::task_main()
#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;
RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms));
FuseMagnetometer();
/* 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;
}
}
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 {
fuseMagData = false;
fuseVelData = false;
fusePosData = false;
}
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated && _initialized) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
if (baro_updated && _initialized) {
fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
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;
RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
}
} else {
fuseVtasData = false;
}
if (airspeed_updated && _initialized
&& _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) {
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
} else {
fuseVtasData = false;
}
// Publish results
if (_initialized) {
// State vector:
@ -894,6 +910,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) @@ -894,6 +910,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
// 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 (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]);

Write Preview
Loading…
Cancel
Save