Forem: Andrea Benfatti

Use GPS and magnetometer for accurate heading

Andrea Benfatti — Mon, 23 Sep 2019 10:02:30 +0000

During my last year, I have been working with affordable autonomous drones and the biggest problem has always been having an accurate heading. The aim was to build a drone able to drive around by itself keeping the price as low as possible.

The Problem

I needed a way to know in which direction the drone is heading, and after a fast google search for less than 50$ I bought a "BNO055 Absolute Position Sensor", or "APS" as I call it, which should fuse magnetometer, gyroscope, and accelerometer to estimate the 3d position of the sensor in the space.
This worked really well, until with took the drone for a ride outside where the heading started shifting reaching errors up to 90 degrees.

The Failed Attempts

Storing the calibration data

The BNO055 it's quite nice to work with (except for the wrong heading), using the Adafruit library to communicate with the APS it is really easy to read the calibration status of each sensor and it's even possible to save and load calibration data.
The calibration goes from 0 which means useless readings to 3. Usually, after connecting the APS and moving it a bit to calibrate it, it would always reach 3 and the heading would be correct. This made me think that if I store the calibration data when the calibration is at 3 and reload them when the calibration goes down it would fix the heading.
This worked in the office but it was useless outside where the calibration would not rise to 3 when reloading the calibration data.

Automatic Calibration System (ACS)

After the previous failed attempt i noticed that rotating the senor on the 3 axis by 90 degree was enough to calibrate the magnetometer and to have an accurate heading so after a joking about it with some colleague I ended up building a mechanism to move the sensor on 2 axes using 2 servo motor since the drone could rotate on the 3rd axis.
This solution gave us high hope and it was working wonderfully in the office, taking the calibration from 0 to 3 in half-second, unfortunately even this was failing while the drone was moving, the weirdest part was that the calibration would rise to 3 but the heading was still incorrect.

These are the two ACS design I tested without success.

The solution

After giving up trying to fix the sensor I noticed that the GPS can give you a really accurate heading when the drone is moving but it's totally useless if the drone it's rotating in place, so we ended up merging the heading from the GPS with the heading given by the APS to have a reliable reading.

Kalman filter

From Wikipedia "The Kalman filter is an algorithm which uses a series of measurements observed over time and produces estimates of unknown variables that tend to be more accurate than those based on a single measurement alone"
I'm not going to explain you the math behind the Kalman filter but you can easily find it online, I will instead explain how I use it to merge the GPS and APS heading.

The Kalman filter has 2 steps, a prediction step, where it takes as input some sensor data which can be used to predict X given the previous value of X, and an update step, where it takes as input the value of X with some noise and it uses it to fix the prediction. The filter will always produce the most likely value of X.

How to apply it

To be able to use the Kalman filter we need an unknown variable X, which will be our heading, a noisy reading of X, which will be the GPS heading, and some sensor which can be used to calculate the next heading knowing the previous one.
For this last part, we will use the APS, but we will not use the heading directly, since it is subject to shifting, instead, we will use the difference in heading between two consecutive reading as a rotation, this can be added to the previous heading to estimate the next one.

Practical example

For my project, I used a Kalman filter implementation found on Github
https://github.com/zziz/kalman-filter

As you can see the Kalman filter is controlled by 7 matrices:

x is the unknown variable we are trying to estimate using Kalman filter, x0 is the initial value we want to assign to it, in our case since the heading is a single value we can give it [[0]] or leave it empty.

F and B are used to calculate the new value of x during the prediction phase using this formula x = F*x + B*u, where u is the value read from the sensor, in our case it will be how much the compass value has changed since the last Kalman filter prediction. This makes things easy because F and B can be identity matrices ([[1]]) which will make the formula x = x+u

H is used to calculate the error between the Kalman prediction and the real value z during the update phase using this formula: e = z - H*x as before in our case we don't need to modify the shape of x so we can use an identity matrix [[1]]

Q R and P are covariances matrices used to indicate how accurate are our sensors and calculation, where R indicate the covariance on the sensor reading while Q and P indicate the error in the calculation process. For our purpose we can set P to [[0]], Q to [[0.1]] to indicate that our calculation given the correct data are pretty accurate while we can test different values for R to give more or less confidence to the GPS reading.

Code

F = np.array([[1]])
B = np.array([[1]])
H = np.array([[1]])
Q = np.array([[0.1]])
R = np.array([[50]])
P = np.array([[0]])
kf = KalmanFilter(F=F, B=B, H=H, Q=Q, R=R, P=P)

old_heading = compass.getHeading()
counter = 0

while True:
    counter += 1

    new_heading = compass.getHeading()
    delta_heading = new_heading - old_heading
    old_heading = new_heading

    predicted_heading = kf.predict(delta_heading)[0][0]

    # normalize heading
    predicted_heading = predicted_heading % 360
    kf.x = predicted_heading

    # read GPS heading only once per second since
    # the sensor has 1 Hz update rate
    if counter % 10:
        gps_heading = gps.getHeading()

        # since the heading goes from 359 to 0 
        # this make the error always < 180 between
        # the predicted heading and the gps heading
        if gps_heading - predicted_heading > 180:
            gps_heading -= 360
        if gps_heading - predicted_heading < -180:
            gps_heading += 360

        kf.update(np.array[[gps_heading]])

    time.sleep(0.1)

Theoretically, this should work but the real world doesn't care, so we need some more tweaks. The problem with the GPS is that if we are not moving the heading is random and it also takes some times to realize we are moving. To fix this I change the last if as follow

# read GPS heading only once per second since
# the sensor has 1 Hz update rate
if is_drone_going_straght and gps.getSpeed() > 0.5 and counter % 10:
    gps_heading = gps.getHeading()

    # since the heading goes from 359 to 0 
    # this make the error always < 180 between
    # the predicted heading and the gps heading
    if gps_heading - predicted_heading > 180:
        gps_heading -= 360
    if gps_heading - predicted_heading < -180:
        gps_heading += 360

    kf.update(np.array[[gps_heading]])

time.sleep(0.1)

where is_drone_going_straight is a boolean calculate using the value that we are sending to the engines, so if the values are both positive and very similar it means we are going straight.
Using this we got a quite reliable heading sensor which works even if the compass starts shifting.

We did some test on the field with the drone using this algorithm and adding random shifts to the compass data to simulate a bad reading and it's possible to see the drone start moving in the wrong direction and going back to the original position after just a few seconds.

Why serverless?

Andrea Benfatti — Tue, 05 Mar 2019 12:45:04 +0000

These days I was looking into serverless and it seems very interesting, cool and futuristic but digging deeper I could not really understand if it's worth given the additional effort.
From what I read it seems that it's more difficult to implement, and it's really hard to predict the cost of it given a lot of hidden costs. Of course, there are pros too, highly scalable and u don't have to manage servers etc.
What are your experiences with serverless? Does it actually cost less? is it worth the additional learning and implementation time?

Async programming in Python with asyncio

Andrea Benfatti — Sun, 15 Jul 2018 23:58:59 +0000

For people coming from JavaScript asynchronous programming is nothing new, but for python developers getting used to async functions and future (the equivalent of promise in JS) may not be trivial

Concurrency vs Parallelism

Concurrency and parallelism can sound really similar but in programming there is an important difference.
Immagine you are writing a book while cooking, even if it seems like you are doing both tasks at the same time, what you are doing is switching between the two tasks, while you wait for the water to boil you are writing your book, but while you are chopping some vegetables you pause your writing. This is called concurrency. The only way to do these two tasks in parallel is having two people, one writing and one cooking, which is what multicore CPU do.

Why asyncio

Async programming allows you to write concurrent code that runs in a single thread. The first advantage compared to multiple threads is that you decide where the scheduler will switch from one task to another, which means that sharing data between tasks it's safer and easier.

def queue_push_back(x):
    if len(list) < max_size:
        list.append(x)

If we run the code above in a multithread program it's possible that two threads execute line 2 at the same time so 2 items will be added to the queue at the same time and potentially making the queue size bigger than max_size

Another advantage of async programming is memory usage. Every time a new thread is created some memory is used to allow context switching, if we use async programming this is not a problem since the code runs in a single thread.

How to write async code in python

Asyncio has 3 main components: coroutines, event loop, and future

Coroutine

A coroutine is the result of an asynchronous function which can be declared using the keyword async before def

async def my_task(args):
    pass

my_coroutine = my_task(args)

When we declare a function using the async keyword the function is not run, instead, a coroutine object is returned.

There are two ways to read the output of an async function from a coroutine.
The first way is to use the await keyword, this is possible only inside async functions and will wait for the coroutine to terminate and return the result

result = await my_task(args)

The second way is to add it to an event loop as we will see in the next sections.

Event loop

The event loop is the object which execute our asyncronous code and decide how to switch between async functions. After creating an event loop we can add multiple coroutines to it, this corutines will all be running concurrently when run_until_complete or run_forever is called.

# create loop
loop = asyncio.new_event_loop()
# add coroutine to the loop
future = loop.create_task(my_coroutine)
# stop the program and execute all coroutine added
# to the loop concurrently
loop.run_until_complete(future)
loop.close()

Future

A future is an object that works as a placeholder for the output of an asynchronous function and it gives us information about the function state.
A future is created when we add a corutine to an event loop. There are two way to this:

future1 = loop.create_task(my_coroutine)
# or
future2 = asyncio.ensure_future(my_coroutine)

The first method adds a coroutine to the loop and returns a task which is a subtype of future. The second method is very similar, it takes a coroutine and it adds it to the default loop, the only difference is that it can also accept a future, in which case it will not do anything and return the future unchanged.

A simple program

import asyncio

async def my_task(args):
    pass

def main():
    loop = asyncio.new_event_loop()
    coroutine1 = my_task()
    coroutine2 = my_task()
    task1 = loop.create_task(coroutine1)
    task2 = loop.create_task(coroutine2)
    loop.run_until_complete(asyncio.wait([task1, task2]))
    print('task1 result:', task1.result())
    print('task2 result:', task2.result())
    loop.close()

As you can see to run an asynchronous function we first need to create a coroutine, then we add it to the event loop which create a future/task. Up to this point none of the code inside our async function has been executed, only when we call loop.run_until_completed the event loop start executing all the coroutines that have been added to the loop with loop.create_task or asyncio.ensure_future.
loop.run_until_completed will block your program until the future you gave as argument is completed. In the example we used asyncio.wait() to create a future which will be complete only when all the futures passed in the argument list are completed.

Async functions

One thing to keep in mind while writing asynchronous functions in python is that just because you used async before def it doesn't mean that your function will be run concurrently. If you take a normal function and add async in front of it the event loop will run your function without interruption because you didn't specify where the loop is allowed to interrupt your function to run another coroutine. Specify where the event loop is allowed to change coroutine is really simple, every time you use the keyword await the event loop can stop running your function and run another coroutine registered to the loop.

async def print_numbers_async1(n, prefix):
    for i in range(n):
        print(prefix, i)

async def print_numbers_async2(n, prefix):
    for i in range(n):
        print(prefix, i)
        if i % 5 == 0:
            await asyncio.sleep(0)

loop1 = asyncio.new_event_loop()
count1_1 = loop1.create_task(print_numbers_async1(10, 'c1_1')
count2_1 = loop1.create_task(print_numbers_async1(10, 'c2_1')
loop1.run_until_complete(asyncio.wait([count1_1, count2_1])
loop1.close()

loop2 = asyncio.new_event_loop()
count1_2 = loop1.create_task(print_numbers_async1(10, 'c1_2')
count2_2 = loop1.create_task(print_numbers_async1(10, 'c2_2')
loop2.run_until_complete(asyncio.wait([count1_2, count2_2])
loop2.close()

If we execute this code we will see that loop1 will print first print all numbers with prefix c1_1 and then with the prefix c2_1 while in the second loop every 5 numbers the loop will change task.

Real world example

Now that we know the basics of asynchronous programming in python let's write some more realistic code which will download a list of pages from the internet and print a preview containing the first 3 lines of the page.

import aiohttp
import asyncio

async def print_preview(url):
    # connect to the server
    async with aiohttp.ClientSession() as session:
        # create get request
        async with session.get(url) as response:
            # wait for response
            response = await response.text()

            # print first 3 not empty lines
            count = 0
            lines = list(filter(lambda x: len(x) > 0, response.split('\n')))
            print('-'*80)
            for line in lines[:3]:
                print(line)
            print()

def print_all_pages():
    pages = [
        'http://textfiles.com/adventure/amforever.txt',
        'http://textfiles.com/adventure/ballyhoo.txt',
        'http://textfiles.com/adventure/bardstale.txt',
    ]

    tasks =  []
    loop = asyncio.new_event_loop()
    for page in pages:
        tasks.append(loop.create_task(print_preview(page)))

    loop.run_until_complete(asyncio.wait(tasks))
    loop.close()

This code should be pretty easy to understand, we start by creating an asynchronous function which downloads an URL and prints the first 3 not empty lines. Then we create a function which for each page in a list of pages call print_preview, add the coroutine the to loop and store the future inside a list of tasks. Finally, we run the event loop which will run the coroutine we added to it and it will print the preview of all the pages.

Async generator

The last feature I want to talk about is asynchronous generator. Implementing an asynchronous generator is quite simple.

import asyncio
import math
import random

async def is_prime(n):
    if n < 2:
        return True
    for i in range(2, n):
        # allow event_loop to run other coroutine
        await asyncio.sleep(0)
        if n % i == 0:
            return False
    return True

async def prime_generator(n_prime):
    counter = 0
    n = 0
    while counter < n_prime:
        n += 1
        # wait for is_prime to finish
        prime = await is_prime(n)
        if prime:
            yield n
            counter += 1

async def check_email(limit):
    for i in range(limit):
        if random.random() > 0.8:
            print('1 new email')
        else:
            print('0 new email')
        await asyncio.sleep(2)

async def print_prime(n):
    async for prime in prime_generator(n):
        print('new prime number found:', prime)

def main():
    loop = asyncio.new_event_loop()
    prime = loop.create_task(print_prime(3000))
    email = loop.create_task(check_email(10))
    loop.run_until_complete(asyncio.wait([prime, email]))
    loop.close()

Exception handling

When an unhandled exception is raised inside a coroutine it doesn't break our program as in normal synchronous programming, instead, it's stored inside the future and if you don't handle the exception before the program exit you will get the following error

Task exception was never retrieved

There are two ways to fix this, catch the exception when you access the future result or calling the future exception method.

try:
    # this will raise the exception raised during the coroutine execution
    my_promise.result()
catch Exception:
    pass

# this will return the exception raised during the coroutine execution
my_promise.exception()

Going deeper

If you have read everything up to this point you should know how to use asyncio to write concurrent code, but if you wish to go deeper and understand how asyncio works I suggest you watch the following video

If you would like to see more complex uses of asyncio or if you have any question leave a comment and I will replay to you as soon as possible

ProofPick