zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_PrecLand: rework estimation, add EST_TYPE param

master
Jonathan Challinger 8 years ago committed by Randy Mackay
parent
054aab738b
commit
2a29d7fedd
2 changed files with 228 additions and 98 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 287
      libraries/AC_PrecLand/AC_PrecLand.cpp
  2. 39
      libraries/AC_PrecLand/AC_PrecLand.h

287
libraries/AC_PrecLand/AC_PrecLand.cpp
Unescape View File

@ -50,7 +50,13 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = { @@ -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) : @@ -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() @@ -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) @@ -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
Unescape View File

@ -58,16 +58,16 @@ public: @@ -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: @@ -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 {

Write Preview
Loading…
Cancel
Save