Lab 5 - Sensors and Filters
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
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?
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
updateself.gps_altitude
using the value from the message - in the method
process_gps_altitude
updateself.pressure_altitude
using the value from the message - in the method
mainloop
create a new message of typeAltitudeStamped
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)