Introduction

The information received from sensors is generally noisy and uncertain. Sensor filters are transformations applied to those sensor measurements to manage the noise in the presence of uncertainty.

Learning Objectives

At the end of this lab, you should understand:

• What are basic sensor filters and why they are used
• Moving average filter
• Basic sensor fusion

Additionally, you will learn about:

• using locks to prevent race conditions
• using thread-safe classes
• initial testing practices in this context

Update Fastsim

Note: if you are using the lightweight version of the simulator (fastersim) replace fastsim with fastersim everywhere below.

$ cd ~/fastsim

$ git pull

$ catkin build

Overall Architecture

The altitude of the drone is provided as a z coordinate by the GPS sensor. Additionally we will calculate the pressure altitude from using values provided by the pressure sensor.

Next, we apply a moving average filter to both the values provided by the GPS and the pressure altitude to smooth out the variance of the data.

Finally, we calculate an average of the two moving averages.

Altitude Estimation

We will start by visualizing the altitude provided by the drone GPS signal. Then we will use the barometric pressure sensor to estimate the altitude. Next, we will introduce a simple filter and sensor fusion based on a moving average.

GPS Sensor Noise

Noise is unwanted modifications to a signal or value. There are many types of noise, but in this lab we consider the Gaussian noise. The Gaussian noise models the effect of random processes and it has a normal distribution with zero average.

For a visual representation of our drone altitude measured using the GPS use the following command after starting the simulator and sourcing the workspace:

$ rqt_plot /uav/sensors/gps/pose/position/z

Try to estimate the average altitude at which the drone hovers and the highest and lowest points. Use the rostopic echo to confirm your your answers.

Pressure Sensor for Altitude Estimation

Another method for calculating the altitude is based on the relationship between the barometric pressure and altitude. In this section, we start by creating a node, pressure_altitude_node, that calculates the altitude from the values provided by the pressure sensor and publishes it on the topic /uav/sensors/pressure_altitude.

We start by creating a new package for this lab, called altitude. We also create a custom message type that contains the real value for altitude and a time stamp (the timestamp will come handy later when we cover Kalman Filters in the extra credit part of Lab 5).

1. Create the altitude package:

$ catkin_create_pkg altitude std_msgs geometry_msgs rospy message_generation

2. Create the custom message type for the pressure altitude measurements:

$ cd ~/fastsim/src/altitude

$ mkdir msg

In the newly created folder, ~/fastsim/src/altitude/msg, use your favorite editor to create the file AltitudeStamped.msg, with content:

float64 value

time stamp

This is the specification of your message type. The new message had type and AltitudeStamped contains two attributes: value of type float, and stamp of type rospy.time. Recall, to create a service you had to define a message specification in a srv file and update package.xml and CMakeLists.txt.

We created the specification of the message in the AltitudeStamped.msg file. Because in the create_package command, we already specified that the package altitude depends on packages that support custom message generation, standard messages and geometry messages, we do not need to add them to package.xml.

Add or uncomment the following lines to CMakeLists.txt if they are not already there:

add_message_files(

  FILES

  AltitudeStamped.msg

)

generate_messages(

   DEPENDENCIES

   std_msgs

)

catkin_package(

  CATKIN_DEPENDS message_runtime 

)

The changes to CMakeList.txt are necessary to indicate the files containing the definition of the custom message and to create the correct dependencies to support custom messages.

To check that the message is created correctly, build the workspace, source it and run the command below:

$ rosmsg show altitude/AltitudeStamped

The expected output is:

float64 value

time stamp

3. Create a node, pressure_altitude_node, that calculates the pressure altitude from pressure values using the formula:

 (1 - pow(pressure/1013.25, 0.190284)) * 145366.45 * 0.3048

The node must subscribe to the topic /uav/sensors/pressure and publish on the topic /uav/sensors/pressure_altitude, messages of type altitude.msg.AltitudeStamped.

The code for the node should be implemented in the file pressure_altitude.py.

You may use the code below to help you with the implementation of this node.

#!/usr/bin/env python

import rospy

import time

from std_msgs.msg import Float64

from altitude.msg import AltitudeStamped

class PressureAltitude():

    # Node initialization

    def __init__(self):

        # create the message first to avoid race conditions with the 

        # subscriber callback

        self.altitude = AltitudeStamped()

        # Create the publisher 

        self.altitude_pub = #TODO

        # Create the subscriber

        self.pressure_sub = #TODO

        rospy.spin()

    # Callback function to calculate the pressure altitude and publish the message

    def set_altitude(self, msg):

        # TODO update self.altitude message with the pressure altitude calculated

        # from the pressure in msg, and with current time stamp

        self.altitude.value = #TODO 

        self.altitude.stamp =  rospy.get_rostime()

        self.altitude_pub.publish(self.altitude)

if __name__ == '__main__':

    rospy.init_node('pressure_altitude_node')

    try:

        PressureAltitude()

    except rospy.ROSInterruptException:

        pass

Remember to add the newly created node to the launch file.

Observations:

1. The call to rospy.spin keeps the process alive until the application shuts down.

2. In the Python ROS API, the callback of each subscriber is executed in a separate thread of execution. In C++, the programmer may choose the threading model used by a node with the most common being single-threaded spinning in which the node has a main loop that first executes the work needed and then calls ros::spinOnce() to allow all the callbacks received up to that moment to execute.

ros::Rate r(10); // 10 hz

while (should_continue)

{

   ... do some work, publish some messages, etc. ...

  ros::spinOnce();

  r.sleep();

}

For more details, check https://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning

Expected RQT Graph

Below is a picture of an RQT graph with new nodes and topics highlighted.

Use the command below to display the altitude value given by the GPS sensor and the pressure altitude after you started the simulator and sourced the workspace. Are the two values displayed the same or different.

$ rqt_plot /uav/sensors/gps/pose/position/z /uav/sensors/pressure_altitude

Moving Average Filter and Fusion

The moving average of a series of values replaces each value in the series with the average of the previous n values, where n is a constant parameter called window size.

Our drone could use moving averages for both sensors to control their noise, so let's implement that.

Moving Average Class

First, we will implement the code for computing the moving average into a Python package that can be imported in the ROS nodes (not depicted in the picture above).

1. Create the directory structure

$ cd ~/fastsim/src/altitude/src/

2. Implement the class MovingAverage in the file named moving_average.py with the following signature:

# The MovingAverage implements the concept of window in a set of measurements.

# The window_size is the number of most recent measurements used in average.

# The measurements are continuously added using the method add.

# The get_average method must return the average of the last window_size measurements.

class MovingAverage():

    def __init__(self, window_size):

        #TODO 

        pass

    # add a new measurement

    def add(self, val):

        #TODO

        pass

    # return the average of the last window_size measurements added 

    # or the average of all measurements if less than window_size were provided

    def get_average(self):

        #TODO

        pass

3. Create the file test_moving_average.py in the directory ~/fastsim/src/altitude/src/ with the code below:

import unittest

from random import seed

from random import random

import numpy as np

from moving_average import MovingAverage

class TestMovingAverage(unittest.TestCase):

    def setUp(self):

        pass

    def test_empty_window(self):

        mw = MovingAverage(10)

        self.assertEqual(mw.get_average(), 0.0)

    def test_short_window(self):

        mw = MovingAverage(10)

        values = [random() for i in range(5)]

        for v in values:

            mw.add(v)

        self.assertTrue( abs(mw.get_average() - np.average(values) <= 0.0001 ) )

    def test_window(self):

        mw = MovingAverage(5)

        values = [random() for i in range(10)]

        for v in values:

            mw.add(v)

        self.assertTrue( abs(mw.get_average() - np.average(values[5:]) <= 0.0001 ) ) 

4. Run the test using the command below:

$ python -m unittest test_moving_average

>>> ----------------------------------------------------------------------

>>> Ran 3 tests in 0.000s

>>> OK

Now that we finished the code that implements the moving average, we will proceed to implement the two nodes that calculate the moving average for the values provided by the pressure and GPS altitude sensors.

Moving Average ROS Nodes for Pressure Sensor and GPS

Next, we need create the new nodes that use the moving average filter for the altitude values provided by the gps sensor and the pressure altitude sensor (created in checkpoint 1).

1. Create a new node in the file named gps_altitude_ma.py that uses the class MovingAverage to calculate the moving average of values received on the topic /uav/sensors/gps and publishes the moving average on the topic /uav/sensors/gps_altitude_ma.

The window size should be a ROS parameter, called gps_window_size, given in the launch file (see Lab 4). You may use the code below to start your implementation:

#!/usr/bin/env python

import rospy

import time

from geometry_msgs.msg import PoseStamped

from altitude.msg import AltitudeStamped

from moving_average import MovingAverage

class GPSAltitudeMovingAverage():

    # Node initialization

    def __init__(self):

        window_size = #TODO read parameter

        self.ma = MovingAverage(window_size)

        # Create the publisher and subscriber

        self.pub = rospy.Publisher('/uav/sensors/gps_altitude_ma',

                                   AltitudeStamped,

                                   queue_size=1)

        self.sub = rospy.Subscriber('/uav/sensors/gps',

                                    PoseStamped, self.process_altitude,

                                    queue_size = 1)

        rospy.spin()

    def process_altitude(self, msg):

        # add new value to the moving average list

        # use MovingAverage to calculate the new moving average

        # use the same timestamp as in msg

        # publish

if __name__ == '__main__':

    rospy.init_node('gps_altitude_ma_node')

    try:

        GPSAltitudeMovingAverage()

    except rospy.ROSInterruptException:

        pass

2. Create a new node in the file pressure_altitude_ma.py that uses the class MovingAverage to calculate the moving average of values received on the topic /uav/sensors/pressure_altitude and publishes on the topic /uav/sensors/pressure_altitude_ma.

The window size should be a ROS parameter, called pressure_window_size, given in the launch file (take a look at what we did in Lab 4 if you do not quite remember it).

#!/usr/bin/env python

import rospy

import time

from altitude.msg import AltitudeStamped

from moving_average import MovingAverage

class PressureAltitudeMovingAverage():

    # Node initialization

    def __init__(self):

        window_size = #TODO read param

        self.ma = MovingAverage(window_size)

        # Create the publisher and subscriber

        self.pub = rospy.Publisher('/uav/sensors/pressure_altitude_ma',

                                   AltitudeStamped,

                                   queue_size=1)

        self.sub = rospy.Subscriber('/uav/sensors/pressure_altitude',

                                    AltitudeStamped, self.process_altitude,

                                    queue_size = 1)

        rospy.spin()

    def process_altitude(self, msg):

        # add new value to the moving average list

        # use MovingAverage to calculate the new moving average

        # use the same timestamp as in msg

        # publish

if __name__ == '__main__':

    rospy.init_node('pressure_altitude_ma_node')

    try:

        PressureAltitudeMovingAverage()

    except rospy.ROSInterruptException:

        pass

Again, make sure to add the new nodes to the launch file.

3. Use the command below to display the values published by the nodes you just created and the unfiltered values. Remember to build and source the workspace first.

$ rqt_plot /uav/sensors/gps_altitude_ma /uav/sensors/gps/pose/position/z

$ rqt_plot /uav/sensors/pressure_altitude_ma /uav/sensors/pressure_altitude

Simple Fusion using Average

Create a new node in the file altitude_ma.py that subscribes to the topics /uav/sensors/gps_altitude_ma and /uav/sensors/pressure_altitude_ma and publishes the average of the two values on the topic /uav/sensors/altitude_ma.

You may use the code below as a template. You need to implement the following:

• in the method process_pressure_altitude update self.gps_altitude using the value from the message
• in the method process_gps_altitude update self.pressure_altitude using the value from the message
• in the method mainloop create a new message of type AltitudeStamped with the value of the average of the values of the two sensors and the last time stamp received as stamp

Explanation on the use of locks for thread synchronization: In the code above for the pressure_altitude_ma_node and gps_altitude_ma_node the message received on the topic was processed and the corresponding message published on one single thread, the thread for the particular topic subscribed, so no synchronization was necessary. In the code below, however, there are three threads: one for each subscriber and one for the mainloop. The three threads in the code below share three variables self.pressure_altitude, self.gps_altitude, and self.last_timestamp. Without proper synchronization, we could get some strange behaviors like having an updated value of one of the variables containing the altitude with an old timestamp.

We use a lock to help synchronize the threads and prevent race conditions (the lock is an instance of the type threading.Lock). The code ensures that only one thread may hold the lock at a time. If the lock is available then the acquire method returns and the lock is allocated to the thread on which the acquire method was called. The lock is released when the release method is called from the same thread. If the lock is held by a thread A and another thread B calls tries to acquire the same lock, then that thread A is blocked in the call until the lock is released. If the thread B never releases the lock, then the thread A is blocked at the acquire call.

#!/usr/bin/env python

import rospy

import time

from threading import Lock

from altitude.msg import AltitudeStamped

class AltitudeFusion():

    # Node initialization

    def __init__(self):

        self.pressure_altitude = 0

        self.gps_altitude = 0

        self.last_timestamp = None

        self.changed = False

        self.lock = Lock()        

        # Create the publisher and subscriber

        self.pub = rospy.Publisher('/uav/sensors/altitude_ma',

                                   AltitudeStamped,

                                   queue_size=1)

        self.pa_sub = rospy.Subscriber('/uav/sensors/pressure_altitude_ma',

                                    AltitudeStamped, self.process_pressure_altitude,

                                    queue_size = 1)

        self.pa_sub = rospy.Subscriber('/uav/sensors/gps_altitude_ma',

                                    AltitudeStamped, self.process_gps_altitude,

                                    queue_size = 1)

        self.mainloop()

    def process_pressure_altitude(self, msg):

        self.lock.acquire()

        #TODO

        self.changed = True

        self.lock.release()

    def process_gps_altitude(self, msg):

        self.lock.acquire()

        #TODO

        self.changed = True

        self.lock.release()

    # The main loop of the function

    def mainloop(self):

        # Set the rate of this loop

        rate = rospy.Rate(10)

        # While ROS is still running

        while not rospy.is_shutdown():

            avg_msg = None

            self.lock.acquire()

            if self.changed:

                #TODO initialize avg_msg

                self.changed = False

            self.lock.release()

            if avg_msg:

                self.pub.publish(avg_msg)

            # Sleep for the remainder of the loop

            rate.sleep()

if __name__ == '__main__':

    rospy.init_node('altitude_ma_node')

    try:

        AltitudeFusion()

    except rospy.ROSInterruptException:

        pass

Expected RQT Graph

Below is a picture of an RQT graph with new nodes and topics highlighted.

Checkpoint 2

1. Use rqt_plot to display the values on the following topics: /uav/sensors/gps/pose/position/z, /uav/sensors/gps_altitude_ma.

What happens when you set a very small value for the window size parameter? What about a very large one?

2. Use rqt_plot to display the values on the following topics: /uav/sensors/pressure_altitude, /uav/sensors/pressure_altitude_ma.

What is different in this plot compared with the previous one?

3. Use rqt_plot to display the values on the following topics: /uav/sensors/gps_altitude_ma, /uav/sensors/pressure_altitude_ma, /uav/sensors/altitude_ma.

How about the average of the the two moving averages compare with the unfiltered altitude provided by the GPS sensor and the pressure altitude sensor?

Congratulations, you are done with lab 5!

To Check

Checkpoint 1:

1. We use the find command to inspect what files were generated by the catkin build command for the custom message type. Execute the following command:

$ find . -name *AltitudeStamped*

What files were found? Open the Python file and inspect the code. Can you identify the type of the message, the attributes?

2. Start rqt_plot to visualize the two altitude measurements (GPS and pressure). Send a position to the drone at a different altitude using the code from Lab 4.

Do you see the change in altitude in the plots?

3. Are the two values calculated for the altitude equal or not? Why are they equal or differ?

Checkpoint 2:

1. Use rqt_plot to display the values on the following topics: /uav/sensors/gps/pose/position/z, /uav/sensors/gps_altitude_ma.

What happens when you set a very small value for the window size parameter? What about a very large one?

2. Use rqt_plot to display the values on the following topics: /uav/sensors/pressure_altitude, /uav/sensors/pressure_altitude_ma.

What is different in this plot compared with the previous one?

3. Use rqt_plot to display the values on the following topics: /uav/sensors/gps_altitude_ma, /uav/sensors/pressure_altitude_ma, /uav/sensors/altitude_ma.

How about the average of the the two moving averages compare with the unfiltered altitude provided by the GPS sensor and the pressure altitude sensor?

Extra

1. Revise your implementation of the moving average class to compute an exponential moving average. (optional)

2. Complete the Kalman Filter Lab (Lab 5 Hwk) for Hwk credit (2 points)