From e80b1c67cd7a82d5b49bc175034074aa7a9248df Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 16 Nov 2014 13:03:21 +1100 Subject: [PATCH] AC_AttitudeControl: Add EKF optical flow noise gain scaler Allows gains to be adjusted to compensate for optical flow noise --- .../AC_AttitudeControl/AC_PosControl.cpp | 24 +++++++++---------- libraries/AC_AttitudeControl/AC_PosControl.h | 8 +++---- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 5c86e3af76..ab78fc0e71 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -530,7 +530,7 @@ void AC_PosControl::init_xy_controller() } /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher -void AC_PosControl::update_xy_controller(bool use_desired_velocity) +void AC_PosControl::update_xy_controller(bool use_desired_velocity, float ekfNavVelGainScaler) { // catch if we've just been started uint32_t now = hal.scheduler->millis(); @@ -561,12 +561,12 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity) break; case 1: // run position controller's position error to desired velocity step - pos_to_rate_xy(use_desired_velocity,_dt_xy); + pos_to_rate_xy(use_desired_velocity,_dt_xy, ekfNavVelGainScaler); _xy_step++; break; case 2: // run position controller's velocity to acceleration step - rate_to_accel_xy(_dt_xy); + rate_to_accel_xy(_dt_xy, ekfNavVelGainScaler); _xy_step++; break; case 3: @@ -612,7 +612,7 @@ void AC_PosControl::init_vel_controller_xyz() /// velocity targets should we set using set_desired_velocity_xyz() method /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller /// throttle targets will be sent directly to the motors -void AC_PosControl::update_vel_controller_xyz() +void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler) { // capture time since last iteration uint32_t now = hal.scheduler->millis(); @@ -636,10 +636,10 @@ void AC_PosControl::update_vel_controller_xyz() desired_vel_to_pos(dt_xy); // run position controller's position error to desired velocity step - pos_to_rate_xy(true, dt_xy); + pos_to_rate_xy(true, dt_xy, ekfNavVelGainScaler); // run velocity to acceleration step - rate_to_accel_xy(dt_xy); + rate_to_accel_xy(dt_xy, ekfNavVelGainScaler); // run acceleration to lean angle step accel_to_lean_angles(); @@ -688,11 +688,11 @@ void AC_PosControl::desired_vel_to_pos(float nav_dt) /// when use_desired_rate is set to true: /// desired velocity (_vel_desired) is combined into final target velocity and /// velocity due to position error is reduce to a maximum of 1m/s -void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt) +void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt, float ekfNavVelGainScaler) { Vector3f curr_pos = _inav.get_position(); float linear_distance; // the distance we swap between linear and sqrt velocity response - float kP = _p_pos_xy.kP(); + float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF // avoid divide by zero if (kP <= 0.0f) { @@ -755,7 +755,7 @@ void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt) /// rate_to_accel_xy - horizontal desired rate to desired acceleration /// converts desired velocities in lat/lon directions to accelerations in lat/lon frame -void AC_PosControl::rate_to_accel_xy(float dt) +void AC_PosControl::rate_to_accel_xy(float dt, float ekfNavVelGainScaler) { const Vector3f &vel_curr = _inav.get_velocity(); // current velocity in cm/s float accel_total; // total acceleration in cm/s/s @@ -802,9 +802,9 @@ void AC_PosControl::rate_to_accel_xy(float dt) lon_i = _pid_rate_lon.get_i(_vel_error.y, dt); } - // combine feed forward accel with PID output from velocity error - _accel_target.x = _accel_feedforward.x + _pid_rate_lat.get_p(_vel_error.x) + lat_i + _pid_rate_lat.get_d(_vel_error.x, dt); - _accel_target.y = _accel_feedforward.y + _pid_rate_lon.get_p(_vel_error.y) + lon_i + _pid_rate_lon.get_d(_vel_error.y, dt); + // combine feed forward accel with PID output from velocity error and scale PID output to compensate for optical flow measurement induced EKF noise + _accel_target.x = _accel_feedforward.x + (_pid_rate_lat.get_p(_vel_error.x) + lat_i + _pid_rate_lat.get_d(_vel_error.x, dt)) * ekfNavVelGainScaler; + _accel_target.y = _accel_feedforward.y + (_pid_rate_lon.get_p(_vel_error.y) + lon_i + _pid_rate_lon.get_d(_vel_error.y, dt)) * ekfNavVelGainScaler; // scale desired acceleration if it's beyond acceptable limit // To-Do: move this check down to the accel_to_lean_angle method? diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 0347c51511..3e8a7df495 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -203,7 +203,7 @@ public: /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher /// when use_desired_velocity is true the desired velocity (i.e. feed forward) is incorporated at the pos_to_rate step - void update_xy_controller(bool use_desired_velocity); + void update_xy_controller(bool use_desired_velocity, float ekfNavVelGainScaler); /// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home void set_target_to_stopping_point_xy(); @@ -230,7 +230,7 @@ public: /// velocity targets should we set using set_desired_velocity_xyz() method /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller /// throttle targets will be sent directly to the motors - void update_vel_controller_xyz(); + void update_vel_controller_xyz(float ekfNavVelGainScaler); /// get desired roll, pitch which should be fed into stabilize controllers float get_roll() const { return _roll_target; } @@ -309,11 +309,11 @@ private: /// when use_desired_rate is set to true: /// desired velocity (_vel_desired) is combined into final target velocity and /// velocity due to position error is reduce to a maximum of 1m/s - void pos_to_rate_xy(bool use_desired_rate, float dt); + void pos_to_rate_xy(bool use_desired_rate, float dt, float ekfNavVelGainScaler); /// rate_to_accel_xy - horizontal desired rate to desired acceleration /// converts desired velocities in lat/lon directions to accelerations in lat/lon frame - void rate_to_accel_xy(float dt); + void rate_to_accel_xy(float dt, float ekfNavVelGainScaler); /// accel_to_lean_angles - horizontal desired acceleration to lean angles /// converts desired accelerations provided in lat/lon frame to roll/pitch angles