From 7405f82d619b394211a1ec33a900f4ba62ab2ec1 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 15 Jul 2016 10:30:21 -0700 Subject: [PATCH] AC_PrecLand: add ekf to estimate vehicle-relative target velocity --- libraries/AC_PrecLand/AC_PrecLand.cpp | 150 +++++++++++++++++++------- libraries/AC_PrecLand/AC_PrecLand.h | 43 +++----- libraries/AC_PrecLand/PosVelEKF.cpp | 93 ++++++++++++++++ libraries/AC_PrecLand/PosVelEKF.h | 19 ++++ 4 files changed, 239 insertions(+), 66 deletions(-) create mode 100644 libraries/AC_PrecLand/PosVelEKF.cpp create mode 100644 libraries/AC_PrecLand/PosVelEKF.h diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 59514bf9a8..4c50e39477 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -22,6 +22,33 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = { // @User: Advanced AP_GROUPINFO("TYPE", 1, AC_PrecLand, _type, 0), + // @Param: YAW_ALIGN + // @DisplayName: Sensor yaw alignment + // @Description: Yaw angle from body x-axis to sensor x-axis. + // @Range: 0 360 + // @Increment: 1 + // @User: Advanced + // @Units: Centi-degrees + AP_GROUPINFO("YAW_ALIGN", 2, AC_PrecLand, _yaw_align, 0), + + // @Param: LAND_OFS_X + // @DisplayName: Land offset forward + // @Description: Desired landing position of the camera forward of the target in vehicle body frame + // @Range: -20 20 + // @Increment: 1 + // @User: Advanced + // @Units: Centimeters + AP_GROUPINFO("LAND_OFS_X", 3, AC_PrecLand, _land_ofs_cm_x, 0), + + // @Param: LAND_OFS_Y + // @DisplayName: Land offset right + // @Description: desired landing position of the camera right of the target in vehicle body frame + // @Range: -20 20 + // @Increment: 1 + // @User: Advanced + // @Units: Centimeters + AP_GROUPINFO("LAND_OFS_Y", 4, AC_PrecLand, _land_ofs_cm_y, 0), + AP_GROUPEND }; @@ -81,81 +108,126 @@ void AC_PrecLand::init() } // update - give chance to driver to get updates from sensor -void AC_PrecLand::update(float alt_above_terrain_cm) +void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) { + _attitude_history.push_back(_ahrs.get_rotation_body_to_ned()); + // run backend update if (_backend != NULL && _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 (_inav.get_filter_status().flags.horiz_pos_rel) { + _ekf_x.fuseVel(-vehicleVelocityNED.x, sq(1.0f)); + _ekf_y.fuseVel(-vehicleVelocityNED.y, sq(1.0f)); + } + } + 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); - - calc_angles_and_pos(target_vec_unit_body, alt_above_terrain_cm); + + // 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; + + if (target_vec_valid && rangefinder_alt_valid && rangefinder_alt_cm > 0.0f) { + float 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(1.0f)); + _ekf_y.init(targetPosRelMeasNED.y, xy_pos_var, -vehicleVelocityNED.y, sq(1.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++; + } + } + } } } } -bool AC_PrecLand::target_acquired() +bool AC_PrecLand::target_acquired() const { - return (AP_HAL::millis()-_last_update_ms) < 1000; + return (AP_HAL::millis()-_last_update_ms) < 2000; } -bool AC_PrecLand::get_target_position(Vector3f& ret) +bool AC_PrecLand::get_target_position_cm(Vector2f& ret) const { if (!target_acquired()) { return false; } - ret = _target_pos; + 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; return true; } -bool AC_PrecLand::get_target_position_relative(Vector3f& ret) +bool AC_PrecLand::get_target_position_relative_cm(Vector2f& ret) const { if (!target_acquired()) { return false; } - ret = _target_pos_rel; - return true; -} + Vector3f land_ofs_ned_cm = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0); -bool AC_PrecLand::get_target_velocity_relative(Vector3f& ret) -{ - return false; + ret.x = _ekf_x.getPos()*100.0f + land_ofs_ned_cm.x; + ret.y = _ekf_y.getPos()*100.0f + land_ofs_ned_cm.y; + return true; } -// converts sensor's body-frame angles to earth-frame angles and position estimate -// raw sensor angles stored in _angle_to_target (might be in earth frame, or maybe body frame) -// earth-frame angles stored in _ef_angle_to_target -// position estimate is stored in _target_pos -void AC_PrecLand::calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm) +bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret) const { - // rotate into NED frame - Vector3f target_vec_unit_ned = _ahrs.get_rotation_body_to_ned()*target_vec_unit_body; - - // extract the angles to target (logging only) - _angle_to_target.x = atan2f(-target_vec_unit_body.y, target_vec_unit_body.z); - _angle_to_target.y = atan2f( target_vec_unit_body.x, target_vec_unit_body.z); - _ef_angle_to_target.x = atan2f(-target_vec_unit_ned.y, target_vec_unit_ned.z); - _ef_angle_to_target.y = atan2f( target_vec_unit_ned.x, target_vec_unit_ned.z); - - if (target_vec_unit_ned.z > 0.0f) { - // get current altitude (constrained to be positive) - float alt = MAX(alt_above_terrain_cm, 0.0f); - float dist = alt/target_vec_unit_ned.z; - _target_pos_rel.x = target_vec_unit_ned.x*dist; - _target_pos_rel.y = target_vec_unit_ned.y*dist; - _target_pos_rel.z = alt; // not used - _target_pos = _inav.get_position()+_target_pos_rel; - - _last_update_ms = AP_HAL::millis(); + if (!target_acquired()) { + return false; } + ret.x = _ekf_x.getVel()*100.0f; + ret.y = _ekf_y.getVel()*100.0f; + return true; } // handle_msg - Process a LANDING_TARGET mavlink message diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 5be6d4390c..cc1671e420 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -6,6 +6,8 @@ #include #include #include +#include "PosVelEKF.h" +#include // declare backend classes class AC_PrecLand_Backend; @@ -42,49 +44,36 @@ public: void init(); // returns true if precision landing is healthy - bool healthy() { return _backend_state.healthy; } + bool healthy() const { return _backend_state.healthy; } // returns time of last update - uint32_t last_update_ms() { return _last_update_ms; } + uint32_t last_update_ms() const { return _last_update_ms; } // give chance to driver to get updates from sensor - void update(float alt_above_terrain_cm); - - // returns 3D vector of earth-frame position adjustments to target - Vector3f get_target_shift(const Vector3f& orig_target); + void update(float rangefinder_alt_cm, bool rangefinder_alt_valid); // returns target position relative to origin - bool get_target_position(Vector3f& ret); + bool get_target_position_cm(Vector2f& ret) const; // returns target position relative to vehicle - bool get_target_position_relative(Vector3f& ret); + bool get_target_position_relative_cm(Vector2f& ret) const; // returns target velocity relative to vehicle - bool get_target_velocity_relative(Vector3f& ret); + bool get_target_velocity_relative_cms(Vector2f& ret) const; // returns true when the landing target has been detected - bool target_acquired(); + bool target_acquired() const; // process a LANDING_TARGET mavlink message void handle_msg(mavlink_message_t* msg); // accessors for logging bool enabled() const { return _enabled; } - const Vector2f& last_bf_angle_to_target() const { return _angle_to_target; } - const Vector2f& last_ef_angle_to_target() const { return _ef_angle_to_target; } - const Vector3f& last_target_pos_offset() const { return _target_pos_rel; } // parameter var table static const struct AP_Param::GroupInfo var_info[]; private: - - // converts sensor's body-frame angles to earth-frame angles and position estimate - // angles stored in _angle_to_target - // earth-frame angles stored in _ef_angle_to_target - // position estimate is stored in _target_pos - void calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm); - // returns enabled parameter as an behaviour enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); } @@ -95,17 +84,17 @@ private: // parameters AP_Int8 _enabled; // enabled/disabled and behaviour AP_Int8 _type; // precision landing controller 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 uint32_t _last_backend_los_meas_ms; - // output from sensor (stored for logging) - Vector2f _angle_to_target; // last raw sensor angle to target - Vector2f _ef_angle_to_target;// last earth-frame angle to target - - // estimator output - Vector3f _target_pos_rel; // estimate target position relative to vehicle in NEU cm - Vector3f _target_pos; // estimate target position in NEU cm + PosVelEKF _ekf_x, _ekf_y; + uint32_t _outlier_reject_count; + + AP_Buffer _attitude_history; // backend state struct precland_state { diff --git a/libraries/AC_PrecLand/PosVelEKF.cpp b/libraries/AC_PrecLand/PosVelEKF.cpp new file mode 100644 index 0000000000..935a3d4ba1 --- /dev/null +++ b/libraries/AC_PrecLand/PosVelEKF.cpp @@ -0,0 +1,93 @@ +#include "PosVelEKF.h" +#include +#include + +#define POSVELEKF_POS_CALC_NIS(__P, __R, __X, __Z, __RET_NIS) \ +__RET_NIS = ((-__X[0] + __Z)*(-__X[0] + __Z))/(__P[0] + __R); + +#define POSVELEKF_POS_CALC_STATE(__P, __R, __X, __Z, __RET_STATE) \ +__RET_STATE[0] = __P[0]*(-__X[0] + __Z)/(__P[0] + __R) + __X[0]; __RET_STATE[1] = __P[1]*(-__X[0] + \ +__Z)/(__P[0] + __R) + __X[1]; + +#define POSVELEKF_POS_CALC_COV(__P, __R, __X, __Z, __RET_COV) \ +__RET_COV[0] = ((__P[0])*(__P[0]))*__R/((__P[0] + __R)*(__P[0] + __R)) + __P[0]*((-__P[0]/(__P[0] + \ +__R) + 1)*(-__P[0]/(__P[0] + __R) + 1)); __RET_COV[1] = __P[0]*__P[1]*__R/((__P[0] + __R)*(__P[0] + \ +__R)) - __P[0]*__P[1]*(-__P[0]/(__P[0] + __R) + 1)/(__P[0] + __R) + __P[1]*(-__P[0]/(__P[0] + __R) + \ +1); __RET_COV[2] = ((__P[1])*(__P[1]))*__R/((__P[0] + __R)*(__P[0] + __R)) - \ +((__P[1])*(__P[1]))/(__P[0] + __R) - __P[1]*(-__P[0]*__P[1]/(__P[0] + __R) + __P[1])/(__P[0] + __R) + \ +__P[2]; + +#define POSVELEKF_PREDICTION_CALC_STATE(__P, __DT, __DV, __DV_NOISE, __X, __RET_STATE) \ +__RET_STATE[0] = __DT*__X[1] + __X[0]; __RET_STATE[1] = __DV + __X[1]; + +#define POSVELEKF_PREDICTION_CALC_COV(__P, __DT, __DV, __DV_NOISE, __X, __RET_COV) \ +__RET_COV[0] = __DT*__P[1] + __DT*(__DT*__P[2] + __P[1]) + __P[0]; __RET_COV[1] = __DT*__P[2] + \ +__P[1]; __RET_COV[2] = ((__DV_NOISE)*(__DV_NOISE)) + __P[2]; + +#define POSVELEKF_VEL_CALC_NIS(__P, __R, __X, __Z, __RET_NIS) \ +__RET_NIS = ((-__X[1] + __Z)*(-__X[1] + __Z))/(__P[2] + __R); + +#define POSVELEKF_VEL_CALC_STATE(__P, __R, __X, __Z, __RET_STATE) \ +__RET_STATE[0] = __P[1]*(-__X[1] + __Z)/(__P[2] + __R) + __X[0]; __RET_STATE[1] = __P[2]*(-__X[1] + \ +__Z)/(__P[2] + __R) + __X[1]; + +#define POSVELEKF_VEL_CALC_COV(__P, __R, __X, __Z, __RET_COV) \ +__RET_COV[0] = __P[0] + ((__P[1])*(__P[1]))*__R/((__P[2] + __R)*(__P[2] + __R)) - \ +((__P[1])*(__P[1]))/(__P[2] + __R) - __P[1]*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1])/(__P[2] + __R); \ +__RET_COV[1] = __P[1]*__P[2]*__R/((__P[2] + __R)*(__P[2] + __R)) + (-__P[2]/(__P[2] + __R) + \ +1)*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1]); __RET_COV[2] = ((__P[2])*(__P[2]))*__R/((__P[2] + \ +__R)*(__P[2] + __R)) + __P[2]*((-__P[2]/(__P[2] + __R) + 1)*(-__P[2]/(__P[2] + __R) + 1)); + +void PosVelEKF::init(float pos, float posVar, float vel, float velVar) +{ + _state[0] = pos; + _state[1] = vel; + _cov[0] = posVar; + _cov[1] = 0.0f; + _cov[2] = velVar; +} + +void PosVelEKF::predict(float dt, float dVel, float dVelNoise) +{ + float newState[2]; + float newCov[3]; + + POSVELEKF_PREDICTION_CALC_STATE(_cov, dt, dVel, dVelNoise, _state, newState) + POSVELEKF_PREDICTION_CALC_COV(_cov, dt, dVel, dVelNoise, _state, newCov) + + memcpy(_state,newState,sizeof(_state)); + memcpy(_cov,newCov,sizeof(_cov)); +} + +void PosVelEKF::fusePos(float pos, float posVar) +{ + float newState[2]; + float newCov[3]; + + POSVELEKF_POS_CALC_STATE(_cov, posVar, _state, pos, newState) + POSVELEKF_POS_CALC_COV(_cov, posVar, _state, pos, newCov) + + memcpy(_state,newState,sizeof(_state)); + memcpy(_cov,newCov,sizeof(_cov)); +} + +void PosVelEKF::fuseVel(float vel, float velVar) +{ + float newState[2]; + float newCov[3]; + + POSVELEKF_VEL_CALC_STATE(_cov, velVar, _state, vel, newState) + POSVELEKF_VEL_CALC_COV(_cov, velVar, _state, vel, newCov) + + memcpy(_state,newState,sizeof(_state)); + memcpy(_cov,newCov,sizeof(_cov)); +} + +float PosVelEKF::getPosNIS(float pos, float posVar) +{ + float ret; + + POSVELEKF_POS_CALC_NIS(_cov, posVar, _state, pos, ret) + + return ret; +} diff --git a/libraries/AC_PrecLand/PosVelEKF.h b/libraries/AC_PrecLand/PosVelEKF.h new file mode 100644 index 0000000000..5ef2fa3a73 --- /dev/null +++ b/libraries/AC_PrecLand/PosVelEKF.h @@ -0,0 +1,19 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#pragma once + +class PosVelEKF { +public: + void init(float pos, float posVar, float vel, float velVar); + void predict(float dt, float dVel, float dVelNoise); + void fusePos(float pos, float posVar); + void fuseVel(float vel, float velVar); + + float getPos() const { return _state[0]; } + float getVel() const { return _state[1]; } + + float getPosNIS(float pos, float posVar); + +private: + float _state[2]; + float _cov[3]; +};