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

AC_AttitudeControl: Add EKF optical flow noise gain scaler 

Allows gains to be adjusted to compensate for optical flow noise
This commit is contained in:
priseborough 2014-11-16 13:03:21 +11:00 committed by Andrew Tridgell
parent 11fb51ceba
commit e80b1c67cd
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -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 /// 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 // catch if we've just been started
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
@ -561,12 +561,12 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity)
break; break;
case 1: case 1:
// run position controller's position error to desired velocity step // 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++; _xy_step++;
break; break;
case 2: case 2:
// run position controller's velocity to acceleration step // run position controller's velocity to acceleration step
rate_to_accel_xy(_dt_xy); rate_to_accel_xy(_dt_xy, ekfNavVelGainScaler);
_xy_step++; _xy_step++;
break; break;
case 3: case 3:
@ -612,7 +612,7 @@ void AC_PosControl::init_vel_controller_xyz()
/// velocity targets should we set using set_desired_velocity_xyz() method /// 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 /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors /// 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 // capture time since last iteration
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
@ -636,10 +636,10 @@ void AC_PosControl::update_vel_controller_xyz()
desired_vel_to_pos(dt_xy); desired_vel_to_pos(dt_xy);
// run position controller's position error to desired velocity step // 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 // run velocity to acceleration step
rate_to_accel_xy(dt_xy); rate_to_accel_xy(dt_xy, ekfNavVelGainScaler);
// run acceleration to lean angle step // run acceleration to lean angle step
accel_to_lean_angles(); 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: /// when use_desired_rate is set to true:
/// desired velocity (_vel_desired) is combined into final target velocity and /// desired velocity (_vel_desired) is combined into final target velocity and
/// velocity due to position error is reduce to a maximum of 1m/s /// 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(); Vector3f curr_pos = _inav.get_position();
float linear_distance; // the distance we swap between linear and sqrt velocity response 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 // avoid divide by zero
if (kP <= 0.0f) { 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 /// rate_to_accel_xy - horizontal desired rate to desired acceleration
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame /// 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 const Vector3f &vel_curr = _inav.get_velocity(); // current velocity in cm/s
float accel_total; // total acceleration in cm/s/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); lon_i = _pid_rate_lon.get_i(_vel_error.y, dt);
} }
// combine feed forward accel with PID output from velocity error // 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); _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); _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 // scale desired acceleration if it's beyond acceptable limit
// To-Do: move this check down to the accel_to_lean_angle method? // To-Do: move this check down to the accel_to_lean_angle method?

8
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -203,7 +203,7 @@ public:
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher /// 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 /// 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 /// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home
void set_target_to_stopping_point_xy(); void set_target_to_stopping_point_xy();
@ -230,7 +230,7 @@ public:
/// velocity targets should we set using set_desired_velocity_xyz() method /// 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 /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors /// 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 /// get desired roll, pitch which should be fed into stabilize controllers
float get_roll() const { return _roll_target; } float get_roll() const { return _roll_target; }
@ -309,11 +309,11 @@ private:
/// when use_desired_rate is set to true: /// when use_desired_rate is set to true:
/// desired velocity (_vel_desired) is combined into final target velocity and /// desired velocity (_vel_desired) is combined into final target velocity and
/// velocity due to position error is reduce to a maximum of 1m/s /// 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 /// rate_to_accel_xy - horizontal desired rate to desired acceleration
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame /// 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 /// accel_to_lean_angles - horizontal desired acceleration to lean angles
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles /// converts desired accelerations provided in lat/lon frame to roll/pitch angles