From 2a29d7fedd9680c46b2c06d59227a35446149af1 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 27 Feb 2017 22:50:04 +0000 Subject: [PATCH] AC_PrecLand: rework estimation, add EST_TYPE param --- libraries/AC_PrecLand/AC_PrecLand.cpp | 287 +++++++++++++++++--------- libraries/AC_PrecLand/AC_PrecLand.h | 39 +++- 2 files changed, 228 insertions(+), 98 deletions(-) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index a811088478..88a31ecd95 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -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; +} diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index f78346b658..5385d03417 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -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 _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_history; // backend state struct precland_state {