fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers: Correct IMU coning correction implementation

Browse Source 
Previous did not match the matlab simulation in: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m
This commit is contained in:
Paul Riseborough 2016-05-26 20:28:01 +10:00 committed by Lorenz Meier
parent 9d489b9b0f
commit 2b370417e8
2 changed files with 40 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/drivers/device/integrator.cpp
View File

@ -46,9 +46,11 @@ Integrator::Integrator(uint64_t auto_reset_interval, bool coning_compensation) :
_auto_reset_interval(auto_reset_interval),
_last_integration_time(0),
_last_reset_time(0),
_integral(0.0f, 0.0f, 0.0f),
_alpha(0.0f, 0.0f, 0.0f),
_last_alpha(0.0f, 0.0f, 0.0f),
_beta(0.0f, 0.0f, 0.0f),
_last_val(0.0f, 0.0f, 0.0f),
_last_delta(0.0f, 0.0f, 0.0f),
_last_delta_alpha(0.0f, 0.0f, 0.0f),
_coning_comp_on(coning_compensation)
{
@ -80,28 +82,39 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ
dt = (double)(timestamp - _last_integration_time) / 1000000.0;
}
math::Vector<3> delta = (val + _last_val) * dt * 0.5f;
// Use trapezoidal integration to calculate the delta integral
math::Vector<3> delta_alpha = (val + _last_val) * dt * 0.5f;
_last_val = val;
// Apply coning compensation if required
// Calculate coning corrections if required
if (_coning_comp_on) {
// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
// following:
// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
delta += ((_integral + _last_delta * (1.0f / 6.0f)) % delta) * 0.5f;
// Sourced: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
// Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m
_beta += ((_last_alpha + _last_delta_alpha * (1.0f / 6.0f)) % delta_alpha) * 0.5f;
_last_delta_alpha = delta_alpha;
_last_alpha = _alpha;
}
_integral += delta;
// accumulate delta integrals
_alpha += delta_alpha;
_last_integration_time = timestamp;
_last_val = val;
_last_delta = delta;
// Only do auto reset if auto reset interval is not 0.
if (_auto_reset_interval > 0 && (timestamp - _last_reset_time) > _auto_reset_interval) {
integral = _integral;
// apply coning corrections if required
if (_coning_comp_on) {
integral = _alpha + _beta;
} else {
integral = _alpha;
}
// reset the integrals and coning corrections
_reset(integral_dt);
return true;
@ -134,7 +147,7 @@ Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math::
math::Vector<3>
Integrator::get(bool reset, uint64_t &integral_dt)
{
math::Vector<3> val = _integral;
math::Vector<3> val = _alpha;
if (reset) {
_reset(integral_dt);
@ -160,9 +173,15 @@ Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3>
void
Integrator::_reset(uint64_t &integral_dt)
{
_integral(0) = 0.0f;
_integral(1) = 0.0f;
_integral(2) = 0.0f;
_alpha(0) = 0.0f;
_alpha(1) = 0.0f;
_alpha(2) = 0.0f;
_last_alpha(0) = 0.0f;
_last_alpha(1) = 0.0f;
_last_alpha(2) = 0.0f;
_beta(0) = 0.0f;
_beta(1) = 0.0f;
_beta(2) = 0.0f;
integral_dt = (_last_integration_time - _last_reset_time);
_last_reset_time = _last_integration_time;

10
src/drivers/device/integrator.h
View File

@ -102,10 +102,12 @@ private:
and the integrator reset, 0 if no auto-reset */
uint64_t _last_integration_time; /**< timestamp of the last integration step */
uint64_t _last_reset_time; /**< last auto-announcement of integral value */
math::Vector<3> _integral; /**< the integrated value */
math::Vector<3> _last_val; /**< previously integrated last value */
math::Vector<3> _last_delta; /**< last local delta */
bool _coning_comp_on; /**< coning compensation */
math::Vector<3> _alpha; /**< integrated value before coning corrections are applied */
math::Vector<3> _last_alpha; /**< previous value of _alpha */
math::Vector<3> _beta; /**< accumulated coning corrections */
math::Vector<3> _last_val; /**< previous input */
math::Vector<3> _last_delta_alpha; /**< integral from previous previous sampling interval */
bool _coning_comp_on; /**< true to turn on coning corrections */
/* we don't want this class to be copied */
Integrator(const Integrator &);