Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: rework estimation, add EST_TYPE param

This commit is contained in:
Jonathan Challinger 2017-02-27 22:50:04 +00:00 committed by Randy Mackay
parent 054aab738b
commit 2a29d7fedd
2 changed files with 228 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

287
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -50,7 +50,13 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
// @Units: Centimeters
AP_GROUPINFO("LAND_OFS_Y", 4, AC_PrecLand, _land_ofs_cm_y, 0),
// 5 RESERVED for EKF_TYPE
// @Param: EST_TYPE
// @DisplayName: Precision Land Estimator Type
// @Description: Specifies the estimation method used.
// @Values: 0:RAW_MEASUREMENTS, 1:TWO_STATE_KF_PER_AXIS
// @User: Advanced
AP_GROUPINFO("EST_TYPE", 5, AC_PrecLand, _estimator_type, 1),
// 6 RESERVED for ACC_NSE
AP_GROUPEND
@ -64,6 +70,7 @@ AC_PrecLand::AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav) :
_ahrs(ahrs),
_inav(inav),
_last_update_ms(0),
_target_acquired(false),
_last_backend_los_meas_ms(0),
_backend(nullptr)
{
@ -122,124 +129,51 @@ void AC_PrecLand::init()
// update - give chance to driver to get updates from sensor
void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
{
_attitude_history.push_back(_ahrs.get_rotation_body_to_ned());
// Collect inertial data and append it to the buffer
struct inertial_data_frame_s inertial_data_newest;
_ahrs.getCorrectedDeltaVelocityNED(inertial_data_newest.correctedVehicleDeltaVelocityNED, inertial_data_newest.dt);
inertial_data_newest.Tbn = _ahrs.get_rotation_body_to_ned();
inertial_data_newest.inertialNavVelocity = _inav.get_velocity()*0.01f;
inertial_data_newest.inertialNavVelocityValid = _inav.get_filter_status().flags.horiz_vel;
_inertial_history.push_back(inertial_data_newest);
// run backend update
if (_backend != nullptr && _enabled) {
// read from sensor
_backend->update();
Vector3f vehicleVelocityNED = _inav.get_velocity()*0.01f;
vehicleVelocityNED.z = -vehicleVelocityNED.z;
if (target_acquired()) {
// EKF prediction step
float dt;
Vector3f targetDelVel;
_ahrs.getCorrectedDeltaVelocityNED(targetDelVel, dt);
targetDelVel = -targetDelVel;
_ekf_x.predict(dt, targetDelVel.x, 0.5f*dt);
_ekf_y.predict(dt, targetDelVel.y, 0.5f*dt);
}
if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) {
// we have a new, unique los measurement
_last_backend_los_meas_ms = _backend->los_meas_time_ms();
Vector3f target_vec_unit_body;
_backend->get_los_body(target_vec_unit_body);
// Apply sensor yaw alignment rotation
float sin_yaw_align = sinf(radians(_yaw_align*0.01f));
float cos_yaw_align = cosf(radians(_yaw_align*0.01f));
Matrix3f Rz = Matrix3f(
cos_yaw_align, -sin_yaw_align, 0,
sin_yaw_align, cos_yaw_align, 0,
0, 0, 1
);
Vector3f target_vec_unit_ned = _attitude_history.front() * Rz * target_vec_unit_body;
bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_cm > 0.0f) || (_backend->distance_to_target() > 0.0f);
if (target_vec_valid && alt_valid) {
float alt;
if (_backend->distance_to_target() > 0.0f) {
alt = _backend->distance_to_target();
} else {
alt = MAX(rangefinder_alt_cm*0.01f, 0.0f);
}
float dist = alt/target_vec_unit_ned.z;
Vector3f targetPosRelMeasNED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt);
float xy_pos_var = sq(targetPosRelMeasNED.z*(0.01f + 0.01f*_ahrs.get_gyro().length()) + 0.02f);
if (!target_acquired()) {
// reset filter state
if (_inav.get_filter_status().flags.horiz_pos_rel) {
_ekf_x.init(targetPosRelMeasNED.x, xy_pos_var, -vehicleVelocityNED.x, sq(2.0f));
_ekf_y.init(targetPosRelMeasNED.y, xy_pos_var, -vehicleVelocityNED.y, sq(2.0f));
} else {
_ekf_x.init(targetPosRelMeasNED.x, xy_pos_var, 0.0f, sq(10.0f));
_ekf_y.init(targetPosRelMeasNED.y, xy_pos_var, 0.0f, sq(10.0f));
}
_last_update_ms = AP_HAL::millis();
} else {
float NIS_x = _ekf_x.getPosNIS(targetPosRelMeasNED.x, xy_pos_var);
float NIS_y = _ekf_y.getPosNIS(targetPosRelMeasNED.y, xy_pos_var);
if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) {
_outlier_reject_count = 0;
_ekf_x.fusePos(targetPosRelMeasNED.x, xy_pos_var);
_ekf_y.fusePos(targetPosRelMeasNED.y, xy_pos_var);
_last_update_ms = AP_HAL::millis();
} else {
_outlier_reject_count++;
}
}
}
}
run_estimator(rangefinder_alt_cm*0.01f, rangefinder_alt_valid);
}
}
bool AC_PrecLand::target_acquired() const
bool AC_PrecLand::target_acquired()
{
return (AP_HAL::millis()-_last_update_ms) < 2000;
_target_acquired = _target_acquired && (AP_HAL::millis()-_last_update_ms) < 2000;
return _target_acquired;
}
bool AC_PrecLand::get_target_position_cm(Vector2f& ret) const
bool AC_PrecLand::get_target_position_cm(Vector2f& ret)
{
if (!target_acquired()) {
return false;
}
Vector3f land_ofs_ned_cm = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0);
ret.x = _ekf_x.getPos()*100.0f + _inav.get_position().x + land_ofs_ned_cm.x;
ret.y = _ekf_y.getPos()*100.0f + _inav.get_position().y + land_ofs_ned_cm.y;
ret.x = _target_pos_rel_out_NE.x*100.0f + _inav.get_position().x;
ret.y = _target_pos_rel_out_NE.y*100.0f + _inav.get_position().y;
return true;
}
bool AC_PrecLand::get_target_position_relative_cm(Vector2f& ret) const
bool AC_PrecLand::get_target_position_relative_cm(Vector2f& ret)
{
if (!target_acquired()) {
return false;
}
Vector3f land_ofs_ned_cm = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0);
ret.x = _ekf_x.getPos()*100.0f + land_ofs_ned_cm.x;
ret.y = _ekf_y.getPos()*100.0f + land_ofs_ned_cm.y;
ret = _target_pos_rel_out_NE*100.0f;
return true;
}
bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret) const
bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret)
{
if (!target_acquired()) {
return false;
}
ret.x = _ekf_x.getVel()*100.0f;
ret.y = _ekf_y.getVel()*100.0f;
ret = _target_vel_rel_out_NE*100.0f;
return true;
}
@ -251,3 +185,172 @@ void AC_PrecLand::handle_msg(mavlink_message_t* msg)
_backend->handle_msg(msg);
}
}
//
// Private methods
//
void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid)
{
const struct inertial_data_frame_s& inertial_data_delayed = _inertial_history.front();
switch (_estimator_type) {
case ESTIMATOR_TYPE_RAW_MEASUREMENTS: {
// Return if there's any invalid velocity data
for (uint8_t i=0; i<_inertial_history.size(); i++) {
const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(i);
if (!inertial_data.inertialNavVelocityValid) {
_target_acquired = false;
return;
}
}
// Predict
if (target_acquired()) {
_target_pos_rel_est_NE.x -= inertial_data_delayed.inertialNavVelocity.x * inertial_data_delayed.dt;
_target_pos_rel_est_NE.y -= inertial_data_delayed.inertialNavVelocity.y * inertial_data_delayed.dt;
_target_vel_rel_est_NE.x = -inertial_data_delayed.inertialNavVelocity.x;
_target_vel_rel_est_NE.y = -inertial_data_delayed.inertialNavVelocity.y;
}
// Update if a new LOS measurement is available
Vector3f target_vec_unit_body;
if (retrieve_los_meas(target_vec_unit_body)) {
Vector3f target_vec_unit_ned = inertial_data_delayed.Tbn * target_vec_unit_body;
bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
if (target_vec_valid && alt_valid) {
float alt;
if (_backend->distance_to_target() > 0.0f) {
alt = _backend->distance_to_target();
} else {
alt = MAX(rangefinder_alt_m, 0.0f);
}
float dist = alt/target_vec_unit_ned.z;
_target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt);
_target_pos_rel_est_NE.x = _target_pos_rel_meas_NED.x;
_target_pos_rel_est_NE.y = _target_pos_rel_meas_NED.y;
_target_vel_rel_est_NE.x = -inertial_data_delayed.inertialNavVelocity.x;
_target_vel_rel_est_NE.y = -inertial_data_delayed.inertialNavVelocity.y;
_last_update_ms = AP_HAL::millis();
_target_acquired = true;
}
}
// Output prediction
if (target_acquired()) {
run_output_prediction();
}
break;
}
case ESTIMATOR_TYPE_TWO_STATE_KF_PER_AXIS: {
// Predict
if (target_acquired()) {
const float& dt = inertial_data_delayed.dt;
const Vector3f& vehicleDelVel = inertial_data_delayed.correctedVehicleDeltaVelocityNED;
_ekf_x.predict(dt, -vehicleDelVel.x, 0.5f*dt);
_ekf_y.predict(dt, -vehicleDelVel.y, 0.5f*dt);
}
// Update if a new LOS measurement is available
Vector3f target_vec_unit_body;
if (retrieve_los_meas(target_vec_unit_body)) {
Vector3f target_vec_unit_ned = inertial_data_delayed.Tbn * target_vec_unit_body;
bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
if (target_vec_valid && alt_valid) {
float alt;
if (_backend->distance_to_target() > 0.0f) {
alt = _backend->distance_to_target();
} else {
alt = MAX(rangefinder_alt_m, 0.0f);
}
float dist = alt/target_vec_unit_ned.z;
_target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt);
float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*_ahrs.get_gyro().length()) + 0.02f);
if (!target_acquired()) {
// reset filter state
if (inertial_data_delayed.inertialNavVelocityValid) {
_ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -inertial_data_delayed.inertialNavVelocity.x, sq(2.0f));
_ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, -inertial_data_delayed.inertialNavVelocity.y, sq(2.0f));
} else {
_ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, 0.0f, sq(10.0f));
_ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, 0.0f, sq(10.0f));
}
_last_update_ms = AP_HAL::millis();
_target_acquired = true;
} else {
float NIS_x = _ekf_x.getPosNIS(_target_pos_rel_meas_NED.x, xy_pos_var);
float NIS_y = _ekf_y.getPosNIS(_target_pos_rel_meas_NED.y, xy_pos_var);
if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) {
_outlier_reject_count = 0;
_ekf_x.fusePos(_target_pos_rel_meas_NED.x, xy_pos_var);
_ekf_y.fusePos(_target_pos_rel_meas_NED.y, xy_pos_var);
_last_update_ms = AP_HAL::millis();
_target_acquired = true;
} else {
_outlier_reject_count++;
}
}
}
}
// Output prediction
if (target_acquired()) {
_target_pos_rel_est_NE.x = _ekf_x.getPos();
_target_pos_rel_est_NE.y = _ekf_y.getPos();
_target_vel_rel_est_NE.x = _ekf_x.getVel();
_target_vel_rel_est_NE.y = _ekf_y.getVel();
run_output_prediction();
}
break;
}
}
}
bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body)
{
if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) {
_last_backend_los_meas_ms = _backend->los_meas_time_ms();
_backend->get_los_body(target_vec_unit_body);
// Apply sensor yaw alignment rotation
float sin_yaw_align = sinf(radians(_yaw_align*0.01f));
float cos_yaw_align = cosf(radians(_yaw_align*0.01f));
Matrix3f Rz = Matrix3f(
cos_yaw_align, -sin_yaw_align, 0,
sin_yaw_align, cos_yaw_align, 0,
0, 0, 1
);
target_vec_unit_body = Rz*target_vec_unit_body;
return true;
} else {
return false;
}
}
void AC_PrecLand::run_output_prediction()
{
_target_pos_rel_out_NE = _target_pos_rel_est_NE;
_target_vel_rel_out_NE = _target_vel_rel_est_NE;
// Predict forward from delayed time horizon
for (uint8_t i=1; i<_inertial_history.size(); i++) {
const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(i);
_target_vel_rel_out_NE.x -= inertial_data.correctedVehicleDeltaVelocityNED.x;
_target_vel_rel_out_NE.y -= inertial_data.correctedVehicleDeltaVelocityNED.y;
_target_pos_rel_out_NE.x += _target_vel_rel_out_NE.x * inertial_data.dt;
_target_pos_rel_out_NE.y += _target_vel_rel_out_NE.y * inertial_data.dt;
}
// Apply offset
Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0) * 0.01f;
_target_pos_rel_out_NE.x += land_ofs_ned_m.x;
_target_pos_rel_out_NE.y += land_ofs_ned_m.y;
}

39
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -58,16 +58,16 @@ public:
void update(float rangefinder_alt_cm, bool rangefinder_alt_valid);
// returns target position relative to origin
bool get_target_position_cm(Vector2f& ret) const;
bool get_target_position_cm(Vector2f& ret);
// returns target position relative to vehicle
bool get_target_position_relative_cm(Vector2f& ret) const;
bool get_target_position_relative_cm(Vector2f& ret);
// returns target velocity relative to vehicle
bool get_target_velocity_relative_cms(Vector2f& ret) const;
bool get_target_velocity_relative_cms(Vector2f& ret);
// returns true when the landing target has been detected
bool target_acquired() const;
bool target_acquired();
// process a LANDING_TARGET mavlink message
void handle_msg(mavlink_message_t* msg);
@ -79,27 +79,54 @@ public:
static const struct AP_Param::GroupInfo var_info[];
private:
enum estimator_type_t {
ESTIMATOR_TYPE_RAW_MEASUREMENTS=0,
ESTIMATOR_TYPE_TWO_STATE_KF_PER_AXIS=1
};
// returns enabled parameter as an behaviour
enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); }
void run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid);
bool retrieve_los_meas(Vector3f& target_vec_unit_body);
void run_output_prediction();
// references to inertial nav and ahrs libraries
const AP_AHRS& _ahrs;
const AP_InertialNav& _inav;
// parameters
AP_Int8 _enabled; // enabled/disabled and behaviour
AP_Int8 _type; // precision landing controller type
AP_Int8 _type; // precision landing sensor type
AP_Int8 _estimator_type; // precision landing estimator type
AP_Float _yaw_align; // Yaw angle from body x-axis to sensor x-axis.
AP_Float _land_ofs_cm_x; // Desired landing position of the camera forward of the target in vehicle body frame
AP_Float _land_ofs_cm_y; // Desired landing position of the camera right of the target in vehicle body frame
uint32_t _last_update_ms; // epoch time in millisecond when update is called
bool _target_acquired;
uint32_t _last_backend_los_meas_ms;
PosVelEKF _ekf_x, _ekf_y;
uint32_t _outlier_reject_count;
AP_Buffer<Matrix3f,8> _attitude_history;
Vector3f _target_pos_rel_meas_NED;
Vector2f _target_pos_rel_est_NE;
Vector2f _target_vel_rel_est_NE;
Vector2f _target_pos_rel_out_NE;
Vector2f _target_vel_rel_out_NE;
struct inertial_data_frame_s {
Matrix3f Tbn;
Vector3f correctedVehicleDeltaVelocityNED;
Vector3f inertialNavVelocity;
bool inertialNavVelocityValid;
float dt;
};
AP_Buffer<inertial_data_frame_s,8> _inertial_history;
// backend state
struct precland_state {