Vehicle Researcher
6 years ago
parent
aa1b61eb8e
commit
e90c41c576
56 changed files with 884 additions and 1094 deletions
Binary file not shown.
@ -1,253 +0,0 @@ |
|||||||
# pylint: skip-file |
|
||||||
from __future__ import print_function |
|
||||||
import abc |
|
||||||
import numpy as np |
|
||||||
# The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use. |
|
||||||
# A subclass must implement: |
|
||||||
# 1) calc_transfer_fun(); see bottom of file for more info. |
|
||||||
# 2) __init__() to initialize self.state, self.covar, and self.process_noise appropriately |
|
||||||
|
|
||||||
# Alternatively, the existing implementations of EKF can be used (e.g. EKF2D) |
|
||||||
|
|
||||||
# Sensor classes are optionally used to pass measurement information into the EKF, to keep |
|
||||||
# sensor parameters and processing methods for a each sensor together. |
|
||||||
# Sensor classes have a read() method which takes raw sensor data and returns |
|
||||||
# a SensorReading object, which can be passed to the EKF update() method. |
|
||||||
|
|
||||||
# For usage, see run_ekf1d.py in selfdrive/new for a simple example. |
|
||||||
# ekf.predict(dt) should be called between update cycles with the time since it was last called. |
|
||||||
# Ideally, predict(dt) should be called at a relatively constant rate. |
|
||||||
# update() should be called once per sensor, and can be called multiple times between predict steps. |
|
||||||
# Access and set the state of the filter directly with ekf.state and ekf.covar. |
|
||||||
|
|
||||||
|
|
||||||
class SensorReading: |
|
||||||
# Given a perfect model and no noise, data = obs_model * state |
|
||||||
def __init__(self, data, covar, obs_model): |
|
||||||
self.data = data |
|
||||||
self.obs_model = obs_model |
|
||||||
self.covar = covar |
|
||||||
|
|
||||||
def __repr__(self): |
|
||||||
return "SensorReading(data={}, covar={}, obs_model={})".format( |
|
||||||
repr(self.data), repr(self.covar), repr(self.obs_model)) |
|
||||||
|
|
||||||
|
|
||||||
# A generic sensor class that does no pre-processing of data |
|
||||||
class SimpleSensor: |
|
||||||
# obs_model can be |
|
||||||
# a full observation model matrix, or |
|
||||||
# an integer or tuple of indices into ekf.state, indicating which variables are being directly observed |
|
||||||
# covar can be |
|
||||||
# a full covariance matrix |
|
||||||
# a float or tuple of individual covars for each component of the sensor reading |
|
||||||
# dims is the number of states in the EKF |
|
||||||
def __init__(self, obs_model, covar, dims): |
|
||||||
# Allow for integer covar/obs_model |
|
||||||
if not hasattr(obs_model, "__len__"): |
|
||||||
obs_model = (obs_model, ) |
|
||||||
if not hasattr(covar, "__len__"): |
|
||||||
covar = (covar, ) |
|
||||||
|
|
||||||
# Full observation model passed |
|
||||||
if dims in np.array(obs_model).shape: |
|
||||||
self.obs_model = np.asmatrix(obs_model) |
|
||||||
self.covar = np.asmatrix(covar) |
|
||||||
# Indices of unit observations passed |
|
||||||
else: |
|
||||||
self.obs_model = np.matlib.zeros((len(obs_model), dims)) |
|
||||||
self.obs_model[:, list(obs_model)] = np.identity(len(obs_model)) |
|
||||||
if np.asarray(covar).ndim == 2: |
|
||||||
self.covar = np.asmatrix(covar) |
|
||||||
elif len(covar) == len(obs_model): |
|
||||||
self.covar = np.matlib.diag(covar) |
|
||||||
else: |
|
||||||
self.covar = np.matlib.identity(len(obs_model)) * covar |
|
||||||
|
|
||||||
def read(self, data, covar=None): |
|
||||||
if covar: |
|
||||||
self.covar = covar |
|
||||||
return SensorReading(data, self.covar, self.obs_model) |
|
||||||
|
|
||||||
|
|
||||||
class EKF: |
|
||||||
__metaclass__ = abc.ABCMeta |
|
||||||
|
|
||||||
def __init__(self, debug=False): |
|
||||||
self.DEBUG = debug |
|
||||||
|
|
||||||
|
|
||||||
def __str__(self): |
|
||||||
return "EKF(state={}, covar={})".format(self.state, self.covar) |
|
||||||
|
|
||||||
# Measurement update |
|
||||||
# Reading should be a SensorReading object with data, covar, and obs_model attributes |
|
||||||
def update(self, reading): |
|
||||||
# Potential improvements: |
|
||||||
# deal with negative covars |
|
||||||
# add noise to really low covars to ensure stability |
|
||||||
# use mahalanobis distance to reject outliers |
|
||||||
# wrap angles after state updates and innovation |
|
||||||
|
|
||||||
# y = z - H*x |
|
||||||
innovation = reading.data - reading.obs_model * self.state |
|
||||||
|
|
||||||
if self.DEBUG: |
|
||||||
print("reading:\n",reading.data) |
|
||||||
print("innovation:\n",innovation) |
|
||||||
|
|
||||||
# S = H*P*H' + R |
|
||||||
innovation_covar = reading.obs_model * self.covar * reading.obs_model.T + reading.covar |
|
||||||
|
|
||||||
# K = P*H'*S^-1 |
|
||||||
kalman_gain = self.covar * reading.obs_model.T * np.linalg.inv( |
|
||||||
innovation_covar) |
|
||||||
|
|
||||||
if self.DEBUG: |
|
||||||
print("gain:\n", kalman_gain) |
|
||||||
print("innovation_covar:\n", innovation_covar) |
|
||||||
print("innovation: ", innovation) |
|
||||||
print("test: ", self.covar * reading.obs_model.T * ( |
|
||||||
reading.obs_model * self.covar * reading.obs_model.T + reading.covar * |
|
||||||
0).I) |
|
||||||
|
|
||||||
# x = x + K*y |
|
||||||
self.state += kalman_gain*innovation |
|
||||||
|
|
||||||
# print "covar", np.diag(self.covar) |
|
||||||
#self.state[(roll_vel, yaw_vel, pitch_vel),:] = reading.data |
|
||||||
|
|
||||||
# Standard form: P = (I - K*H)*P |
|
||||||
# self.covar = (self.identity - kalman_gain*reading.obs_model) * self.covar |
|
||||||
|
|
||||||
# Use the Joseph form for numerical stability: P = (I-K*H)*P*(I - K*H)' + K*R*K' |
|
||||||
aux_mtrx = (self.identity - kalman_gain * reading.obs_model) |
|
||||||
self.covar = aux_mtrx * self.covar * aux_mtrx.T + kalman_gain * reading.covar * kalman_gain.T |
|
||||||
|
|
||||||
if self.DEBUG: |
|
||||||
print("After update") |
|
||||||
print("state\n", self.state) |
|
||||||
print("covar:\n",self.covar) |
|
||||||
|
|
||||||
def update_scalar(self, reading): |
|
||||||
# like update but knowing that measurement is a scalar |
|
||||||
# this avoids matrix inversions and speeds up (surprisingly) drived.py a lot |
|
||||||
|
|
||||||
# innovation = reading.data - np.matmul(reading.obs_model, self.state) |
|
||||||
# innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar |
|
||||||
# kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar |
|
||||||
# self.state += np.matmul(kalman_gain, innovation) |
|
||||||
# aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model) |
|
||||||
# self.covar = np.matmul(aux_mtrx, np.matmul(self.covar, aux_mtrx.T)) + np.matmul(kalman_gain, np.matmul(reading.covar, kalman_gain.T)) |
|
||||||
|
|
||||||
# written without np.matmul |
|
||||||
es = np.einsum |
|
||||||
ABC_T = "ij,jk,lk->il" |
|
||||||
AB_T = "ij,kj->ik" |
|
||||||
AB = "ij,jk->ik" |
|
||||||
innovation = reading.data - es(AB, reading.obs_model, self.state) |
|
||||||
innovation_covar = es(ABC_T, reading.obs_model, self.covar, |
|
||||||
reading.obs_model) + reading.covar |
|
||||||
kalman_gain = es(AB_T, self.covar, reading.obs_model) / innovation_covar |
|
||||||
|
|
||||||
self.state += es(AB, kalman_gain, innovation) |
|
||||||
aux_mtrx = self.identity - es(AB, kalman_gain, reading.obs_model) |
|
||||||
self.covar = es(ABC_T, aux_mtrx, self.covar, aux_mtrx) + \ |
|
||||||
es(ABC_T, kalman_gain, reading.covar, kalman_gain) |
|
||||||
|
|
||||||
# Prediction update |
|
||||||
def predict(self, dt): |
|
||||||
es = np.einsum |
|
||||||
ABC_T = "ij,jk,lk->il" |
|
||||||
AB = "ij,jk->ik" |
|
||||||
|
|
||||||
# State update |
|
||||||
transfer_fun, transfer_fun_jacobian = self.calc_transfer_fun(dt) |
|
||||||
|
|
||||||
# self.state = np.matmul(transfer_fun, self.state) |
|
||||||
# self.covar = np.matmul(np.matmul(transfer_fun_jacobian, self.covar), transfer_fun_jacobian.T) + self.process_noise * dt |
|
||||||
|
|
||||||
# x = f(x, u), written in the form x = A(x, u)*x |
|
||||||
self.state = es(AB, transfer_fun, self.state) |
|
||||||
|
|
||||||
# P = J*P*J' + Q |
|
||||||
self.covar = es(ABC_T, transfer_fun_jacobian, self.covar, |
|
||||||
transfer_fun_jacobian) + self.process_noise * dt #!dt |
|
||||||
|
|
||||||
#! Clip covariance to avoid explosions |
|
||||||
self.covar = np.clip(self.covar,-1e10,1e10) |
|
||||||
|
|
||||||
@abc.abstractmethod |
|
||||||
def calc_transfer_fun(self, dt): |
|
||||||
"""Return a tuple with the transfer function and transfer function jacobian |
|
||||||
The transfer function and jacobian should both be a numpy matrix of size DIMSxDIMS |
|
||||||
|
|
||||||
The transfer function matrix A should satisfy the state-update equation |
|
||||||
x_(k+1) = A * x_k |
|
||||||
|
|
||||||
The jacobian J is the direct jacobian A*x_k. For linear systems J=A. |
|
||||||
|
|
||||||
Current implementations calculate A and J as functions of state. Control input |
|
||||||
can be added trivially by adding a control parameter to predict() and calc_tranfer_update(), |
|
||||||
and using it during calculation of A and J |
|
||||||
""" |
|
||||||
|
|
||||||
|
|
||||||
class FastEKF1D(EKF): |
|
||||||
"""Fast version of EKF for 1D problems with scalar readings.""" |
|
||||||
|
|
||||||
def __init__(self, dt, var_init, Q): |
|
||||||
super(FastEKF1D, self).__init__(False) |
|
||||||
self.state = [0, 0] |
|
||||||
self.covar = [var_init, var_init, 0] |
|
||||||
|
|
||||||
# Process Noise |
|
||||||
self.dtQ0 = dt * Q[0] |
|
||||||
self.dtQ1 = dt * Q[1] |
|
||||||
|
|
||||||
def update(self, reading): |
|
||||||
raise NotImplementedError |
|
||||||
|
|
||||||
def update_scalar(self, reading): |
|
||||||
# TODO(mgraczyk): Delete this for speed. |
|
||||||
# assert np.all(reading.obs_model == [1, 0]) |
|
||||||
|
|
||||||
rcov = reading.covar[0, 0] |
|
||||||
|
|
||||||
x = self.state |
|
||||||
S = self.covar |
|
||||||
|
|
||||||
innovation = reading.data - x[0] |
|
||||||
innovation_covar = S[0] + rcov |
|
||||||
|
|
||||||
k0 = S[0] / innovation_covar |
|
||||||
k1 = S[2] / innovation_covar |
|
||||||
|
|
||||||
x[0] += k0 * innovation |
|
||||||
x[1] += k1 * innovation |
|
||||||
|
|
||||||
mk = 1 - k0 |
|
||||||
S[1] += k1 * (k1 * (S[0] + rcov) - 2 * S[2]) |
|
||||||
S[2] = mk * (S[2] - k1 * S[0]) + rcov * k0 * k1 |
|
||||||
S[0] = mk * mk * S[0] + rcov * k0 * k0 |
|
||||||
|
|
||||||
def predict(self, dt): |
|
||||||
# State update |
|
||||||
x = self.state |
|
||||||
|
|
||||||
x[0] += dt * x[1] |
|
||||||
|
|
||||||
# P = J*P*J' + Q |
|
||||||
S = self.covar |
|
||||||
S[0] += dt * (2 * S[2] + dt * S[1]) + self.dtQ0 |
|
||||||
S[2] += dt * S[1] |
|
||||||
S[1] += self.dtQ1 |
|
||||||
|
|
||||||
# Clip covariance to avoid explosions |
|
||||||
S = max(-1e10, min(S, 1e10)) |
|
||||||
|
|
||||||
def calc_transfer_fun(self, dt): |
|
||||||
tf = np.identity(2) |
|
||||||
tf[0, 1] = dt |
|
||||||
tfj = tf |
|
||||||
return tf, tfj |
|
||||||
@ -1,116 +0,0 @@ |
|||||||
import numpy as np |
|
||||||
import numpy.matlib |
|
||||||
import unittest |
|
||||||
import timeit |
|
||||||
|
|
||||||
from common.kalman.ekf import EKF, SimpleSensor, FastEKF1D |
|
||||||
|
|
||||||
class TestEKF(EKF): |
|
||||||
def __init__(self, var_init, Q): |
|
||||||
super(TestEKF, self).__init__(False) |
|
||||||
self.identity = numpy.matlib.identity(2) |
|
||||||
self.state = numpy.matlib.zeros((2, 1)) |
|
||||||
self.covar = self.identity * var_init |
|
||||||
|
|
||||||
self.process_noise = numpy.matlib.diag(Q) |
|
||||||
|
|
||||||
def calc_transfer_fun(self, dt): |
|
||||||
tf = numpy.matlib.identity(2) |
|
||||||
tf[0, 1] = dt |
|
||||||
return tf, tf |
|
||||||
|
|
||||||
|
|
||||||
class EKFTest(unittest.TestCase): |
|
||||||
def test_update_scalar(self): |
|
||||||
ekf = TestEKF(1e3, [0.1, 1]) |
|
||||||
dt = 1. / 100 |
|
||||||
|
|
||||||
sensor = SimpleSensor(0, 1, 2) |
|
||||||
readings = map(sensor.read, np.arange(100, 300)) |
|
||||||
|
|
||||||
for reading in readings: |
|
||||||
ekf.update_scalar(reading) |
|
||||||
ekf.predict(dt) |
|
||||||
|
|
||||||
np.testing.assert_allclose(ekf.state, [[300], [100]], 1e-4) |
|
||||||
np.testing.assert_allclose( |
|
||||||
ekf.covar, |
|
||||||
np.asarray([[0.0563, 0.10278], [0.10278, 0.55779]]), |
|
||||||
atol=1e-4) |
|
||||||
|
|
||||||
def test_unbiased(self): |
|
||||||
ekf = TestEKF(1e3, [0., 0.]) |
|
||||||
dt = np.float64(1. / 100) |
|
||||||
|
|
||||||
sensor = SimpleSensor(0, 1, 2) |
|
||||||
readings = map(sensor.read, np.arange(1000)) |
|
||||||
|
|
||||||
for reading in readings: |
|
||||||
ekf.update_scalar(reading) |
|
||||||
ekf.predict(dt) |
|
||||||
|
|
||||||
np.testing.assert_allclose(ekf.state, [[1000.], [100.]], 1e-4) |
|
||||||
|
|
||||||
|
|
||||||
class FastEKF1DTest(unittest.TestCase): |
|
||||||
def test_correctness(self): |
|
||||||
dt = 1. / 100 |
|
||||||
reading = SimpleSensor(0, 1, 2).read(100) |
|
||||||
|
|
||||||
ekf = TestEKF(1e3, [0.1, 1]) |
|
||||||
fast_ekf = FastEKF1D(dt, 1e3, [0.1, 1]) |
|
||||||
|
|
||||||
ekf.update_scalar(reading) |
|
||||||
fast_ekf.update_scalar(reading) |
|
||||||
self.assertAlmostEqual(ekf.state[0] , fast_ekf.state[0]) |
|
||||||
self.assertAlmostEqual(ekf.state[1] , fast_ekf.state[1]) |
|
||||||
self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0]) |
|
||||||
self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2]) |
|
||||||
self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1]) |
|
||||||
|
|
||||||
ekf.predict(dt) |
|
||||||
fast_ekf.predict(dt) |
|
||||||
self.assertAlmostEqual(ekf.state[0] , fast_ekf.state[0]) |
|
||||||
self.assertAlmostEqual(ekf.state[1] , fast_ekf.state[1]) |
|
||||||
self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0]) |
|
||||||
self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2]) |
|
||||||
self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1]) |
|
||||||
|
|
||||||
def test_speed(self): |
|
||||||
setup = """ |
|
||||||
import numpy as np |
|
||||||
from common.kalman.tests.test_ekf import TestEKF |
|
||||||
from common.kalman.ekf import SimpleSensor, FastEKF1D |
|
||||||
|
|
||||||
dt = 1. / 100 |
|
||||||
reading = SimpleSensor(0, 1, 2).read(100) |
|
||||||
|
|
||||||
var_init, Q = 1e3, [0.1, 1] |
|
||||||
ekf = TestEKF(var_init, Q) |
|
||||||
fast_ekf = FastEKF1D(dt, var_init, Q) |
|
||||||
""" |
|
||||||
|
|
||||||
timeit.timeit(""" |
|
||||||
ekf.update_scalar(reading) |
|
||||||
ekf.predict(dt) |
|
||||||
""", setup=setup, number=1000) |
|
||||||
|
|
||||||
ekf_speed = timeit.timeit(""" |
|
||||||
ekf.update_scalar(reading) |
|
||||||
ekf.predict(dt) |
|
||||||
""", setup=setup, number=20000) |
|
||||||
|
|
||||||
timeit.timeit(""" |
|
||||||
fast_ekf.update_scalar(reading) |
|
||||||
fast_ekf.predict(dt) |
|
||||||
""", setup=setup, number=1000) |
|
||||||
|
|
||||||
fast_ekf_speed = timeit.timeit(""" |
|
||||||
fast_ekf.update_scalar(reading) |
|
||||||
fast_ekf.predict(dt) |
|
||||||
""", setup=setup, number=20000) |
|
||||||
|
|
||||||
assert fast_ekf_speed < ekf_speed / 4 |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
unittest.main() |
|
||||||
Binary file not shown.
Binary file not shown.
@ -1 +1 @@ |
|||||||
#define COMMA_VERSION "0.6.1-release" |
#define COMMA_VERSION "0.6.2-release" |
||||||
|
|||||||
Loading…
Reference in new issue