In this homework you are tasked with collecting acceleration data from an IMU and then fitting that to a constrained route using a particle filter. Kalman filters are covered in section 14.4 of the text, and particle filtering is covered in section 14.5.3. There are also numerous references online if you need more details. Your particle filters will randomize both location and heading, and you will need to convert the relative motion of the IMU to the new heading of each particle (most easily accomplished by using a rotation matrix).
IMUs are used extensively in robotic systems, both for data collection and for control. When the TrailNet drone flew in the direction indicated by its trail following DNN, it was a system using an IMU that estimated its progress and adjusted its control signals in response to that feedback. IMUs vary greatly in quality, from a few dollars to thousands of dollars. See digikey for an example list.
Despite their ubiquity, IMU data is a poor substitute for real location information. The IMU accelerometer provides acceleration measurements, which must be double-integrated to create location estimates, and most accelerometers are extremely noisy. The gyroscopes drift over time, requiring online calibration. It is impossible to use an IMU without some type of filtering.
This homework is meant to illustrate a few things. First, the necessity of filtering data. Second, the difficulty of working with real world data. Below are filtered location estimated from IMU recordings after walking three identical paths. Notice that they are not consistent.
|
|
|
|
Kalman filters require tuning, meaning that estimates of your measurement noise and system (or process) noise should be adjusted based upon reality. It turns out that this is nontrivial, and some recordings are simply “more noisy” than others, despite being made from the same hardware. Depending upon the quality of the IMU in your device, you may find it easier to simply make multiple recordings rather than finding a way to optimally tune your filter. The variance of your data is an okay place to begin for noise estimates.
The particle filter will also require tuning. You will learn the struggle of working with real-world data, even if it should be simple, but these struggles can be overcome with properly tuned algorithms.
An example Kalman filter is provided, but you may also use an external library. A python program is preferable, but as long as your code runs on the CS department’s iLab machines any language is acceptable. You must write your own particle filtering code.
For the particle filter, there will be a fair amount of parameter tuning:
Recording Advice
The recording should come from an outside area where you can access a satellite map, or from an inside area where you have a blueprint. You may measure an area and then draw up your own blueprint.
Your recording doesn’t need to be long. The total distance travelled should be somewhere between 10 and 20 meters. Choose a region with some restrictions on your movement, such as planters blocking off movement through an open area, or the particle filtering won’t work.
We aren’t using the gyroscope (unless you want to give yourself extra work) so be sure to keep the orientation of your recording device (probably a phone) constant. If you move extremely slowly, the signal of your acceleration will be lost in the noise. Try to move at a consistent pace to reduce the noise in the accelerometer.
To make the recording easier to process, try to include a second or two of stillness at the beginning and end of the recording.
Data Preprocessing
Gravity will create a constant acceleration of 9.8m/s. Hold your device so that gravity is isolated on a single axis. It should be possible to estimate the orientation of your device by searching for a consistent gravity component over time, but you can also simply ignore it. If your device is not perfectly aligned, you can also attempt to subtract away the gravity component by getting the mean of the signal when you are not moving and subtracting it from your samples:
# Assume that you have times and accelerations in the accel_data array.
# Find gravity and other biases during a time period where you dont' move at the beginning and end of your traces.
bias = numpy.mean(numpy.concatenate((accel_data[:still_time,1:4], accel_data[-still_time:,1:4])), axis=0)
# Now when iterating through the data, you can remove gravity
for datum in (accel_data - bias):class KalmanFilter:
def __init__(self, dt, system_noise, measurement_noise):
# State vector/mean estimate holds x,y,z acceleration, velocity, and position
self.x = numpy.zeros((9, 1))
# Transition matrix
self.A = numpy.array([[1, 0, 0, 0, 0, 0, 0, 0, 0], # Acceleration comes from itself
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
[dt, 0, 0, 1, 0, 0, 0, 0, 0], # Velocity is the current estimate plus the time delta * accel
[0, dt, 0, 0, 1, 0, 0, 0, 0],
[0, 0, dt, 0, 0, 1, 0, 0, 0],
[dt**2/2, 0, 0, dt, 0, 0, 1, 0, 0], # x, y, z location are the current estimate + dt * velocity + dt^2 * accel/2
[0, dt**2/2, 0, 0, dt, 0, 0, 1, 0],
[0, 0, dt**2/2, 0, 0, dt, 0, 0, 1]])
# Our observation matrix is only the acceleration
self.C = numpy.array([[1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0]])
# Make an observation matrix that contains the velocity for 0 velocity updates.
self.zeroC = numpy.array([[1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0]])
# Make an observation matrix that contains the location for location updates.
self.fuseLocation = numpy.array([[1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1]])
# Initial uncertainty (Sigma, the covariance matrix)
# Notice that this is initialized to a diagonal matrix, but the
# variance update will project P by the transition matrix when
# we do this: self.C @ self.P @ self.C.T
self.P = numpy.eye(9) * 1
# System noise (sigma in the equations)
self.Q = numpy.eye(9) * system_noise
# Measurement noise (delta in the equations)
self.R = numpy.ones((3,1)) * measurement_noise
# Measurement noise used during a zero velocity update
self.jitter = numpy.ones((6, 1)) * 10e-5
# Measurement noise used during sensor fusion
self.fusion_jitter = numpy.ones((9, 1)) * 10e-5
def predict(self):
# Update mean and variance predictions using the transition matrix
self.x = self.A @ self.x
self.P = self.A @ self.P @ self.A.T + self.Q
def control(self, B, u):
# Call after predict to update with a control input
# If the control matrix exists we follow the full equation:
# \mu_{t|t-1} \triangleeq A_t\mu{t-1} + B_t u_t
self.x += B @ u
def _internal_update(self, C, y, measure_noise):
# Compare the prediction to the observed state
y_hat = C @ self.x
residual = y - y_hat
# Update the variance
S = C @ self.P @ C.T + measure_noise
# Kalman gain matrix
K = self.P @ C.T @ numpy.linalg.inv(S)
# Update from the mean and covariance. 18.31-18.32 in Murphy's book.
self.x = self.x + K @ residual
self.P = (numpy.eye(len(self.P)) - K @ C) @ self.P
def zero_velocity_update(self):
# As an alternative to the control input, we can also expand the
# observation to include velocity, and indicate that it is 0.
# This doesn't interrupt the statistics of the system, so it could be preferable.
y = numpy.zeros((6, 1))
self._internal_update(self.zeroC, y, self.jitter)
def fused_location_update(self, y_accel, y_location):
# As an alternative to the control input, we can also expand the
# observation to include location.
# This doesn't interrupt the statistics of the system, so it could be preferable.
y = numpy.concatenate((y_accel.reshape(-1, 1), self.x[3:6], y_location.reshape(-1, 1)), axis=0)
self._internal_update(self.fuseLocation, y, self.fusion_jitter)
def update(self, y):
"""
Arguments:
y: y is the observed state.
"""
y = y.reshape(-1, 1)
self._internal_update(self.C, y, self.R)Using the filter:
times = accel_data[still_time:-still_time,0]
accel_data = accel_data[still_time:-still_time,1:4]
kfilter = KalmanFilter(mean_dt, system_noise = process_noise, measurement_noise=still_var)
filtered_data = []
filtered_covariances = []
# Subtract the bias, which hopefully includes gravity
for datum in (accel_data - bias):
kfilter.predict()
kfilter.update(datum)
filtered_data.append(kfilter.x.flatten())
filtered_covariances.append(kfilter.P)
filtered_data = numpy.array(filtered_data)