Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

InertialNav: Fixed signs, remove body-frame rotation, apply correction at 100hz.

Browse Source
This commit is contained in:
Jonathan Challinger 2013-01-05 18:59:09 -08:00 committed by rmackay9
parent 546ed19ffc
commit 6565d83e73
3 changed files with 29 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Log.pde
View File

@ -790,7 +790,7 @@ struct log_INAV {
static void Log_Write_INAV() static void Log_Write_INAV()
{ {
#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED #if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
Vector3f accel_corr = inertial_nav.accel_correction.get(); Vector3f accel_corr = inertial_nav.accel_correction_ef;
struct log_INAV pkt = { struct log_INAV pkt = {
LOG_PACKET_HEADER_INIT(LOG_INAV_MSG), LOG_PACKET_HEADER_INIT(LOG_INAV_MSG),

108
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -6,25 +6,6 @@ extern const AP_HAL::HAL& hal;
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo AP_InertialNav::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_InertialNav::var_info[] PROGMEM = {
// @Param: ACORR_X
// @DisplayName: Inertial Nav accelerometer offset correction on x-axis
// @Description: Accelerometer offset correction for the x-axis. Calculated automatically
// @Range: -100 100
// @Increment: 0.1
// @Param: ACORR_Y
// @DisplayName: Inertial Nav accelerometer correction on y-axis
// @Description: Accelerometer offset correction for the y-axis. Calculated automatically
// @Range: -100 100
// @Increment: 0.1
// @Param: ACORR_Z
// @DisplayName: Inertial Nav accelerometer correction on z-axis
// @Description: Accelerometer offset correction for the z-axis. Calculated automatically
// @Range: -100 100
// @Increment: 0.1
AP_GROUPINFO("ACORR", 0, AP_InertialNav, accel_correction, 0),
// @Param: TC_XY // @Param: TC_XY
// @DisplayName: Horizontal Time Constant // @DisplayName: Horizontal Time Constant
// @Description: Time constant for GPS and accel mixing. Higher TC decreases GPS impact on position estimate // @Description: Time constant for GPS and accel mixing. Higher TC decreases GPS impact on position estimate
@ -51,15 +32,11 @@ void AP_InertialNav::init()
// save_params - save all parameters to eeprom // save_params - save all parameters to eeprom
void AP_InertialNav::save_params() void AP_InertialNav::save_params()
{ {}
Vector3f accel_corr = accel_correction.get();
accel_correction.set_and_save(accel_corr);
}
// update - updates velocities and positions using latest info from ahrs, ins and barometer if new data is available; // update - updates velocities and positions using latest info from ahrs, ins and barometer if new data is available;
void AP_InertialNav::update(float dt) void AP_InertialNav::update(float dt)
{ {
Vector3f acc_corr = accel_correction.get();
Vector3f accel_ef; Vector3f accel_ef;
Vector3f velocity_increase; Vector3f velocity_increase;
@ -74,8 +51,6 @@ void AP_InertialNav::update(float dt)
// check gps // check gps
check_gps(); check_gps();
// convert accelerometer readings to earth frame
Matrix3f dcm = _ahrs->get_dcm_matrix();
accel_ef = _ahrs->get_accel_ef(); accel_ef = _ahrs->get_accel_ef();
// remove influence of gravity // remove influence of gravity
@ -88,11 +63,23 @@ void AP_InertialNav::update(float dt)
accel_ef.y = 0; accel_ef.y = 0;
} }
// get earth frame accelerometer correction //Convert North-East-Down to North-East-Up
accel_correction_ef = dcm * acc_corr; accel_ef.z = -accel_ef.z;
accel_correction_ef.x += _position_error.x * _k3_xy * dt;
accel_correction_ef.y += _position_error.y * _k3_xy * dt;
accel_correction_ef.z += _position_error.z * _k3_z * dt;
_velocity.x += _position_error.x * _k2_xy * dt;
_velocity.y += _position_error.y * _k2_xy * dt;
_velocity.z += _position_error.z * _k2_z * dt;
_position_correction.x += _position_error.x * _k1_xy * dt;
_position_correction.y += _position_error.y * _k1_xy * dt;
_position_correction.z += _position_error.z * _k1_z * dt;
// calculate velocity increase adding new acceleration from accelerometers // calculate velocity increase adding new acceleration from accelerometers
velocity_increase = (-accel_ef + accel_correction_ef) * dt; velocity_increase = (accel_ef + accel_correction_ef) * dt;
// calculate new estimate of position // calculate new estimate of position
_position_base += (_velocity + velocity_increase*0.5) * dt; _position_base += (_velocity + velocity_increase*0.5) * dt;
@ -175,9 +162,6 @@ void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt)
x = (float)(lat - _base_lat) * AP_INERTIALNAV_LATLON_TO_CM; x = (float)(lat - _base_lat) * AP_INERTIALNAV_LATLON_TO_CM;
y = (float)(lon - _base_lon) * _lon_to_m_scaling * AP_INERTIALNAV_LATLON_TO_CM; y = (float)(lon - _base_lon) * _lon_to_m_scaling * AP_INERTIALNAV_LATLON_TO_CM;
// convert accelerometer readings to earth frame
Matrix3f dcm = _ahrs->get_dcm_matrix();
// correct accelerometer offsets using gps // correct accelerometer offsets using gps
// ublox gps positions are delayed by 400ms // ublox gps positions are delayed by 400ms
@ -192,21 +176,8 @@ void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt)
// calculate error in position from gps with our historical estimate // calculate error in position from gps with our historical estimate
// To-Do: check why x and y are reversed // To-Do: check why x and y are reversed
float err_x = -x - (hist_position_base_x + _position_correction.x); _position_error.x = x - (hist_position_base_x + _position_correction.x);
float err_y = -y - (hist_position_base_y + _position_correction.y); _position_error.y = y - (hist_position_base_y + _position_correction.y);
// calculate correction to accelerometers and apply in the body frame
Vector3f accel_corr = accel_correction.get();
accel_corr += dcm.mul_transpose(Vector3f((err_x*_k3_xy)*dt,(err_y*_k3_xy)*dt,0));
accel_correction.set(accel_corr);
// correct velocity
_velocity.x += (err_x*_k2_xy) * dt;
_velocity.y += (err_y*_k2_xy) * dt;
// correct position
_position_correction.x += err_x*_k1_xy * dt;
_position_correction.y += err_y*_k1_xy * dt;
} }
// get accel based latitude // get accel based latitude
@ -217,7 +188,7 @@ int32_t AP_InertialNav::get_latitude()
return 0; return 0;
} }
return _base_lat - (int32_t)((_position_base.x + _position_correction.x)/AP_INERTIALNAV_LATLON_TO_CM); return _base_lat + (int32_t)((_position_base.x + _position_correction.x)/AP_INERTIALNAV_LATLON_TO_CM);
} }
// get accel based longitude // get accel based longitude
@ -228,7 +199,7 @@ int32_t AP_InertialNav::get_longitude()
return 0; return 0;
} }
return _base_lon - (int32_t)((_position_base.y+_position_correction.y) / (_lon_to_m_scaling*AP_INERTIALNAV_LATLON_TO_CM)); return _base_lon + (int32_t)((_position_base.y+_position_correction.y) / (_lon_to_m_scaling*AP_INERTIALNAV_LATLON_TO_CM));
} }
// set_current_position - all internal calculations are recorded as the distances from this point // set_current_position - all internal calculations are recorded as the distances from this point
@ -264,7 +235,7 @@ float AP_InertialNav::get_latitude_diff()
return 0; return 0;
} }
return -((_position_base.x+_position_correction.x)/AP_INERTIALNAV_LATLON_TO_CM); return ((_position_base.x+_position_correction.x)/AP_INERTIALNAV_LATLON_TO_CM);
} }
// get accel based longitude // get accel based longitude
@ -275,7 +246,7 @@ float AP_InertialNav::get_longitude_diff()
return 0; return 0;
} }
return -(_position_base.y+_position_correction.y) / (_lon_to_m_scaling*AP_INERTIALNAV_LATLON_TO_CM); return (_position_base.y+_position_correction.y) / (_lon_to_m_scaling*AP_INERTIALNAV_LATLON_TO_CM);
} }
// get velocity in latitude & longitude directions // get velocity in latitude & longitude directions
@ -286,7 +257,7 @@ float AP_InertialNav::get_latitude_velocity()
return 0; return 0;
} }
return -_velocity.x; return _velocity.x;
// Note: is +_velocity.x the output velocity in logs is in reverse direction from accel lat // Note: is +_velocity.x the output velocity in logs is in reverse direction from accel lat
} }
@ -297,7 +268,7 @@ float AP_InertialNav::get_longitude_velocity()
return 0; return 0;
} }
return -_velocity.y; return _velocity.y;
} }
// set_velocity_xy - set velocity in latitude & longitude directions (in cm/s) // set_velocity_xy - set velocity in latitude & longitude directions (in cm/s)
@ -345,7 +316,6 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
{ {
static uint8_t first_reads = 0; static uint8_t first_reads = 0;
float hist_position_base_z; float hist_position_base_z;
float accel_ef_z_correction;
// discard samples where dt is too large // discard samples where dt is too large
if( dt > 0.5f ) { if( dt > 0.5f ) {
@ -358,9 +328,6 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
first_reads++; first_reads++;
} }
// get dcm matrix
Matrix3f dcm = _ahrs->get_dcm_matrix();
// 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz) // 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz)
// so we should calculate error using historical estimates // so we should calculate error using historical estimates
if( _hist_position_estimate_z.num_items() >= 15 ) { if( _hist_position_estimate_z.num_items() >= 15 ) {
@ -370,32 +337,7 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
} }
// calculate error in position from baro with our estimate // calculate error in position from baro with our estimate
float err = baro_alt - (hist_position_base_z + _position_correction.z); _position_error.z = baro_alt - (hist_position_base_z + _position_correction.z);
// retrieve the existing accelerometer corrections
Vector3f accel_corr = accel_correction.get();
// calculate the accelerometer correction from this iteration in the earth frame
accel_ef_z_correction = err * _k3_z * dt;
// rotate the correction into the body frame (note: this is a shortened form of dcm.mul_transpose(..) because we have only one axis
accel_corr.x += accel_ef_z_correction * dcm.c.x;
accel_corr.y += accel_ef_z_correction * dcm.c.y;
accel_corr.z += accel_ef_z_correction * dcm.c.z;
// ensure corrections are reasonable
accel_corr.x = constrain(accel_corr.x,-AP_INTERTIALNAV_ACCEL_CORR_MAX,AP_INTERTIALNAV_ACCEL_CORR_MAX);
accel_corr.y = constrain(accel_corr.y,-AP_INTERTIALNAV_ACCEL_CORR_MAX,AP_INTERTIALNAV_ACCEL_CORR_MAX);
accel_corr.z = constrain(accel_corr.z,-AP_INTERTIALNAV_ACCEL_CORR_MAX,AP_INTERTIALNAV_ACCEL_CORR_MAX);
// set the parameter to include the corrections
accel_correction.set(accel_corr);
// correct velocity
_velocity.z += (err*_k2_z) * dt;
// correct position
_position_correction.z += err*_k1_z * dt;
} }
// set_altitude - set base altitude estimate in cm // set_altitude - set base altitude estimate in cm

2
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -112,7 +112,6 @@ public:
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
// public variables // public variables
AP_Vector3f accel_correction; // acceleration corrections
Vector3f accel_correction_ef; // earth frame accelerometer corrections. here for logging purposes only Vector3f accel_correction_ef; // earth frame accelerometer corrections. here for logging purposes only
protected: protected:
@ -151,6 +150,7 @@ protected:
Vector3f _position_base; // position estimate Vector3f _position_base; // position estimate
Vector3f _position_correction; // sum of correction to _comp_h from delayed 1st order samples Vector3f _position_correction; // sum of correction to _comp_h from delayed 1st order samples
Vector3f _velocity; // latest velocity estimate (integrated from accelerometer values) Vector3f _velocity; // latest velocity estimate (integrated from accelerometer values)
Vector3f _position_error;
}; };
#endif // __AP_INERTIALNAV_H__ #endif // __AP_INERTIALNAV_H__