dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #43 from PX4/pr-baroHeightOffset 

Correct for sensor noise and baro offset during alignment
This commit is contained in:
Lorenz Meier 2016-02-10 08:02:48 +01:00
parent a6515543da fff2bd50f6
commit 0a9f7e58db
5 changed files with 102 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
EKF/ekf.cpp
View File

@ -196,73 +196,90 @@ 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;
// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)
resetVelocity();
// initialise the state covariance matrix
initialiseCovariance();
return true;
}
}
void Ekf::predictState()

9
EKF/ekf.h
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;
@ -174,6 +181,8 @@ private:
void resetPosition();
void resetHeight();
void makeCovSymetrical();
void limitCov();

25
EKF/ekf_helper.cpp
View File

@ -54,7 +54,7 @@ void Ekf::resetVelocity()
// if we have a valid GPS measurement use it to initialise velocity states
gpsSample gps_newest = _gps_buffer.get_newest();
if (_time_last_imu - gps_newest.time_us < 100000) {
if (_time_last_imu - gps_newest.time_us < 400000) {
_state.vel = gps_newest.vel;
} else {
@ -66,19 +66,32 @@ void Ekf::resetVelocity()
// gps measurement then use for position initialisation
void Ekf::resetPosition()
{
// if we have a valid GPS measurement use it to initialise position states
// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
gpsSample gps_newest = _gps_buffer.get_newest();
if (_time_last_imu - gps_newest.time_us < 100000) {
_state.pos(0) = gps_newest.pos(0);
_state.pos(1) = gps_newest.pos(1);
float time_delay = 1e-6f * (float)(_time_last_imu - gps_newest.time_us);
if (time_delay < 0.4f) {
_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
} else {
// XXX use the value of the last known position
}
}
// Reset height state using the last baro measurement
void Ekf::resetHeight()
{
// if we have a valid height measurement, use it to initialise the vertical position state
baroSample baro_newest = _baro_buffer.get_newest();
_state.pos(2) = -baro_newest.hgt;
if (_time_last_imu - baro_newest.time_us < 200000) {
_state.pos(2) = _baro_at_alignment - baro_newest.hgt;
} else {
// XXX use the value of the last known position
}
}
#if defined(__PX4_POSIX) && !defined(__PX4_QURT)

3
EKF/gps_checks.cpp
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;
}

2
EKF/vel_pos_fusion.cpp
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];