Gyrometer/accelerometer sensor fusion with sigmoid transfer function

I noticed, that accelerometer and gyrometer sensor data tend to have problems. The gyrometer is not very precise and the accelerometer is getting problems with vibrations. I built a quadrocopter (to be honest more then one). My first one had a poor frame and the motors (very big ones) were vibrating a lot, because the fix-plates were flexible.

So I was thinking about a software solution to compensate all the built-problems. I used an approach similiar to neural networks in which (sigmoid or other) transfer functions are used to anneal to a solution.

For quadcopters it is easy. The position to hold is normally always the straight/horizontal (equilibrium) one (if no rc input interfering). In this position the accelerometer shouldn't ideally appear much acceleration and give out values close to pitch=0 and roll=0. So I decided that my gyrometer stats, should anneal the faster to the accelerometer values, the closer it is to equilibrium position. In other scenarios I rely more to the gyrometer values (strong vibration or strong g-forces), which means the annealing rate goes down massivly.

I tested this approach and I think it is working for me. The attitude is stored in a Vector3f and contains the fused values of the gyrometer and the accelerometer. The raw sensor readings can be filtered previously of course before they get fused like here (high/low path for gyro/accelerometer).

I built a very poor model which is vibrating like hell at some degree of throttle, but it is stabilizing itself without flipping or crashs. Just the motors are sometimes upregulated, which results in increasing the height. At the moment I built already better models, but I still think about how to make it better. I mean there are a lot of filter-implementations out there, but I think that e.g. the Kalman filter wouldn't be able to compensate for problems like very strong vibrations and the complementary filter would be a total desaster. Maybe this is even helpful for someone.

In this example I used an annealing rate of 20. To calculate the absolute attitude from the gyrometer you have to keep in mind the angular velocity. This is why I used a timer. The attitude based on the accelermoter can be calculated and filtered easily by known methods.

/*
* Fuses two sensor values together by annealing angle_1 to angle_2
* in every time step by a given rate value.
*/
inline float smaller_float(float value, float bias) {
return value < bias ? value : bias;
}

inline float activ_float(float x, float force_mod = 20.f){
float val = (180.f - smaller_float(abs(force_mod * x), 179.9f) ) / 180.f;
return val / sqrt(1 + pow(val, 2));
}

inline float anneal_float(float angle_cor, float angle_fix, uint32_t time, float rate) {
return angle_cor += wrap180_float(angle_fix-angle_cor)*((float)time/1000.f)*rate;
}

// All sensor readouts works with absolute attitude in degrees:
void Device::update_inertial() {
m_pInert->update();
// Update sensor data

// Compensate yaw drift a bit with the help of the compass
uint32_t time = m_iTimer != 0 ? m_pHAL->scheduler->millis() - m_iTimer : INERTIAL_TIMEOUT;
m_iTimer = m_pHAL->scheduler->millis();

// Calculate absolute attitude from relative gyrometer changes
m_vAttitude.x += m_vGyro.x * (float)time/1000.f; // Pitch
m_vAttitude.x = wrap180_float(m_vAttitude.x);

m_vAttitude.y += m_vGyro.y * (float)time/1000.f; // Roll
m_vAttitude.y = wrap180_float(m_vAttitude.y);

m_vAttitude.z += m_vGyro.z * (float)time/1000.f; // Yaw
m_vAttitude.z = wrap180_float(m_vAttitude.z);

// Calculate absolute attitude from relative gyrometer changes
m_vAttitude.x = anneal_float(m_vAttitude.x, m_vAccel.x, time, activ_float(m_vAccel.x, 20.f) );
m_vAttitude.y = anneal_float(m_vAttitude.y, m_vAccel.y, time, activ_float(m_vAccel.y, 20.f) );
}


Edit: This is my slightly modified version

inline float pow2_f(float fVal) {
return fVal * fVal;
}

inline float wrap180_f(float x) {
return x < -180.f ? (x + 360.f) : (x > 180.f ? (x - 360.f) : x);
}

/*
* This function changes the function parameter (for performance reasons)
*/
inline Vector3f wrap180_V3f(Vector3f &vec) {
vec.x = wrap180_f(vec.x);
vec.y = wrap180_f(vec.y);
vec.z = wrap180_f(vec.z);
return vec;
}

inline float sigm_f(float x, float mod = 20.f){
float val = (180.f - smaller_f(abs(mod * x), 179.9f) ) / 180.f;
return val / sqrt(1 + pow2_f(val) );
}

/*
* Fuses two sensor values together by annealing angle_fuse to angle_ref
* mod: Determines the slope (decay) of the sigmoid activation function
* rate: Determines how fast the annealing takes place
*/
inline Vector3f anneal_V3f( Vector3f &angle_fuse,
const Vector3f &angle_ref, float time, float mod, float rate) {
float fR = rate * time;
angle_fuse.x += (angle_ref.x-angle_fuse.x) * fR * sigm_f(angle_ref.x, mod);
angle_fuse.y += (angle_ref.y-angle_fuse.y) * fR * sigm_f(angle_ref.y, mod);
angle_fuse.z += (angle_ref.z-angle_fuse.z) * fR * sigm_f(angle_ref.z, mod);
angle_fuse = wrap180_V3f(angle_fuse);
return angle_fuse;
}
void Device::update_inertial() {
m_pInert->update();
// Update sensor data
// Calculate time (in s) passed
float time_s = time_elapsed_s();
// Calculate attitude from relative gyrometer changes
m_vAttitude += m_vGyro * time_s;
// For yaw changes the compass could be used as reference,
// otherwise take the gyrometer
if(COMPASS_FOR_YAW) {
} else {
m_vAccel.z = m_vAttitude.z;
}
// Calculate absolute attitude from relative gyrometer changes
m_vAttitude = anneal_V3f(m_vAttitude, m_vAccel, time_s, 20.f, 5.f);
}


A few notes:

inline float smaller_float(float value, float bias) {
return value < bias ? value : bias;
}


This seems intended to do exactly the same thing as std::min in the standard library. You're probably better off using the standard version.

inline float activ_float(float x, float force_mod = 20.f){
float val = (180.f - smaller_float(abs(force_mod * x), 179.9f) ) / 180.f;
return val / sqrt(1 + pow(val, 2));
}


I'm not sure about the name you've used here, and specifically why you'd use activ instead of active. Obviously, you'd want to incorporate the previous change, and use std::min here. Along with that, I'd at least consider val * val rather than pow(val, 2). pow is good for arbitrary exponents, but for a fixed exponent of 2 can waste a fair amount of time, and doesn't (at least IMO) improve readability at all.

[ ... ]

// Compensate yaw drift a bit with the help of the compass
uint32_t time = m_iTimer != 0 ? m_pHAL->scheduler->millis() - m_iTimer : INERTIAL_TIMEOUT;
m_iTimer = m_pHAL->scheduler->millis();


I think I'd prefer to move most of this into a function, so this came out something like:

uint32_t time = delta_time();


I also question the use of uint32_t here. Do you really need the result to be exactly 32 bits, or would uint_least32_t or uint_fast32_t really express your intent better?

  // Calculate absolute attitude from relative gyrometer changes
m_vAttitude.x += m_vGyro.x * (float)time/1000.f; // Pitch
m_vAttitude.x = wrap180_float(m_vAttitude.x);

m_vAttitude.y += m_vGyro.y * (float)time/1000.f; // Roll
m_vAttitude.y = wrap180_float(m_vAttitude.y);

m_vAttitude.z += m_vGyro.z * (float)time/1000.f; // Yaw
m_vAttitude.z = wrap180_float(m_vAttitude.z);


I think I'd move this repeated code into a function, so this would come out something like:

float millis = time / 1000.f;
m_vAttitude.x = foo(m_vGyro.x, millis);
m_vAttitude.y = foo(m_vGyro.y, millis);
v_vAttitude.z = foo(m_vGyro.z, millis);


...and probably move that all into a function as well, so the top-level call was something like:

m_vAttitude = foo(m_vGyro, time);

• The tip with "uint_fast32_t", .. is really interesting! Commented Mar 5, 2014 at 19:58

In addition to what Jerry Coffin said, I would suggest renaming active_float and anneal_float to active and anneal. Since you are using C++, you don't need to have different names for the same function taking different types, you can provide overloads when needed. Generally, you provide different names only in C, and had you followed the C convention, your functions would probably have been named activef and annealf.