AHRS: added wind estimation code

this allows us to estimate the wind while we have GPS lock, then use
that estimate in the long term dead reckoning
This commit is contained in:
Andrew Tridgell 2012-08-11 15:55:53 +10:00
parent 20a30e3a6b
commit 2c7ab7c659
3 changed files with 105 additions and 18 deletions

View File

@ -102,6 +102,11 @@ public:
return true; return true;
} }
// return a wind estimation vector, in m/s
Vector3f wind_estimate(void) {
return _wind;
}
protected: protected:
// pointer to compass object, if available // pointer to compass object, if available
Compass * _compass; Compass * _compass;
@ -126,6 +131,9 @@ protected:
// radians/s/s // radians/s/s
float _gyro_drift_limit; float _gyro_drift_limit;
// estimated wind in m/s
Vector3f _wind;
// acceleration due to gravity in m/s/s // acceleration due to gravity in m/s/s
static const float _gravity = 9.80665; static const float _gravity = 9.80665;
}; };

View File

@ -51,6 +51,7 @@ const AP_Param::GroupInfo AP_AHRS_DCM::var_info[] PROGMEM = {
// @Range: 0.0 1.0 // @Range: 0.0 1.0
// @Increment: .01 // @Increment: .01
AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS_DCM, gps_gain, 1.0), AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS_DCM, gps_gain, 1.0),
AP_GROUPEND AP_GROUPEND
}; };
@ -295,6 +296,15 @@ AP_AHRS_DCM::_P_gain(float spin_rate)
return spin_rate/ToDeg(50); return spin_rate/ToDeg(50);
} }
// return true if we have and should use GPS
bool AP_AHRS_DCM::have_gps(void)
{
if (!_gps || _gps->status() != GPS::GPS_OK) {
return false;
}
return true;
}
// yaw drift correction using the compass or GPS // yaw drift correction using the compass or GPS
// this function prodoces the _omega_yaw_P vector, and also // this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate // contributes to the _omega_I.z long term yaw drift estimate
@ -318,7 +328,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
new_value = true; new_value = true;
yaw_error = yaw_error_compass(); yaw_error = yaw_error_compass();
} }
} else if (_fly_forward && _gps && _gps->status() == GPS::GPS_OK) { } else if (_fly_forward && have_gps()) {
if (_gps->last_fix_time != _gps_last_update && if (_gps->last_fix_time != _gps_last_update &&
_gps->ground_speed >= GPS_SPEED_MIN) { _gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3; yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3;
@ -398,7 +408,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// we have integrated over // we have integrated over
_ra_deltat += deltat; _ra_deltat += deltat;
if (_gps == NULL || _gps->status() != GPS::GPS_OK) { if (!have_gps()) {
// no GPS, or no lock. We assume zero velocity. This at // no GPS, or no lock. We assume zero velocity. This at
// least means we can cope with gyro drift while sitting // least means we can cope with gyro drift while sitting
// on a bench with no GPS lock // on a bench with no GPS lock
@ -406,24 +416,19 @@ AP_AHRS_DCM::drift_correction(float deltat)
// not enough time has accumulated // not enough time has accumulated
return; return;
} }
float speed_estimate; float airspeed;
if (_airspeed && _airspeed->use()) { if (_airspeed && _airspeed->use()) {
// use airspeed to estimate our ground velocity in airspeed = _airspeed->get_airspeed();
// earth frame. This isn't exactly right, but is
// better than nothing
speed_estimate = _airspeed->get_airspeed() * cos(pitch);
} else if (_gps != NULL) {
// use the last gps speed for our speed estimate. This
// will work for short GPS outages, or where we don't
// change speed a lot
speed_estimate = _gps->last_ground_speed();
} else { } else {
speed_estimate = 0; airspeed = _last_airspeed;
} }
// calculate our ground speed vector using our speed estimate // use airspeed to estimate our ground velocity in
velocity.x = speed_estimate * cos(yaw); // earth frame by subtracting the wind
velocity.y = speed_estimate * sin(yaw); velocity = _dcm_matrix.colx() * airspeed;
velocity.z = 0;
// add in wind estimate
velocity += _wind;
last_correction_time = millis(); last_correction_time = millis();
_have_gps_lock = false; _have_gps_lock = false;
@ -453,6 +458,11 @@ AP_AHRS_DCM::drift_correction(float deltat)
// once we have a single GPS lock, we update using // once we have a single GPS lock, we update using
// dead-reckoning from then on // dead-reckoning from then on
_have_position = true; _have_position = true;
// keep last airspeed estimate for dead-reckoning purposes
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
} }
#define USE_BAROMETER_FOR_VERTICAL_VELOCITY 1 #define USE_BAROMETER_FOR_VERTICAL_VELOCITY 1
@ -524,7 +534,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// flat, but still allow for yaw correction using the // flat, but still allow for yaw correction using the
// accelerometers at high roll angles as long as we have a GPS // accelerometers at high roll angles as long as we have a GPS
if (_compass && _compass->use_for_yaw()) { if (_compass && _compass->use_for_yaw()) {
if (_gps && _gps->status() == GPS::GPS_OK && gps_gain == 1.0) { if (have_gps() && gps_gain == 1.0) {
error.z *= sin(fabs(roll)); error.z *= sin(fabs(roll));
} else { } else {
error.z = 0; error.z = 0;
@ -572,9 +582,70 @@ AP_AHRS_DCM::drift_correction(float deltat)
// remember the velocity for next time // remember the velocity for next time
_last_velocity = velocity; _last_velocity = velocity;
if (_have_gps_lock && _fly_forward) {
// update wind estimate
estimate_wind(velocity);
}
} }
// update our wind speed estimate
// this is based on the wind speed estimation code from MatrixPilot by
// Bill Premerlani. Adaption for ArduPilot by Jon Challinger
// See http://gentlenav.googlecode.com/files/WindEstimation.pdf
void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
{
Vector3f fuselageDirection = Vector3f(_dcm_matrix.a.x,_dcm_matrix.b.x,_dcm_matrix.c.x);
Vector3f fuselageDirectionDiff = fuselageDirection - _last_fuse;
uint32_t now = millis();
// scrap our data and start over if we're taking too long to get a direction change
if (now - _last_wind_time > 10000) {
_last_wind_time = now;
_last_fuse = fuselageDirection;
_last_vel = velocity;
return;
}
float diff_length = fuselageDirectionDiff.length();
if (diff_length > 0.2) {
float V;
Vector3f velocityDiff = velocity - _last_vel;
// if we have an airspeed sensor, then use that,
// otherwise estimate it using equation 6
if (_airspeed && _airspeed->use()) {
float airspeed = _airspeed->get_airspeed();
V = (_dcm_matrix.colx() * airspeed).length();
} else {
V = velocityDiff.length() / diff_length;
}
_last_fuse = fuselageDirection;
_last_vel = velocity;
Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse;
Vector3f velocitySum = velocity + _last_vel;
float theta = atan2(velocityDiff.y, velocityDiff.x) - atan2(fuselageDirectionDiff.y, fuselageDirectionDiff.x);
float sintheta = sin(theta);
float costheta = cos(theta);
Vector3f wind = Vector3f();
wind.x = velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y);
wind.y = velocitySum.y - V * (sintheta * fuselageDirectionSum.x + costheta * fuselageDirectionSum.y);
wind.z = velocitySum.z - V * fuselageDirectionSum.z;
wind *= 0.5;
_wind = _wind * 0.92 + wind * 0.08;
_last_wind_time = now;
}
}
// calculate the euler angles which will be used for high level // calculate the euler angles which will be used for high level
// navigation control // navigation control
void void

View File

@ -68,6 +68,8 @@ private:
float yaw_error_compass(); float yaw_error_compass();
float yaw_error_gps(); float yaw_error_gps();
void euler_angles(void); void euler_angles(void);
void estimate_wind(Vector3f &velocity);
bool have_gps(void);
// primary representation of attitude // primary representation of attitude
Matrix3f _dcm_matrix; Matrix3f _dcm_matrix;
@ -120,6 +122,12 @@ private:
// whether we have a position estimate // whether we have a position estimate
bool _have_position; bool _have_position;
// support for wind estimation
Vector3f _last_fuse;
Vector3f _last_vel;
uint32_t _last_wind_time;
float _last_airspeed;
}; };
#endif // AP_AHRS_DCM_H #endif // AP_AHRS_DCM_H