Fusion.

Pink is the range finder distance differentiated against time giving velocity in the quad Z axis – note how spikey it is.

Green is the accelerometer output integrated over time giving velocity in the quad Z axis – note that it’s drifting downwards between 6 and 10 seconds when the quad is actually hovering – I know because I’m holding it in my hands.

Orange is what you get when you fuse these together with a complementary filter – note it doesn’t drift downwards, and much of the spikiness is gone.

Time for real test flight tomorrow – weather is looking good too.

Dear Hove

Iam working on a quadcopter too. Do you mind telling me what happend in ur flight test after getting this sensor fusion? And if u can explain the mathematical representation of that

Thank you

Hi Ahmed,

I got distracted, and so this post was the last I did about the range finder fusion. Looking at the graphs above, and the code from back then, it looked like it worked well. Assuming you know my code a bit, this is what I did to fuse the differentiated range finder readings with the integrated accelerometer readings:

#---------------------------------------------------------------------------

# Get the latest URF height from the quadframe, differentiate to get velocity

# and kick off another ping - units of the URF are cm hence 100 factor.

#---------------------------------------------------------------------------

if time_now - prev_ping_time >= urf_period:

proximity = urf.readProximity() * math.cos(ta)

proximity /= 100.0

proximity_dt = time_now - prev_ping_time

urfv = (proximity - prev_proximity) / proximity_dt

prev_ping_time = time_now

prev_proximity = proximity

#-------------------------------------------------------------------

# Kick off another URF ping

#-------------------------------------------------------------------

urf.pingProximity()

`#---------------------------------------------------------------------------`

# Delete reorientated gravity from raw accelerometer readings and sum to make

# velocity all in quad frame - for the quad frame Z axis, merge the differentiate URF value

# with the integrated accelerometer value

#---------------------------------------------------------------------------

tau_fraction = tau / (tau + proximity_dt)

qvx_input = (qax - qfrgv_x) * mp_dt * GRAV_ACCEL

qvy_input = (qay - qfrgv_y) * mp_dt * GRAV_ACCEL

qvz_input = tau_fraction * (qvz_input + (qaz - qfrgv_z) * mp_dt * GRAV_ACCEL) + (1 - tau_fraction) * urfv