What do you get if you cross an accelerometer with an ultrasonic range finder?

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.

Velocity fusion

Velocity fusion

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

2 thoughts on “What do you get if you cross an accelerometer with an ultrasonic range finder?

  1. 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

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.