def test_linear_2d_simplex():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = SimplexSigmaPoints(n=4)
kf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=dt,
fx=fx,
hx=hx,
points=points)
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 0.0001
mss = MerweScaledSigmaPoints(4, .1, 2., -1)
ukf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=dt,
fx=fx,
hx=hx,
points=mss)
ukf.x = np.array([-1., 1., -1., 1])
ukf.P *= 1
x = kf.x
zs = []
for i in range(20):
x = fx(x, dt)
z = np.array([x[0] + randn() * 0.1, x[2] + randn() * 0.1])
zs.append(z)
Ms, Ps = kf.batch_filter(zs)
smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dts=dt)
if DO_PLOT:
zs = np.asarray(zs)
plt.plot(Ms[:, 0])
plt.plot(smooth_x[:, 0], smooth_x[:, 2])
print(smooth_x)
def __init__(self):
self.cap = cv2.VideoCapture(CV2_CAPTURE_PATH, apiPreference=cv2.CAP_V4L2)
self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
self.cap.set(cv2.CAP_PROP_FPS, 60)
self.cap.set(cv2.CAP_PROP_AUTOFOCUS, 0)
self.cap.set(cv2.CAP_PROP_AUTO_WB, 0)
self.cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 0)
self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
with open('calibration/board_transform.npy', 'rb') as f:
self.board_transform = np.load(f)
with open('calibration/bot_transform.npy', 'rb') as f:
self.bot_transform = np.load(f)
self.bot_filter = UnscentedKalmanFilter(
dim_x=6, dim_z=4,
dt=None, fx=bot_filter_f, hx=bot_filter_h,
points=MerweScaledSigmaPoints(6, alpha=.1, beta=2., kappa=-1))
self.bot_filter.P = np.diag([5**2, 5**2, 0.1**2, 0.1**2, 1**2, 0.1**2])
self.bot_filter.Q = np.array([
[(5)**2, 0, 0, 0, 0, 0],
[0, (5)**2, 0, 0, 0, 0],
[0, 0, (0.1)**2, 0, 0, 0],
[0, 0, 0, (0.1)**2, 0, 0],
[0, 0, 0, 0, (200)**2, 0],
[0, 0, 0, 0, 0, (1)**2]], dtype=np.float32)
self.bot_filter.R = np.diag([5**2, 5**2, 0.1**2, 0.1**2])
self.bot_filter_last_update = None
self.dc = DataClient()
def __init__(self, vehicle):
super(LastVehicleUKF, self).__init__(vehicle)
#noises2 = self.vehicle.observation_noises.sigma**2
#for i in range(len(noises2)):
# if noises2[i] == 0.:
# noises2[i] = 1e-16
points = MerweScaledSigmaPoints(n=6, alpha=.1, beta=2., kappa=0.1)
self.kf = UnscentedKalmanFilter(6,
39,
self.delta_t,
fx=self.fx,
hx=self.hx,
points=points)
#self.Q_factor = 10
#self.Q_factor_power = 1
self.kf.Q = Q_discrete_white_noise(3,
dt=self.delta_t,
var=1e-2,
block_size=2)
#print(self.kf.R,'RRR')
self.kf.R *= max(np.median(self.vehicle.observation_noises.sigma**2),
1e-16)
self.kf.x = np.concatenate(
(self.vehicle.ground_truth, self.vehicle.ground_truth))
self.kf.x[0] += self.vehicle.safty_dist
self.kf.P *= max(self.vehicle.actuator_noise.sigma**2, 1e-16)
def get_ukf(R_std, Q_std, dt):
def f_(x, dt, v_angle):
v = v_angle[0]
rad = v_angle[1]
x[2] += rad * dt
x[2] %= 2 * np.pi
dist = (v * dt)
x[0] += np.cos(x[2]) * dist
x[1] += np.sin(x[2]) * dist
return x
def h_(x):
return x[:2]
sigmas = MerweScaledSigmaPoints(3, alpha=.1, beta=2., kappa=0.)
ukf = UnscentedKalmanFilter(dim_x=3,
dim_z=2,
fx=f_,
hx=h_,
dt=dt,
points=sigmas)
ukf.x = np.array([0., 0., 0.])
ukf.R *= R_std
ukf.Q *= Q_std
#ukf.Q = Q_discrete_white_noise(3, dt=dt, var=Q_std)
return ukf
def test_batch_missing_data():
""" batch filter should accept missing data with None in the measurements """
def fx(x, dt):
F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=dt,
fx=fx,
hx=hx,
points=points)
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 0.0001
zs = []
for i in range(20):
z = np.array([i + randn() * 0.1, i + randn() * 0.1])
zs.append(z)
zs[2] = None
Rs = [1] * len(zs)
Rs[2] = None
Ms, Ps = kf.batch_filter(zs)
def __init__(self, dt, state=np.array([0.0, 0.0, -2.0, 0.0, 0.0, 0.0])):
M = len(state)
points = JulierSigmaPoints(M)
def project(state, world_positions):
return project_plane_markers(world_positions,
state.reshape(2, 3)).reshape(-1)
self.kf = kf = UnscentedKalmanFilter(
dt=dt,
dim_x=M,
dim_z=1,
points=points,
#fx=lambda X, dt: predict_state(X.reshape(4, -1), dt).reshape(-1),
fx=lambda x, dt: x,
hx=project,
)
z_dim = 2 # This changes according to the measurement
kf.P = np.eye(M) * 0.3
kf.x = state.reshape(-1).copy() # Initial state guess
self.z_var = 0.05**2
#kf.R = z_var # Observation variance
dpos_var = 0.01**2 * dt
drot_var = np.radians(1.0)**2 * dt
#kf.Q = np.diag([0]*3 + [0]*3 + [dpos_var]*3 + [drot_var]*3)
kf.Q = np.diag([dpos_var] * 3 + [drot_var] * 3)
def __init__(self):
n_dim_state = 4 # x = (x, y, vx, vy)
def fx(x, dt):
# state transition function - predict next state based
# on constant velocity model x = vt + x_0
F = np.array(
[[1, 0, dt, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return x[0:2]
points = MerweScaledSigmaPoints(n_dim_state,
alpha=.1,
beta=2.,
kappa=-1)
self.kf = UnscentedKalmanFilter(n_dim_state, 2, step, hx, fx, points)
self.kf.x = np.array([0.0, 0.0, 0.0, 0.0])
self.kf.P *= 0.1
self.kf.R = np.diag([0.01**2, 0.01**2])
self.kf.Q = Q_discrete_white_noise(dim=2,
dt=step,
var=0.2**2,
block_size=2)
def test_saver_UKF():
def fx(x, dt):
F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=dt,
fx=fx,
hx=hx,
points=points)
z_std = 0.1
kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 1.
zs = [[i, i] for i in range(40)]
s = Saver(kf, skip_private=False, skip_callable=False, ignore=['z_mean'])
for z in zs:
kf.predict()
kf.update(z)
#print(kf.x, kf.log_likelihood, kf.P.diagonal())
s.save()
s.to_array()
def test_rts():
def fx(x, dt):
A = np.eye(3) + dt * np.array([[0, 1, 0], [0, 0, 0], [0, 0, 0]])
f = np.dot(A, x)
return f
def hx(x):
return [np.sqrt(x[0]**2 + x[2]**2)]
dt = 0.05
sp = JulierSigmaPoints(n=3, kappa=1.)
kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 100.
radar = RadarSim(dt)
t = np.arange(0, 20 + dt, dt)
n = len(t)
xs = np.zeros((n, 3))
random.seed(200)
rs = []
#xs = []
for i in range(len(t)):
r = radar.get_range()
#r = GetRadar(dt)
kf.predict()
kf.update(z=[r])
xs[i, :] = kf.x
rs.append(r)
kf.x = np.array([0., 90., 1100.])
kf.P = np.eye(3) * 100
M, P = kf.batch_filter(rs)
assert np.array_equal(M, xs), "Batch filter generated different output"
Qs = [kf.Q] * len(t)
M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs)
if DO_PLOT:
print(xs[:, 0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:, 0])
plt.plot(t, M2[:, 0], c='g')
plt.subplot(312)
plt.plot(t, xs[:, 1])
plt.plot(t, M2[:, 1], c='g')
plt.subplot(313)
plt.plot(t, xs[:, 2])
plt.plot(t, M2[:, 2], c='g')
def __init__(self, fx, hx):
self.sigmas = MerweScaledSigmaPoints(n=3, alpha=.3, beta=2., kappa=.1)
self.ukf = UnscentedKalmanFilter(dim_x=3,
dim_z=2,
dt=.02,
hx=hx,
fx=fx,
points=self.sigmas)
def initialize_ukf(self):
'''
Initialize the parameters of the Unscented Kalman Filter (UKF) that is
used to estimate the state of the drone.
'''
# Number of state variables being tracked
self.state_vector_dim = 12
# The state vector consists of the following column vector.
# Note that FilterPy initializes the state vector with zeros.
# [[x],
# [y],
# [z],
# [x_vel],
# [y_vel],
# [z_vel],
# [roll],
# [pitch],
# [yaw],
# [roll_vel],
# [pitch_vel],
# [yaw_vel]]
# Number of measurement variables that the drone receives
self.measurement_vector_dim = 7
# The measurement variables consist of the following column vector:
# [[x_vel],
# [y_vel],
# [yaw_vel],
# [slant_range],
# [roll],
# [pitch],
# [yaw]]
# Function to generate sigma points for the UKF
# TODO: Modify these sigma point parameters appropriately. Currently
# just guesses
sigma_points = MerweScaledSigmaPoints(n=self.state_vector_dim,
alpha=0.1,
beta=2.0,
kappa=(3.0 -
self.state_vector_dim))
#kappa=0.0)
# Create the UKF object
# Note that dt will get updated dynamically as sensor data comes in,
# as will the measurement function, since measurements come in at
# distinct rates. Setting compute_log_likelihood=False saves some
# computation.
self.ukf = UnscentedKalmanFilter(dim_x=self.state_vector_dim,
dim_z=self.measurement_vector_dim,
dt=1.0,
hx=self.measurement_function,
fx=self.state_transition_function,
points=sigma_points,
compute_log_likelihood=False)
#residual_x=self.residual_x_account_for_angles)
self.initialize_ukf_matrices()
def filter_pose(ukf, z, R, dt, model):
ukf = UnscentedKalmanFilter()
ukf.predict(dt=dt)
predicted_state = ukf.x
positions_relative_to_drone = model.features_positions - np.tile(
predicted_state[7:10], (model.number_of_features, 1))
positions_in_drone_space = quaternion.rotate_vectors(
predicted_state[0:4], positions_relative_to_drone)
def __init__(self, OctorotorParams):
super(OctorotorBaseEnv, self).__init__()
# Octorotor Params
self.octorotor = Octocopter(OctorotorParams)
self.state = self.octorotor.get_state()
self.xref = 5
self.yref = 0
self.xrefarr = pd.read_csv("./Paths/EpathX5.csv", header=None).iloc[:,
1]
self.yrefarr = pd.read_csv("./Paths/EpathY5.csv", header=None).iloc[:,
1]
self.allocation = ControlAllocation(OctorotorParams)
self.resistance = np.full(8, OctorotorParams["resistance"])
self.dt = OctorotorParams["dt"]
self.motor = OctorotorParams["motor"]
self.motorController = OctorotorParams["motorController"]
self.posc = OctorotorParams["positionController"]
self.attc = OctorotorParams["attitudeController"]
self.altc = OctorotorParams["altitudeController"]
self.OctorotorParams = OctorotorParams
self.omega = np.zeros(8)
self.step_count = 0
self.total_step_count = OctorotorParams["total_step_count"]
self.zref = 0
self.psiref = np.zeros(3)
self.reward_discount = OctorotorParams["reward_discount"]
# OpenAI Gym Params
# State vector
# state[0:2] pos
# state[3:5] vel
# state[6:8] angle
# state[9:11] angle vel
# poserrs+eulererrs
# above + state
# state
self.observation_space = spaces.Box(np.full(1,
-np.inf,
dtype="float32"),
np.full(1, np.inf,
dtype="float32"),
dtype="float32")
#U = [T, tau]
self.action_space = spaces.Box(np.array([0.1, 0.1, 0.1, 0.1]),
np.array([1, 1, 1, 1]),
dtype="float32")
self.viewer = None
# introduce filtering for parameter estimator
points = MerweScaledSigmaPoints(6, alpha=.1, beta=2, kappa=0)
self.kf = UnscentedKalmanFilter(dim_x=6,
dim_z=6,
dt=OctorotorParams["dt"],
fx=self.fx,
hx=hx,
points=points)
self.kf.x = np.zeros(6)
def test_radar():
def fx(x, dt):
A = np.eye(3) + dt * np.array([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])
return A.dot(x)
def hx(x):
return [np.sqrt(x[0]**2 + x[2]**2)]
dt = 0.05
sp = JulierSigmaPoints(n=3, kappa=0.)
kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp)
assert np.allclose(kf.x, kf.x_prior)
assert np.allclose(kf.P, kf.P_prior)
# test __repr__ doesn't crash
str(kf)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 100.
radar = RadarSim(dt)
t = np.arange(0, 20+dt, dt)
n = len(t)
xs = np.zeros((n, 3))
random.seed(200)
rs = []
for i in range(len(t)):
r = radar.get_range()
kf.predict()
kf.update(z=[r])
xs[i, :] = kf.x
rs.append(r)
# test mahalanobis
a = np.zeros(kf.y.shape)
maha = scipy_mahalanobis(a, kf.y, kf.SI)
assert kf.mahalanobis == approx(maha)
if DO_PLOT:
print(xs[:, 0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:, 0])
plt.subplot(312)
plt.plot(t, xs[:, 1])
plt.subplot(313)
plt.plot(t, xs[:, 2])
def reset(self):
"""
Reset our SIRD model.
"""
# Reset β, γ and μ to the values mentioned on Wikipedia (see https://bit.ly/2VMvb6h).
self.__beta = 0.4
self.__gamma = 0.035
self.__mu = 0.005
# Reset I, R and D to the data at day 0 or the values mentioned on Wikipedia (see https://bit.ly/2VMvb6h).
if self.__use_data:
self.__x = np.array([self.__data_i(0), self.__data_r(0), self.__data_d(0)])
self.__n = self.__population
else:
self.__x = np.array([3, 0, 0])
self.__n = 1000
# Reset our Unscented Kalman filter (if required). Note tat we use a dt value of 1 (day) and not the value of
# Model.__DELTA_T.
if self.__use_data:
points = MerweScaledSigmaPoints(Model.__N_FILTERED,
1e-3, # Alpha value (usually a small positive value like 1e-3).
2, # Beta value (a value of 2 is optimal for a Gaussian distribution).
0, # Kappa value (usually, either 0 or 3-n).
)
self.__ukf = UnscentedKalmanFilter(Model.__N_FILTERED, Model.__N_MEASURED, 1, self.__h, Model.__f, points)
self.__ukf.x = np.array([self.__data_i(0), self.__data_r(0), self.__data_d(0),
self.__beta, self.__gamma, self.__mu, self.__n])
# Reset our data (if requested).
if self.__use_data:
self.__data_s_values = np.array([self.__data_s(0)])
self.__data_i_values = np.array([self.__data_i(0)])
self.__data_r_values = np.array([self.__data_r(0)])
self.__data_d_values = np.array([self.__data_d(0)])
# Reset our predicted/estimated values.
self.__s_values = np.array([self.__s_value()])
self.__i_values = np.array([self.__i_value()])
self.__r_values = np.array([self.__r_value()])
self.__d_values = np.array([self.__d_value()])
# Reset our estimated SIRD model parameters.
self.__beta_values = np.array([self.__beta])
self.__gamma_values = np.array([self.__gamma])
self.__mu_values = np.array([self.__mu])
def __init__(self,initial_x,initial_y,initial_vx,initial_vy):
self.dt = 0.05
# create sigma points to use in the filter. This is standard for Gaussian processes
self.points = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1)
self.kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=self.dt, fx=fx, hx=hx, points=self.points)
self.kf.x = np.array([initial_x, 0, initial_y, 0]) # initial state
self.kf.P *= 0.1 # initial uncertainty
self.z_std = 0.1
self.kf.R = np.diag([self.z_std**2, self.z_std**2]) # 1 standard
self.kf.Q = Q_discrete_white_noise(dim=2, dt=self.dt, var=0.01**2, block_size=2)
def __init__(self, current_temp, dt):
"""Unscented kalman filter"""
self._interval = 0
self._last_update = time.time()
# sigmas = JulierSigmaPoints(n=2, kappa=0)
sigmas = MerweScaledSigmaPoints(n=2, alpha=0.001, beta=2, kappa=0)
self._kf_temp = UnscentedKalmanFilter(
dim_x=2, dim_z=1, dt=dt, hx=hx, fx=fx, points=sigmas
)
self._kf_temp.x = np.array([float(current_temp), 0.0])
self._kf_temp.P *= 0.2 # initial uncertainty
self.interval = dt
def test_vhartman():
"""
Code provided by vhartman on github #172
https://github.com/rlabbe/filterpy/issues/172
"""
def fx(x, dt):
# state transition function - predict next state based
# on constant velocity model x = vt + x_0
F = np.array([[1.]], dtype=np.float32)
return np.dot(F, x)
def hx(x):
# measurement function - convert state into a measurement
# where measurements are [x_pos, y_pos]
return np.array([x[0]])
dt = 1.0
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(1, alpha=1, beta=2., kappa=0.1)
kf = UnscentedKalmanFilter(dim_x=1,
dim_z=1,
dt=dt,
fx=fx,
hx=hx,
points=points)
kf.x = np.array([0.]) # initial state
kf.P = np.array([[1]]) # initial uncertainty
kf.R = np.diag([1]) # 1 standard
kf.Q = np.diag([1]) # 1 standard
ekf = ExtendedKalmanFilter(dim_x=1, dim_z=1)
ekf.F = np.array([[1]])
ekf.x = np.array([0.]) # initial state
ekf.P = np.array([[1]]) # initial uncertainty
ekf.R = np.diag([1]) # 1 standard
ekf.Q = np.diag([1]) # 1 standard
np.random.seed(0)
zs = [[np.random.randn()] for i in range(50)] # measurements
for z in zs:
kf.predict()
ekf.predict()
assert np.allclose(ekf.P, kf.P)
assert np.allclose(ekf.x, kf.x)
kf.update(z)
ekf.update(z, lambda x: np.array([[1]]), hx)
assert np.allclose(ekf.P, kf.P)
assert np.allclose(ekf.x, kf.x)
def __init__(self, bbox):
"""
Initialises a tracker using initial bounding box.
"""
def fx(x, dt):
# state transition function - predict next state based on constant velocity model x = x_0 + vt
F = np.array([[1, 0, 0, 0, dt, 0, 0], [0, 1, 0, 0, 0, dt, 0],
[0, 0, 1, 0, 0, 0, dt], [0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
# measurement function - convert state into a measurement where measurements are [u, v, s, r]
return np.array([x[0], x[1], x[2], x[3]])
dt = 0.1
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(7, alpha=.1, beta=2., kappa=-1)
#define constant velocity model
self.kf = UnscentedKalmanFilter(dim_x=7,
dim_z=4,
dt=dt,
fx=fx,
hx=hx,
points=points)
# Assign the initial value of the state
self.kf.x[:4] = convert_bbox_to_z(bbox)
# Covariance matrix (dim_x, dim_x), default I
self.kf.P[
4:,
4:] *= 1000. # Give high uncertainty to the unobservable initial velocities
self.kf.P *= 0.2
# Measuerment uncertainty (dim_z, dim_z), default I
self.kf.R *= 0.01
# Process noise (dim_x, dim_x), default I
self.kf.Q[-1, -1] *= 0.0001
self.kf.Q[4:, 4:] *= 0.0001
self.time_since_update = 0
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
self.history = []
self.hits = 0
self.hit_streak = 0
self.age = 0
def __init__(self, env, dim_x, dim_y, X_0, P, Q, R, mn):
# self.sigmas = JulierSigmaPoints(dim_x, alpha=.1, beta=2., kappa=1.)
self.sigmas = JulierSigmaPoints(dim_x, kappa=0.1)
self.env = env
self.measure_nums = mn
self.ukf = UnscentedKalmanFilter(dim_x=dim_x,
dim_z=dim_y,
dt=env.tau,
hx=self.measurement,
fx=UKF_model,
points=self.sigmas)
self.ukf.x = X_0[:, 0]
# print(self.ukf.x.shape)
self.ukf.P = np.copy(P)
self.ukf.R = np.copy(R)
self.ukf.Q = np.copy(Q)
def Unscentedfilter(zs): # Filter function
points = MerweScaledSigmaPoints(2, alpha=.1, beta=2., kappa=1)
ukf = UnscentedKalmanFilter(dim_x=2,
dim_z=1,
fx=fx,
hx=hx,
points=points,
dt=dt)
ukf.Q = array(([50, 0], [0, 50]))
ukf.R = 100
ukf.P = eye(2) * 2
mu, cov = ukf.batch_filter(zs)
x, _, _ = ukf.rts_smoother(mu, cov)
return x[:, 0]
def initialize_ukf(self):
"""
Initialize the parameters of the Unscented Kalman Filter (UKF) that is
used to estimate the state of the drone.
"""
# Number of state variables being tracked
self.state_vector_dim = 7
# The state vector consists of the following column vector.
# Note that FilterPy initializes the state vector with zeros.
# [[x],
# [y],
# [z],
# [x_vel],
# [y_vel],
# [z_vel],
# [yaw]]
# Number of measurement variables that the drone receives
self.measurement_vector_dim = 6
# The measurement variables consist of the following vector:
# [[slant_range],
# [x],
# [y],
# [x_vel],
# [y_vel],
# [yaw]]
# Object to generate sigma points for the UKF
sigma_points = MerweScaledSigmaPoints(n=self.state_vector_dim,
alpha=0.1,
beta=2.0,
kappa=(3.0 -
self.state_vector_dim))
# Create the UKF object
# Note that dt will get updated dynamically as sensor data comes in,
# as will the measurement function, since measurements come in at
# distinct rates. Setting compute_log_likelihood=False saves some
# computation.
self.ukf = UnscentedKalmanFilter(dim_x=self.state_vector_dim,
dim_z=self.measurement_vector_dim,
dt=1.0,
hx=self.measurement_function,
fx=self.state_transition_function,
points=sigma_points,
compute_log_likelihood=False)
self.initialize_ukf_matrices()
def _test_log_likelihood():
from filterpy.common import Saver
def fx(x, dt):
F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=dt,
fx=fx,
hx=hx,
points=points)
z_std = 0.1
kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=1.1**2, block_size=2)
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 1.
zs = [[i + randn() * z_std, i + randn() * z_std] for i in range(40)]
s = Saver(kf)
for z in zs:
kf.predict()
kf.update(z)
print(kf.x, kf.log_likelihood, kf.P.diagonal())
s.save()
# test mahalanobis
a = np.zeros(kf.y.shape)
maha = scipy_mahalanobis(a, kf.y, kf.SI)
assert kf.mahalanobis == approx(maha)
s.to_array()
if DO_PLOT:
plt.plot(s.x[:, 0], s.x[:, 2])
def test_ukf_ekf_comparison():
def fx(x, dt):
# state transition function - predict next state based
# on constant velocity model x = vt + x_0
F = np.array([[1.]], dtype=np.float32)
return np.dot(F, x)
def hx(x):
# measurement function - convert state into a measurement
# where measurements are [x_pos, y_pos]
return np.array([x[0]])
dt = 1.0
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(1, alpha=1, beta=2., kappa=0.1)
ukf = UnscentedKalmanFilter(dim_x=1,
dim_z=1,
dt=dt,
fx=fx,
hx=hx,
points=points)
ukf.x = np.array([0.]) # initial state
ukf.P = np.array([[1]]) # initial uncertainty
ukf.R = np.diag([1]) # 1 standard
ukf.Q = np.diag([1]) # 1 standard
ekf = ExtendedKalmanFilter(dim_x=1, dim_z=1)
ekf.F = np.array([[1]])
ekf.x = np.array([0.]) # initial state
ekf.P = np.array([[1]]) # initial uncertainty
ekf.R = np.diag([1]) # 1 standard
ekf.Q = np.diag([1]) # 1 standard
np.random.seed(0)
zs = [[np.random.randn()] for i in range(50)] # measurements
for z in zs:
ukf.predict()
ekf.predict()
assert np.allclose(ekf.P, ukf.P), 'ekf and ukf differ after prediction'
ukf.update(z)
ekf.update(z, lambda x: np.array([[1]]), hx)
assert np.allclose(ekf.P, ukf.P), 'ekf and ukf differ after update'
def iniciar_ukf(list_z):
dt = 1
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1)
kf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=dt,
fx=fx,
hx=hx,
points=points)
kf.x = np.array([1., 1., 1., 1]) # initial state
kf.P *= 0.2 # initial uncertainty
z_std = 0.1
kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01**2, block_size=2)
zs = list_z
x_predichas = []
y_predichas = []
x_estimadas = []
y_estimadas = []
for z in zs:
# Predicción
kf.predict()
xp = kf.x[0]
yp = kf.x[1]
x_predichas.append(xp)
y_predichas.append(yp)
print("PREDICCION: x:", xp, "y:", yp)
# Actualización
kf.update(z)
xe = kf.x[0]
ye = kf.x[1]
x_estimadas.append(xe)
y_estimadas.append(ye)
print("ESTIMADO: x:", xe, "y:", ye)
print("--------------------------------------")
plt.plot(x_predichas, y_predichas, linestyle="-", color='orange')
plt.plot(x_estimadas, y_estimadas, linestyle="-", color='b')
plt.show()
def smooth(data: ndarray, dt: float):
points = MerweScaledSigmaPoints(3, alpha=1e-3, beta=2.0, kappa=0)
noisy_kalman = UnscentedKalmanFilter(
dim_x=3,
dim_z=1,
dt=dt,
hx=state_to_measurement,
fx=state_transition,
points=points,
)
noisy_kalman.x = array([0, data[1], data[1] - data[0]], dtype="float32")
noisy_kalman.R *= 20**2 # sensor variance
noisy_kalman.P = diag([5**2, 5**2, 1**2]) # variance of the system
noisy_kalman.Q = Q_discrete_white_noise(3, dt=dt, var=0.05)
means, covariances = noisy_kalman.batch_filter(data)
means[:, 1][means[:, 1] < 0] = 0 # clip velocity
return means[:, 1]
def __init__(self, g_params, dim_z=3):
self.sigmas = MerweScaledSigmaPoints(n=2, alpha=.1, beta=10., kappa=0.)
self.ukf = UnscentedKalmanFilter(dim_x=2,
dim_z=dim_z,
dt=1.,
hx=self.hx,
fx=self.fx,
points=self.sigmas)
self.s1params_a = g_params[0]
self.s1params_b = g_params[1]
self.s1params_c = g_params[2]
self.s2params_a = g_params[3]
self.s2params_b = g_params[4]
self.s2params_c = g_params[5]
self.s3params_a = g_params[6]
self.s3params_b = g_params[7]
def plot_filtered(log_data):
def f(x, dt):
""" state transition function """
F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0,
1]])
return np.dot(F, x)
def h(x):
""" measurement function (state to measurement transform) """
return np.array(x[0], x[2])
dt = 1.
points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=-1)
ukf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
fx=f,
hx=h,
dt=dt,
points=points)
ukf.x = np.array([0., 0., 0., 0.])
ukf.R = np.diag([0.09, 0.09])
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.02)
ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.02)
uxs = []
zs = np.stack((log_data['longitude'], log_data['latitude']), axis=-1)
for z in zs:
ukf.predict()
ukf.update(z)
uxs.append(ukf.x.copy())
uxs = np.array(uxs)
plt.subplot(211)
plt.plot(log_data['longitude'], log_data['latitude'])
plt.subplot(212)
plt.plot(uxs[:, 0], uxs[:, 2])
plt.show()
def test_linear_2d_merwe():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=dt,
fx=fx,
hx=hx,
points=points)
kf.x = np.array([-1., 1, -1, 1])
kf.P *= 1.1
# test __repr__ doesn't crash
str(kf)
zs = [[i + randn() * 0.1, i + randn() * 0.1] for i in range(20)]
Ms, Ps = kf.batch_filter(zs)
smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dts=dt)
if DO_PLOT:
plt.figure()
zs = np.asarray(zs)
plt.plot(zs[:, 0], marker='+')
plt.plot(Ms[:, 0], c='b')
plt.plot(smooth_x[:, 0], smooth_x[:, 2], c='r')
print(smooth_x)
def create_ukf(
self, dt: float, hx: callable, fx: callable
) -> UnscentedKalmanFilter:
"""Create an unscented Kalman filter with state transition functions
`fx`, measurement function `hx` with timestep between measurements
estimated as `dt`.
Parameters
----------
dt : float
control timestep
hx : callable
function describing mapping between sensor inputs and measurements
fx : callable
function describing system. Can be non-linear.
Returns
-------
UnscentedKalmanFilter
UKF object with sensible defaults for sigma points.
"""
n = 4
points = sigma_points.MerweScaledSigmaPoints(
n,
alpha=1e-4,
beta=2,
kappa=3 - n,
)
kf = UnscentedKalmanFilter(
dim_x=n,
dim_z=n,
dt=dt,
hx=hx,
fx=fx,
points=points,
)
return kf
