This video shows the 250mm quadcopter from my previous post, holding altitude by means of its MaxBotix MB1242 ulatrasonic rangefinder (sonar). The Flip32 flight controller onboard is running a modified version of the Baseflight firmware to support the MB1242. Original Baseflight requires a barometer for alt-hold, and then fuses it with data from an HC-SR04 sonar if one is available. So I removed the baro requirement and replaced the HC-SR04 code with code that uses the MB1242.
The real-time plot (made in Python with RealTimePlotter) shows AGL (Above Ground Level) in centimeters (blue) as well as the desired altitude that locks in when I flip a switch on the transmitter (red). The desired altitude starts out with its value from a previous run, which has no effect until I hit the switch, at which point it resets to the current AGL. The bottom plot is the PID (Position / Integral / Derivative) -based correction to the throttle in support of altitude hold, which likewise has no effect until I hit the switch.
Although the vehicle holds low altitude (here, around a meter) pretty well, the values being displayed are quite a bit off from what you expect to see (vehicle is holding AGL at around 100cm, though reported AGL is 60cm, with locked-in AGL at 40cm). At higher altitudes, big transient jumps in the sonar-measured AGL can cause the vehicle to jump slightly. So my next steps are to (1) figure out the discrepancy between the observed behavior and displayed signals; (2) add an Extended Kalman Filter to smoothe out transient jumps in the sonar signal.
Comments
Great work Simon!
Ach, sorry Linus, I misunderstood you and I keep forgetting, YouTube blocks DRMed content in some countries! I will try to to remember for next vid. Meanwhile if you can't view the video, let me know and I'll remove the soundtrack.
Thanks, Linus! Good suggestion on the music, but I always buy each song from Amazon before using it.
Maybe you might consider choosing free music for your video next time. Else i have to say, geat stuff! As always Simon. Thanks for sharing
Cool, thanks for the tip Randy!
What we do in Copter is maintain a separate target sonar alt and then use the error between the target sonar alt and actual sonar target alt to adjust the target baro+accel (i.e EKF) altitude. The ultimate solution, as perhaps you're suggesting, is to have the EKF maintain separate alt-above-sea-level and alt-above-terrain values.
yes, the sonar is tricky.
in my tests the ultrasoinic devices where a tad better as the maxbotix devices.
will be very interesting to see the your nice kalman filter in action.
best
robert
Update #2: built a more manageable rig for securing the vehicle level to the ground:
In addition to the carpet shown above, I collected measurements over the tiled floor from the video. Carpet vs. tile seemed to make no difference, so I just ran a little linear regression on the sonar measurements vs. ground truth (as measured with a tape measure from the downward-facing surface of the sonar barrel to the floor): Got slope = 0.933 intercept = -2.894 rvalue = 1.000. So that'll go into the part of the firmware that acquires data from the MB1242.
For any Pythonistas reading this, here's the little program I wrote to calculate and plot the regression:
from scipy.stats import linregress
import numpy as np
import matplotlib.pyplot as plt
groundtruth = np.array([64, 123, 63, 104, 66, 397, 26, 26])
sonar = np.array([57, 111, 55, 94, 59, 368, 22, 22])
slope, intercept, rvalue, _, _ = linregress(groundtruth, sonar)
print('slope = %3.3f intercept = %3.3f rvalue = %3.3f' %
(slope, intercept, rvalue))
plt.plot(groundtruth, sonar, 'o')
plt.xlabel('Ground Truth (cm)')
plt.ylabel('MB1242 Sonar (cm)')
plt.axis('equal')
plt.show()
Using apparatus below I read AGL as 123 cm off a tape measure. (Vehicle is actually more level than it appears in photo.) Logfile shows undershoot plus noise. Hence alt-hold value is likely to lock on to a value significantly higher or lower than the actual AGL at the instance when I flip the switch.