From 6565d83e73edee46c3860c39f0111b9eba79b1a3 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 5 Jan 2013 18:59:09 -0800 Subject: [PATCH] InertialNav: Fixed signs, remove body-frame rotation, apply correction at 100hz. --- ArduCopter/Log.pde | 2 +- libraries/AP_InertialNav/AP_InertialNav.cpp | 112 +++++--------------- libraries/AP_InertialNav/AP_InertialNav.h | 2 +- 3 files changed, 29 insertions(+), 87 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 741fe25f0c..bdb7b0aa45 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -790,7 +790,7 @@ struct log_INAV { static void Log_Write_INAV() { #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 = { LOG_PACKET_HEADER_INIT(LOG_INAV_MSG), diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 779c186a9f..7b2e073841 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -6,25 +6,6 @@ extern const AP_HAL::HAL& hal; // table of user settable parameters 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 // @DisplayName: Horizontal Time Constant // @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 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; void AP_InertialNav::update(float dt) { - Vector3f acc_corr = accel_correction.get(); Vector3f accel_ef; Vector3f velocity_increase; @@ -74,8 +51,6 @@ void AP_InertialNav::update(float dt) // check gps check_gps(); - // convert accelerometer readings to earth frame - Matrix3f dcm = _ahrs->get_dcm_matrix(); accel_ef = _ahrs->get_accel_ef(); // remove influence of gravity @@ -87,12 +62,24 @@ void AP_InertialNav::update(float dt) accel_ef.x = 0; accel_ef.y = 0; } - - // get earth frame accelerometer correction - accel_correction_ef = dcm * acc_corr; - + + //Convert North-East-Down to North-East-Up + 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 - velocity_increase = (-accel_ef + accel_correction_ef) * dt; + velocity_increase = (accel_ef + accel_correction_ef) * dt; // calculate new estimate of position _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; 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 // 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 // To-Do: check why x and y are reversed - float err_x = -x - (hist_position_base_x + _position_correction.x); - float err_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; + _position_error.x = x - (hist_position_base_x + _position_correction.x); + _position_error.y = y - (hist_position_base_y + _position_correction.y); } // get accel based latitude @@ -217,7 +188,7 @@ int32_t AP_InertialNav::get_latitude() 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 @@ -228,7 +199,7 @@ int32_t AP_InertialNav::get_longitude() 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 @@ -264,7 +235,7 @@ float AP_InertialNav::get_latitude_diff() 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 @@ -275,7 +246,7 @@ float AP_InertialNav::get_longitude_diff() 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 @@ -286,7 +257,7 @@ float AP_InertialNav::get_latitude_velocity() return 0; } - return -_velocity.x; + return _velocity.x; // 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 -_velocity.y; + return _velocity.y; } // 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; float hist_position_base_z; - float accel_ef_z_correction; // discard samples where dt is too large if( dt > 0.5f ) { @@ -358,9 +328,6 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt) 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) // so we should calculate error using historical estimates 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 - float err = 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; + _position_error.z = baro_alt - (hist_position_base_z + _position_correction.z); } // set_altitude - set base altitude estimate in cm diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index bbfc520d8e..bd4fa7d160 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -112,7 +112,6 @@ public: static const struct AP_Param::GroupInfo var_info[]; // public variables - AP_Vector3f accel_correction; // acceleration corrections Vector3f accel_correction_ef; // earth frame accelerometer corrections. here for logging purposes only protected: @@ -151,6 +150,7 @@ protected: Vector3f _position_base; // position estimate Vector3f _position_correction; // sum of correction to _comp_h from delayed 1st order samples Vector3f _velocity; // latest velocity estimate (integrated from accelerometer values) + Vector3f _position_error; }; #endif // __AP_INERTIALNAV_H__