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 e

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);

StackExchange Code Review Q#43393, answer score: 6