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

Merge pull request #7 from PX4/ekf2_fixes 

Ekf2 fixes
master
Roman Bapst 9 years ago
parent
b542e2b49b 2af5856361
commit
749156d4aa
5 changed files with 182 additions and 144 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. 108
      EKF/ekf.cpp
  2. 7
      EKF/ekf.h
  3. 2
      EKF/ekf_helper.cpp
  4. 8
      EKF/estimator_base.cpp
  5. 181
      EKF/vel_pos_fusion.cpp

108
EKF/ekf.cpp
Unescape View File

@ -47,7 +47,8 @@ Ekf::Ekf():
_earth_rate_initialised(false), _earth_rate_initialised(false),
_fuse_height(false), _fuse_height(false),
_fuse_pos(false), _fuse_pos(false),
_fuse_vel(false), _fuse_hor_vel(false),
_fuse_vert_vel(false),
_mag_fuse_index(0), _mag_fuse_index(0),
_time_last_fake_gps(0) _time_last_fake_gps(0)
{ {
@ -67,12 +68,15 @@ Ekf::~Ekf()
bool Ekf::update() bool Ekf::update()
{ {
bool ret = false; // indicates if there has been an update bool ret = false; // indicates if there has been an update
if (!_filter_initialised) { if (!_filter_initialised) {
_filter_initialised = initialiseFilter(); _filter_initialised = initialiseFilter();
if (!_filter_initialised) { if (!_filter_initialised) {
return false; return false;
} }
} }
//printStates(); //printStates();
//printStatesFast(); //printStatesFast();
// prediction // prediction
@ -85,9 +89,9 @@ bool Ekf::update()
// measurement updates // measurement updates
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
fuseHeading(); //fuseHeading();
//fuseMag(_mag_fuse_index); fuseMag(_mag_fuse_index);
//_mag_fuse_index = (_mag_fuse_index + 1) % 3; _mag_fuse_index = (_mag_fuse_index + 1) % 3;
} }
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
@ -96,16 +100,19 @@ bool Ekf::update()
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) { if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
_fuse_pos = true; _fuse_pos = true;
_fuse_vel = true; _fuse_vert_vel = true;
_fuse_hor_vel = true;
} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) { } else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
_fuse_vel = true; // if gps does not update then fake position and horizontal veloctiy measurements
_fuse_hor_vel = true; // we only fake horizontal velocity because we still have baro
_gps_sample_delayed.vel.setZero(); _gps_sample_delayed.vel.setZero();
} }
if (_fuse_height || _fuse_pos || _fuse_vel) { if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight(); fuseVelPosHeight();
_fuse_vel = _fuse_pos = _fuse_height = false; _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
} }
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) { if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
@ -133,7 +140,7 @@ bool Ekf::initialiseFilter(void)
_state.mag_B.setZero(); _state.mag_B.setZero();
_state.wind_vel.setZero(); _state.wind_vel.setZero();
// get initial roll and pitch estimate from accel vector, assuming vehicle is static // get initial roll and pitch estimate from accel vector, assuming vehicle is static
Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt; Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;
float pitch = 0.0f; float pitch = 0.0f;
@ -146,32 +153,44 @@ bool Ekf::initialiseFilter(void)
roll = -asinf(accel_init(1) / cosf(pitch)); roll = -asinf(accel_init(1) / cosf(pitch));
} }
matrix::Euler<float> euler_init(roll, pitch, 0.0f); matrix::Euler<float> euler_init(roll, pitch, 0.0f);
// Get the latest magnetic field measurement.
// If we don't have a measurement then we cannot initialise the filter
magSample mag_init = _mag_buffer.get_newest();
// Get the latest magnetic field measurement.
// If we don't have a measurement then we cannot initialise the filter
magSample mag_init = _mag_buffer.get_newest();
if (mag_init.time_us == 0) { if (mag_init.time_us == 0) {
return false; return false;
} }
// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination // rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
// TODO use declination if available // TODO use declination if available
matrix::Dcm<float> R_to_earth_zeroyaw(euler_init); matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag; Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
float declination = 0.0f; float declination = 0.0f;
euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0)); euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
// calculate initial quaternion states // calculate initial quaternion states
_state.quat_nominal = Quaternion(euler_init); _state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// calculate initial earth magnetic field states // calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init); matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init.mag; _state.mag_I = R_to_earth * mag_init.mag;
resetVelocity(); resetVelocity();
resetPosition(); resetPosition();
// initialize vertical position with newest baro measurement
baroSample baro_init = _baro_buffer.get_newest();
if (baro_init.time_us == 0) {
return false;
}
_state.pos(2) = -baro_init.hgt;
_output_new.pos(2) = -baro_init.hgt;
initialiseCovariance(); initialiseCovariance();
return true; return true;
@ -181,14 +200,15 @@ void Ekf::predictState()
{ {
if (!_earth_rate_initialised) { if (!_earth_rate_initialised) {
if (_gps_initialised) { if (_gps_initialised) {
calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad ); calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad);
_earth_rate_initialised = true; _earth_rate_initialised = true;
} }
} }
// attitude error state prediciton // attitude error state prediciton
matrix::Dcm<float> R_to_earth(_state.quat_nominal); // transformation matrix from body to world frame matrix::Dcm<float> R_to_earth(_state.quat_nominal); // transformation matrix from body to world frame
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED *
_imu_sample_delayed.delta_ang_dt;
Quaternion dq; // delta quaternion since last update Quaternion dq; // delta quaternion since last update
dq.from_axis_angle(corrected_delta_ang); dq.from_axis_angle(corrected_delta_ang);
_state.quat_nominal = dq * _state.quat_nominal; _state.quat_nominal = dq * _state.quat_nominal;
@ -245,8 +265,7 @@ void Ekf::calculateOutputStates()
_imu_updated = false; _imu_updated = false;
} }
if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed)) if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed)) {
{
return; return;
} }
@ -259,6 +278,7 @@ void Ekf::calculateOutputStates()
if (q_error(0) >= 0.0f) { if (q_error(0) >= 0.0f) {
scalar = -2.0f; scalar = -2.0f;
} else { } else {
scalar = 2.0f; scalar = 2.0f;
} }
@ -292,7 +312,8 @@ void Ekf::printStates()
if (counter % 50 == 0) { if (counter % 50 == 0) {
printf("quaternion\n"); printf("quaternion\n");
for(int i = 0; i < 4; i++) {
for (int i = 0; i < 4; i++) {
printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i)); printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i));
} }
@ -300,31 +321,38 @@ void Ekf::printStates()
printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0)); printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));
printf("vel\n"); printf("vel\n");
for(int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
printf("v %i %.5f\n", i, (double)_state.vel(i)); printf("v %i %.5f\n", i, (double)_state.vel(i));
} }
printf("pos\n"); printf("pos\n");
for(int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
printf("p %i %.5f\n", i, (double)_state.pos(i)); printf("p %i %.5f\n", i, (double)_state.pos(i));
} }
printf("gyro_scale\n"); printf("gyro_scale\n");
for(int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i)); printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
} }
printf("mag earth\n"); printf("mag earth\n");
for(int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
printf("mI %i %.5f\n", i, (double)_state.mag_I(i)); printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
} }
printf("mag bias\n"); printf("mag bias\n");
for(int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
printf("mB %i %.5f\n", i, (double)_state.mag_B(i)); printf("mB %i %.5f\n", i, (double)_state.mag_B(i));
} }
counter = 0; counter = 0;
} }
counter++; counter++;
} }
@ -335,21 +363,25 @@ void Ekf::printStatesFast()
if (counter_fast % 50 == 0) { if (counter_fast % 50 == 0) {
printf("quaternion\n"); printf("quaternion\n");
for(int i = 0; i < 4; i++) {
for (int i = 0; i < 4; i++) {
printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i)); printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i));
} }
printf("vel\n"); printf("vel\n");
for(int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
printf("v %i %.5f\n", i, (double)_output_new.vel(i)); printf("v %i %.5f\n", i, (double)_output_new.vel(i));
} }
printf("pos\n"); printf("pos\n");
for(int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
printf("p %i %.5f\n", i, (double)_output_new.pos(i)); printf("p %i %.5f\n", i, (double)_output_new.pos(i));
} }
counter_fast = 0; counter_fast = 0;
} }
counter_fast++; counter_fast++;
} }

7
EKF/ekf.h
Unescape View File

@ -60,9 +60,10 @@ private:
bool _filter_initialised; bool _filter_initialised;
bool _earth_rate_initialised; bool _earth_rate_initialised;
bool _fuse_height; // true if there is new baro data bool _fuse_height; // baro height data should be fused
bool _fuse_pos; // true if there is new position data from gps bool _fuse_pos; // gps position data should be fused
bool _fuse_vel; // true if there is new velocity data from gps bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused
bool _fuse_vert_vel; // gps vertical velocity measurement should be fused
uint8_t _mag_fuse_index; // counter for sequential mag axis fusion uint8_t _mag_fuse_index; // counter for sequential mag axis fusion

2
EKF/ekf_helper.cpp
Unescape View File

@ -78,7 +78,7 @@ void Ekf::resetPosition()
} }
baroSample baro_newest = _baro_buffer.get_newest(); baroSample baro_newest = _baro_buffer.get_newest();
_state.pos(2) = baro_newest.hgt; _state.pos(2) = -baro_newest.hgt;
} }
#if defined(__PX4_POSIX) && !defined(__PX4_QURT) #if defined(__PX4_POSIX) && !defined(__PX4_QURT)

8
EKF/estimator_base.cpp
Unescape View File

@ -145,7 +145,7 @@ void EstimatorBase::setMagData(uint64_t time_usec, float *data)
mag_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; mag_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_mag = mag_sample_new.time_us; _time_last_mag = time_usec;
memcpy(&mag_sample_new.mag._data[0], data, sizeof(mag_sample_new.mag._data)); memcpy(&mag_sample_new.mag._data[0], data, sizeof(mag_sample_new.mag._data));
@ -168,7 +168,7 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_gps = gps_sample_new.time_us; _time_last_gps = time_usec;
_last_valid_gps_time_us = hrt_absolute_time(); _last_valid_gps_time_us = hrt_absolute_time();
@ -200,7 +200,7 @@ void EstimatorBase::setBaroData(uint64_t time_usec, float *data)
baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000; baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000;
baro_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; baro_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_baro = baro_sample_new.time_us; _time_last_baro = time_usec;
baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us); baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us);
@ -216,7 +216,7 @@ void EstimatorBase::setAirspeedData(uint64_t time_usec, float *data)
airspeed_sample_new.time_us -= _params.airspeed_delay_ms * 1000; airspeed_sample_new.time_us -= _params.airspeed_delay_ms * 1000;
airspeed_sample_new.time_us = time_usec -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; airspeed_sample_new.time_us = time_usec -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_airspeed = airspeed_sample_new.time_us; _time_last_airspeed = time_usec;
_airspeed_buffer.push(airspeed_sample_new); _airspeed_buffer.push(airspeed_sample_new);
} }

181
EKF/vel_pos_fusion.cpp
Unescape View File

@ -41,64 +41,69 @@
#include "ekf.h" #include "ekf.h"
void Ekf::fuseVelPosHeight() void Ekf::fuseVelPosHeight()
{ {
bool fuse_map[6] = {}; bool fuse_map[6] = {};
float innovations[6] = {}; float innovations[6] = {};
float R[6] = {}; float R[6] = {};
float Kfusion[24] = {}; float Kfusion[24] = {};
// calculate innovations
if (_fuse_vel) { // calculate innovations
fuse_map[0] = fuse_map[1] = fuse_map[2] = true; if (_fuse_hor_vel) {
innovations[0] = _state.vel(0) - _gps_sample_delayed.vel(0); fuse_map[0] = fuse_map[1] = true;
innovations[1] = _state.vel(1) - _gps_sample_delayed.vel(1); innovations[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
innovations[2] = _state.vel(2) - _gps_sample_delayed.vel(2); innovations[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
R[0] = _params.gps_vel_noise; R[0] = _params.gps_vel_noise;
R[1] = _params.gps_vel_noise; R[1] = _params.gps_vel_noise;
R[2] = _params.gps_vel_noise; }
}
if (_fuse_vert_vel) {
if (_fuse_pos) { fuse_map[2] = true;
fuse_map[3] = fuse_map[4] = true; innovations[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
innovations[3] = _state.pos(0) - _gps_sample_delayed.pos(0); R[2] = _params.gps_vel_noise;
innovations[4] = _state.pos(1) - _gps_sample_delayed.pos(1); }
R[3] = _params.gps_pos_noise;
R[4] = _params.gps_pos_noise; if (_fuse_pos) {
fuse_map[3] = fuse_map[4] = true;
} innovations[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
innovations[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
if (_fuse_height) { R[3] = _params.gps_pos_noise;
fuse_map[5] = true; R[4] = _params.gps_pos_noise;
innovations[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); // baro measurement has inversed z axis
R[5] = _params.baro_noise; }
}
if (_fuse_height) {
// XXX Do checks here fuse_map[5] = true;
innovations[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); // baro measurement has inversed z axis
for (unsigned obs_index = 0; obs_index < 6; obs_index++) { R[5] = _params.baro_noise;
if (!fuse_map[obs_index]) { }
continue;
} // XXX Do checks here
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
if (!fuse_map[obs_index]) {
// compute the innovation variance SK = HPH + R continue;
float S = P[state_index][state_index] + R[obs_index]; }
S = 1.0f / S;
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state
// calculate kalman gain K = PHS
for (int row = 0; row < 24; row++) { // compute the innovation variance SK = HPH + R
Kfusion[row] = P[row][state_index] * S; float S = P[state_index][state_index] + R[obs_index];
} S = 1.0f / S;
// by definition the angle error state is zero at the fusion time // calculate kalman gain K = PHS
_state.ang_error.setZero(); for (int row = 0; row < 24; row++) {
Kfusion[row] = P[row][state_index] * S;
// fuse the observation }
fuse(Kfusion, innovations[obs_index]);
// by definition the angle error state is zero at the fusion time
// correct the nominal quaternion _state.ang_error.setZero();
Quaternion dq;
// fuse the observation
fuse(Kfusion, innovations[obs_index]);
// correct the nominal quaternion
Quaternion dq;
dq.from_axis_angle(_state.ang_error); dq.from_axis_angle(_state.ang_error);
_state.quat_nominal = dq * _state.quat_nominal; _state.quat_nominal = dq * _state.quat_nominal;
_state.quat_nominal.normalize(); _state.quat_nominal.normalize();
@ -120,43 +125,43 @@
makeSymmetrical(); makeSymmetrical();
limitCov(); limitCov();
} }
} }
void Ekf::fuse(float *K, float innovation) void Ekf::fuse(float *K, float innovation)
{ {
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
_state.ang_error(i) = _state.ang_error(i) - K[i] * innovation; _state.ang_error(i) = _state.ang_error(i) - K[i] * innovation;
} }
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
_state.vel(i) = _state.vel(i) - K[i + 3] * innovation; _state.vel(i) = _state.vel(i) - K[i + 3] * innovation;
} }
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
_state.pos(i) = _state.pos(i) - K[i + 6] * innovation; _state.pos(i) = _state.pos(i) - K[i + 6] * innovation;
} }
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation; _state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation;
} }
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
_state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation; _state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation;
} }
_state.accel_z_bias -= K[15] * innovation; _state.accel_z_bias -= K[15] * innovation;
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation; _state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
} }
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
_state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation; _state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation;
} }
for (unsigned i = 0; i < 2; i++) { for (unsigned i = 0; i < 2; i++) {
_state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation; _state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation;
} }
} }

Write Preview
Loading…
Cancel
Save