xioTechnologies commented 1 month ago

The following is copied from our website About page and summarises the difference. It would not be correct to call Fusion a gradient descent algorithm because it calculates the feedback error in a differnt way.

The original algorithm, commonly known as the Madgwick algorithm was first published in 2011 and is cited by thousands of research publications. The algorithm has continued to evolve over the years, benefiting from more than a decade of experience developing IMU products and providing consulting services to a wide range of applications. Today, the algorithm is called Fusion and is available on GitHub.

lucker26 commented 1 week ago

以下内容复制自我们的网站“关于”页面，并总结了差异。将 Fusion 称为梯度下降算法是不正确的，因为它以不同的方式计算反馈误差。

最初的算法，通常被称为 Madgwick 算法，于 2011 年首次发表，并被数千篇研究出版物引用。多年来，该算法不断发展，受益于十多年的 IMU 产品开发经验和为广泛的应用提供咨询服务。今天，该算法称为 Fusion，可在 GitHub 上使用。

Thank you for your answer. There are still some questions I want to ask you. When the algorithm has been initialized, the gain will drop from 10 to the final value. After a few seconds, it will suddenly start to drop again from 10, leading to the abnormality of quaternion, and the coordinates displayed on the screen will also drift around. What are the conditions that lead to the gain falling again? And if you want to improve the accuracy of this algorithm, what directions can you try? Looking forward to your reply

xioTechnologies commented 1 week ago

What are the conditions that lead to the gain falling again?

I suggest you monitor the algorithm flags and internal states to understand why might be going on.

And if you want to improve the accuracy of this algorithm, what directions can you try?

Your first priority should always be improving the sensor calibration.

lucker26 commented 1 week ago

哪些条件导致收益再次下跌？

我建议您监控算法标志和内部状态，以了解可能发生的原因。

而如果你想提高这个算法的准确性，你可以尝试哪些方向呢？

您的首要任务应该是改进传感器校准。

define INITIAL_GAIN (10.0f)

/**

@brief Initialisation period in seconds. */
define INITIALISATION_PERIOD (3.0f)

//------------------------------------------------------------------------------ // Function declarations

static FusionVector HalfGravity(const FusionAhrs *const ahrs);

static FusionVector HalfMagnetic(const FusionAhrs *const ahrs);

//------------------------------------------------------------------------------ // Functions

/**

@brief Initialises the AHRS algorithm structure.
@param ahrs AHRS algorithm structure. */

void FusionAhrsInitialise(FusionAhrs *const ahrs) {

const FusionAhrsSettings settings = {
    .convention = FusionConventionNwu,
    .gain = 0.5f,
    .accelerationRejection = 90.0f,
    .magneticRejection = 90.0f,
    .rejectionTimeout = 0,
};
FusionAhrsSetSettings(ahrs, &settings);
FusionAhrsReset(ahrs);

}

/**

@brief Resets the AHRS algorithm. This is equivalent to reinitialising the
algorithm while maintaining the current settings.
@param ahrs AHRS algorithm structure. / void FusionAhrsReset(FusionAhrs const ahrs) { ahrs->quaternion = FUSION_IDENTITY_QUATERNION; ahrs->accelerometer = FUSION_VECTOR_ZERO; ahrs->initialising = true; ahrs->rampedGain = INITIAL_GAIN; ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO; ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO; ahrs->accelerometerIgnored = false; ahrs->accelerationRejectionTimer = 0; ahrs->accelerationRejectionTimeout = false; ahrs->magnetometerIgnored = false; ahrs->magneticRejectionTimer = 0; ahrs->magneticRejectionTimeout = false; }

/**

@brief Sets the AHRS algorithm settings.
@param ahrs AHRS algorithm structure.
@param settings Settings. / void FusionAhrsSetSettings(FusionAhrs const ahrs, const FusionAhrsSettings const settings) { ahrs->settings.convention = settings->convention; ahrs->settings.gain = settings->gain; if ((settings->accelerationRejection == 0.0f) || (settings->rejectionTimeout == 0)) { ahrs->settings.accelerationRejection = FLT_MAX; } else { ahrs->settings.accelerationRejection = powf(0.5f sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2); } if ((settings->magneticRejection == 0.0f) || (settings->rejectionTimeout == 0)) { ahrs->settings.magneticRejection = FLT_MAX; } else { ahrs->settings.magneticRejection = powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2); } ahrs->settings.rejectionTimeout = settings->rejectionTimeout; if (ahrs->initialising == false) { ahrs->rampedGain = ahrs->settings.gain; } ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD; }

/**

@brief Updates the AHRS algorithm using the gyroscope, accelerometer, and
magnetometer measurements.
@param ahrs AHRS algorithm structure.
@param gyroscope Gyroscope measurement in degrees per second.
@param accelerometer Accelerometer measurement in g.
@param magnetometer Magnetometer measurement in arbitrary units.

@param deltaTime Delta time in seconds. / void FusionAhrsUpdate(FusionAhrs const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime) {

define Q ahrs->quaternion.element

// Store accelerometer ahrs->accelerometer = accelerometer;

// Log the current value of rampedGain NRF_LOG_INFO("rampedGain: " NRF_LOG_FLOAT_MARKER, NRF_LOG_FLOAT(ahrs->rampedGain));

// Log the initialization status if (ahrs->initialising) { NRF_LOG_INFO("Initialization in progress. RampedGain: " NRF_LOG_FLOAT_MARKER, NRF_LOG_FLOAT(ahrs->rampedGain)); } else { NRF_LOG_INFO("Initialization completed. Current RampedGain: " NRF_LOG_FLOAT_MARKER, NRF_LOG_FLOAT(ahrs->rampedGain)); }

// Ramp down gain during initialisation if (ahrs->initialising == true) { ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime;

if (ahrs->rampedGain < ahrs->settings.gain)
{
    ahrs->rampedGain = ahrs->settings.gain;
    ahrs->initialising = false;
    ahrs->accelerationRejectionTimeout = false;
}

}

// Calculate direction of gravity indicated by algorithm const FusionVector halfGravity = HalfGravity(ahrs);

// Calculate accelerometer feedback FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO; ahrs->accelerometerIgnored = true; if (FusionVectorIsZero(accelerometer) == false) {

// Enter acceleration recovery state if acceleration rejection times out
if (ahrs->accelerationRejectionTimer > ahrs->settings.rejectionTimeout)
{
    const FusionQuaternion quaternion = ahrs->quaternion;
    FusionAhrsReset(ahrs);
    ahrs->quaternion = quaternion;
    ahrs->accelerationRejectionTimer = 0;
    ahrs->accelerationRejectionTimeout = true;
}

// Calculate accelerometer feedback scaled by 0.5
ahrs->halfAccelerometerFeedback = FusionVectorCrossProduct(FusionVectorNormalise(accelerometer), halfGravity);

// Ignore accelerometer if acceleration distortion detected
if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))
{
    halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback;
    ahrs->accelerometerIgnored = false;
    ahrs->accelerationRejectionTimer -= ahrs->accelerationRejectionTimer >= 10 ? 10 : 0;
}
else
{
    ahrs->accelerationRejectionTimer++;
}

}

// Calculate magnetometer feedback FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO; ahrs->magnetometerIgnored = true; if (FusionVectorIsZero(magnetometer) == false) {

// Set to compass heading if magnetic rejection times out
ahrs->magneticRejectionTimeout = false;
if (ahrs->magneticRejectionTimer > ahrs->settings.rejectionTimeout)
{
    FusionAhrsSetHeading(ahrs, FusionCompassCalculateHeading(ahrs->settings.convention, halfGravity, magnetometer));
    ahrs->magneticRejectionTimer = 0;
    ahrs->magneticRejectionTimeout = true;
}

// Calculate direction of magnetic field indicated by algorithm
const FusionVector halfMagnetic = HalfMagnetic(ahrs);

// Calculate magnetometer feedback scaled by 0.5
ahrs->halfMagnetometerFeedback = FusionVectorCrossProduct(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic);

// Ignore magnetometer if magnetic distortion detected
if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))
{
    halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback;
    ahrs->magnetometerIgnored = false;
    ahrs->magneticRejectionTimer -= ahrs->magneticRejectionTimer >= 10 ? 10 : 0;
}
else
{
    ahrs->magneticRejectionTimer++;
}

}

// Convert gyroscope to radians per second scaled by 0.5 const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f));

// Apply feedback to gyroscope const FusionVector adjustedHalfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain));

// Integrate rate of change of quaternion ahrs->quaternion = FusionQuaternionAdd(ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));

// Normalise quaternion ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);

undef Q

} 这是我的设置，通过log输出确实发现增益会重新从10开始下降的问题，您看我这边的代码设置有问题吗

xioTechnologies commented 1 week ago

这是我的设置，通过log输出确实发现增益会重新从10开始下降的问题，您看我这边的代码设置有问题吗

Google translate: This is my setting. Through the log output, I found that the gain will start to decrease again from 10. Do you think there is something wrong with my code setting?

The gain is designed to start at 10 and ramp down to the final value. This is documented as initialisation. Initialisation also occurs as a part of angular rate recovery.

The code you have shared does not match that provided in this repository. I suggest you use the code as provided.

lucker26 commented 1 week ago

这是我的设置，通过log输出确实发现增益会重新从10开始下降的问题，您看我这边的代码设置有问题吗

谷歌翻译：这是我的设置。通过对数输出，我发现增益将从 10 开始再次下降。您认为我的代码设置有问题吗？

增益设计为从 10 开始，然后逐渐下降到最终值。这被记录为 initialisation。初始化也是角速率恢复的一部分。

您共享的代码与此存储库中提供的代码不匹配。我建议您使用提供的代码。

I want to know under what circumstances the gain will be initialized, and it will start to decline about the third second after the device is started.

xioTechnologies commented 1 week ago

The circumstances are described in initialisation and angular rate recovery.

xioTechnologies / Fusion

What is the difference between this algorithm and madwick gradient algorithm? Can it still be called gradient descent algorithm? #171

define INITIAL_GAIN (10.0f)

define INITIALISATION_PERIOD (3.0f)

define Q ahrs->quaternion.element

undef Q