EKF: Correct for sensor noise and baro offset during alignment

This commit is contained in:
Paul Riseborough 2016-02-10 10:02:30 +11:00
parent a6515543da
commit 40e174b81c
5 changed files with 81 additions and 57 deletions

View File

@ -196,73 +196,89 @@ bool Ekf::update()
bool Ekf::initialiseFilter(void)
{
_state.ang_error.setZero();
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
_state.accel_z_bias = 0.0f;
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter
// 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;
// Sum the IMU delta angle measurements
_delVel_sum += _imu_down_sampled.delta_vel;
float pitch = 0.0f;
float roll = 0.0f;
if (accel_init.norm() > 0.001f) {
accel_init.normalize();
pitch = asinf(accel_init(0));
roll = -asinf(accel_init(1) / cosf(pitch));
}
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
// Sum the magnetometer measurements
magSample mag_init = _mag_buffer.get_newest();
if (mag_init.time_us == 0) {
return false;
if (mag_init.time_us != 0) {
_mag_counter ++;
_mag_sum += mag_init.mag;
}
// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
// TODO use declination if available
matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
float declination = 0.0f;
euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
// calculate initial quaternion states
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// TODO replace this with a conditional test based on fitered angle error states.
_control_status.flags.angle_align = true;
// calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init.mag;
resetVelocity();
resetPosition();
// Sum the barometer measurements
// initialize vertical position with newest baro measurement
baroSample baro_init = _baro_buffer.get_newest();
if (baro_init.time_us == 0) {
return false;
if (baro_init.time_us != 0) {
_baro_counter ++;
_baro_sum += baro_init.hgt;
}
_state.pos(2) = -baro_init.hgt;
_output_new.pos(2) = -baro_init.hgt;
// check to see if we have enough measruements and return false if not
if (_baro_counter < 10 || _mag_counter < 10) {
return false;
initialiseCovariance();
} else {
// Zero all of the states
_state.ang_error.setZero();
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
_state.accel_z_bias = 0.0f;
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
return true;
// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
float pitch = 0.0f;
float roll = 0.0f;
if (_delVel_sum.norm() > 0.001f) {
_delVel_sum.normalize();
pitch = asinf(_delVel_sum(0));
roll = -asinf(_delVel_sum(1) / cosf(pitch));
} else {
return false;
}
// calculate the averaged magnetometer reading
Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter)));
// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
// TODO use declination if available
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init;
float declination = 0.0f;
euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
// calculate initial quaternion states
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// TODO replace this with a conditional test based on fitered angle error states.
_control_status.flags.angle_align = true;
// calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init;
// calculate the averaged barometer reading
_baro_at_alignment = _baro_sum / (float)_baro_counter;
resetVelocity();
resetPosition();
initialiseCovariance();
return true;
}
}
void Ekf::predictState()

View File

@ -145,6 +145,13 @@ private:
uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec)
float _gps_alt_ref = 0.0f; // WGS-84 height (m)
// Variables used to initialise the filter states
uint8_t _baro_counter = 0; // number of baro samples averaged
float _baro_sum = 0.0f; // summed baro measurement
uint8_t _mag_counter = 0; // number of magnetometer samples averaged
Vector3f _mag_sum = {}; // summed magnetometer measurement
Vector3f _delVel_sum = {}; // summed delta velocity
float _baro_at_alignment; // baro offset relative to alignment position
gps_check_fail_status_u _gps_check_fail_status;

View File

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

View File

@ -63,7 +63,8 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
double lat = gps->lat / 1.0e7;
double lon = gps->lon / 1.0e7;
map_projection_init(&_pos_ref, lat, lon);
_gps_alt_ref = gps->alt / 1e3f;
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
_NED_origin_initialised = true;
_last_gps_origin_time_us = _time_last_imu;
}

View File

@ -100,7 +100,7 @@ void Ekf::fuseVelPosHeight()
if (_fuse_height) {
fuse_map[5] = true;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) - (-_baro_sample_delayed.hgt);
_vel_pos_innov[5] = _state.pos(2) - (_baro_at_alignment -_baro_sample_delayed.hgt);
// observation variance - user parameter defined
R[5] = fmaxf(_params.baro_noise, 0.01f);
R[5] = R[5] * R[5];