def build_ukf(x, P, std_r, std_b, dt=1.0):
'''
Build UKF.
x: initial state.
P: initial covariance matrix.
std_r: standard var. of laser measurement.
std_b: standard var. of IMU measurement.
dt: time interval.
Plus some defined functions as parameters.
returns ukf.
'''
# Calculate sigma points.
points = MerweScaledSigmaPoints(n=6, alpha=0.001, beta=2, kappa=-3, subtract=residual_x)
ukf = UKF(dim_x=6, dim_z=4, fx=move, hx=Hx, \
dt=dt, points=points, x_mean_fn=state_mean, \
z_mean_fn=z_mean, residual_x=residual_x, residual_z=residual_z)
ukf.x = np.array(x)
ukf.P = P
ukf.R = np.diag([std_r ** 2, std_r ** 2, std_b ** 2, std_b ** 2])
q1 = Q_discrete_white_noise(dim=2, dt=dt, var=1.0)
q2 = Q_discrete_white_noise(dim=2, dt=dt, var=1.0)
q3 = Q_discrete_white_noise(dim=2, dt=dt, var=3.05 * pow(10, -4))
ukf.Q = block_diag(q1, q2, q3)
# ukf.Q = np.eye(3) * 0.0001
return ukf
def create_ukf(cmds,
landmarks,
sigma_vel,
sigma_steer,
sigma_range,
sigma_bearing,
ellipse_step=1,
step=10):
points = MerweScaledSigmaPoints(n=3,
alpha=0.03,
beta=2.,
kappa=0,
subtract=residual_x,
sqrt_method=sqrt_func)
ukf = UKF(dim_x=3,
dim_z=2 * len(landmarks),
fx=move,
hx=Hx,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([203.0, 1549.2, 1.34])
ukf.P = np.diag([100., 100., .5])
ukf.R = np.diag([sigma_range**2, sigma_bearing**2] * len(landmarks))
ukf.Q = np.diag([10.**2, 10.**2, 0.3**2])
return ukf
def build_ukf(x0=None, P0=None,
Q = None, R = None
):
# build ukf
if x0 is None:
x0 = np.zeros(6)
if P0 is None:
P0 = np.diag([1e-6,1e-6,1e-6, 1e-1, 1e-1, 1e-1])
if Q is None:
Q = np.diag([1e-4, 1e-4, 1e-2, 1e-1, 1e-1, 1e-1]) #xyhvw
if R is None:
R = np.diag([1e-1, 1e-1, 1e-1]) # xyh
#spts = MerweScaledSigmaPoints(6, 1e-3, 2, 3-6, subtract=ukf_residual)
spts = JulierSigmaPoints(6, 6-2, sqrt_method=np.linalg.cholesky, subtract=ukf_residual)
ukf = UKF(6, 3, (1.0 / 30.), # dt guess
ukf_hx, ukf_fx, spts,
x_mean_fn=ukf_mean,
z_mean_fn=ukf_mean,
residual_x=ukf_residual,
residual_z=ukf_residual)
ukf.x = x0.copy()
ukf.P = P0.copy()
ukf.Q = Q
ukf.R = R
return ukf
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 = UKF(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()
s.to_array()
plt.plot(s.x[:, 0], s.x[:, 2])
def main():
[t, dt, s, v, a, theta, omega, alpha] = build_real_values()
zs = build_measurement_values(t, [a, omega])
u = build_control_values(t, v)
[F, B, H, Q, R] = init_kalman(t, dt)
sigmas = MerweScaledSigmaPoints(n=9, alpha=.1, beta=2., kappa=-1)
kf = UKF(dim_x=9, dim_z=3, fx=f_bot, hx=h_bot, dt=0.2, points=sigmas)
kf.x = np.array([0., 0., 0., 0., 0., 0., 0., 0., 0.])
kf.R = R
kf.F = F
kf.H = H
kf.Q = Q
xs, cov = [], []
for zk, uk in zip(zs, u):
kf.predict(fx_args=uk)
kf.update(z=zk)
xs.append(kf.x.copy())
cov.append(kf.P)
xs, cov = np.array(xs), np.array(cov)
xground = construct_xground(s, v, a, theta, omega, alpha, xs.shape)
nees = NEES(xground, xs, cov)
print(np.mean(nees))
plot_results(t, xs, xground, zs, nees)
def unscented_kf(self, number=NUMBER):
global Time
P0 = np.diag([
3e-2, 3e-2, 3e-2, 3e-6, 3e-6, 3e-6, 3e-2, 3e-2, 3e-2, 3e-6, 3e-6,
3e-6
])
error = np.random.multivariate_normal(mean=np.zeros(12), cov=P0)
X0 = np.hstack((HPOP_1[0], HPOP_2[0])) + error
points = MerweScaledSigmaPoints(n=12, alpha=0.001, beta=2.0, kappa=-9)
ukf = UKF(dim_x=12,
dim_z=4,
fx=self.state_equation,
hx=self.measure_equation,
dt=STEP,
points=points)
ukf.x = X0
ukf.P = P0
ukf.R = Rk
ukf.Q = Qk
XF, XP = [X0], [X0]
print(error, "\n", Qk[0][0], "\n", Rk[0][0])
for i in range(1, number + 1):
ukf.predict()
Z = nav.measure_stk(i)
ukf.update(Z)
X_Up = ukf.x.copy()
XF.append(X_Up)
Time = Time + STEP
XF = np.array(XF)
return XF
def ukf_process(x, P, sigma_range, sigma_bearing, dt=1.0):
""" construct Unscented Kalman Filter with the initial state x
and the initial covaiance matrix P
sigma_range: the std of laser range sensors
sigma_bearing: the std of IMU
"""
# construct the sigma points
points = MerweScaledSigmaPoints(n=3,
alpha=0.001,
beta=2,
kappa=0,
subtract=residual)
# build the UKF based on previous functions
ukf = UKF(dim_x=3,
dim_z=3,
fx=move,
hx=Hx,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual,
residual_z=residual)
# assign the parameters of ukf
ukf.x = np.array(x)
ukf.P = P
ukf.R = np.diag([sigma_range**2, sigma_range**2, sigma_bearing**2])
ukf.Q = np.eye(3) * 0.0001
return ukf
def test_linear_1d():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array([[1., dt], [0, 1]])
return np.dot(F, x)
def hx(x):
return np.array([x[0]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1], [1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
z = np.array([2.])
kf.predict()
kf.update(z)
zs = []
for i in range(50):
z = np.array([i + randn() * 0.1])
zs.append(z)
kf.predict()
kf.update(z)
print('K', kf.K.T)
print('x', kf.x)
def run_localization(
cmds, landmarks, sigma_vel, sigma_steer, sigma_range,
sigma_bearing, ellipse_step=1, step=10):
plt.figure()
points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0,
subtract=residual_x)
ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=fx, hx=Hx,
dt=dt, points=points, x_mean_fn=state_mean,
z_mean_fn=z_mean, residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_range**2,
sigma_bearing**2]*len(landmarks))
ukf.Q = np.eye(3)*0.0001
sim_pos = ukf.x.copy()
# plot landmarks
if len(landmarks) > 0:
plt.scatter(landmarks[:, 0], landmarks[:, 1],
marker='s', s=60)
track = []
for i, u in enumerate(cmds):
sim_pos = move(sim_pos, u, dt/step, wheelbase)
track.append(sim_pos)
if i % step == 0:
ukf.predict(fx_args=u)
if i % ellipse_step == 0:
plot_covariance_ellipse(
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
facecolor='k', alpha=0.3)
x, y = sim_pos[0], sim_pos[1]
z = []
for lmark in landmarks:
dx, dy = lmark[0] - x, lmark[1] - y
d = sqrt(dx**2 + dy**2) + randn()*sigma_range
bearing = atan2(lmark[1] - y, lmark[0] - x)
a = (normalize_angle(bearing - sim_pos[2] +
randn()*sigma_bearing))
z.extend([d, a])
ukf.update(z, hx_args=(landmarks,))
if i % ellipse_step == 0:
plot_covariance_ellipse(
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
facecolor='g', alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:,1], color='k', lw=2)
plt.axis('equal')
plt.title("UKF Robot localization")
plt.show()
return ukf
def run_localization(
cmds, landmarks, sigma_vel, sigma_steer, sigma_range,
sigma_bearing, ellipse_step=1, step=10):
plt.figure()
points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0,
subtract=residual_x)
ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=fx, hx=Hx,
dt=dt, points=points, x_mean_fn=state_mean,
z_mean_fn=z_mean, residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_range**2,
sigma_bearing**2]*len(landmarks))
ukf.Q = np.eye(3)*0.0001
sim_pos = ukf.x.copy()
# plot landmarks
if len(landmarks) > 0:
plt.scatter(landmarks[:, 0], landmarks[:, 1],
marker='s', s=60)
track = []
for i, u in enumerate(cmds):
sim_pos = move(sim_pos, u, dt/step, wheelbase)
track.append(sim_pos)
if i % step == 0:
ukf.predict(fx_args=u)
if i % ellipse_step == 0:
plot_covariance_ellipse(
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
facecolor='k', alpha=0.3)
x, y = sim_pos[0], sim_pos[1]
z = []
for lmark in landmarks:
dx, dy = lmark[0] - x, lmark[1] - y
d = sqrt(dx**2 + dy**2) + randn()*sigma_range
bearing = atan2(lmark[1] - y, lmark[0] - x)
a = (normalize_angle(bearing - sim_pos[2] +
randn()*sigma_bearing))
z.extend([d, a])
ukf.update(z, hx_args=(landmarks,))
if i % ellipse_step == 0:
plot_covariance_ellipse(
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
facecolor='g', alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:,1], color='k', lw=2)
plt.axis('equal')
plt.title("UKF Robot localization")
plt.show()
return ukf
def myUKF(fx, hx, P, Q, R):
points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=-1.)
kf = UKF(4, 2, dt, fx=fx, hx=hx,
points=points) #(x_dimm, z_dimm,dt, hx, fx, sigmaPoints)
kf.P = P
kf.Q = Q
kf.R = R
kf.x = np.array([0., 90., 1100., 0.]) # initial gauss
return kf
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)):
n_state = len(Q)
n_meas = len(R)
sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=1.)
ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal, hx=h_kal,
dt=dt, points=sigmas)
ukf.Q = Q
ukf.R = R
self.ukf = ukf
self.isFirst = True
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)):
n_state = len(Q)
n_meas = len(R)
sigmas = SigmaPoints(n_state, alpha=.5, beta=2., kappa=0.)
ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_accel, hx=h_kal_accel,
dt=dt, points=sigmas, x_mean_fn = state_mean, residual_x=res_x,
residual_z=res_x)
ukf.Q = Q
ukf.R = R
self.ukf = ukf
self.isFirst = True
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 estimateUKF(camPoses):
points = MerweScaledSigmaPoints(3,.1,2.,0.)
filter = UKF(3,3,0,h, f, points, sqrt_fn=None, x_mean_fn=state_mean, z_mean_fn=state_mean, residual_x=residual, residual_z=residual)
filter.P = np.diag([0.04,0.04,0.003])
filter.Q = np.diag([(0.2)**2,(0.2)**2,(1*3.14/180)**2])
filter.R = np.diag([100,100,0.25])
Uposes = [[],[]]
for i in range(len(speed)):
x = filter.x
Uposes[0].append(x[0])
Uposes[1].append(x[1])
filter.predict(fx_args=[speed[i],angle[i]*0.01745])
filter.update(z = [camPoses[0][i],camPoses[1][i],camPoses[2][i]])
return Uposes
def create(self, pose, *args, **kwargs):
print 'CREATE!'
ukf = UKF(**self.ukf_args)
ukf._Q = self.Q.copy()
ukf.Q = self.Q.copy()
ukf.R = self.R.copy()
ukf.x = pose #np.zeros(5, dtype=np.float32)
#ukf.x[:3] = pose[:3]
ukf.P = self.P.copy()
# TODO : fill in more info, such as color(red/green/unknown), type(target/obs/unknown)
self.est[self.p_idx] = UKFEstimate(pose, *args, ukf=ukf, **kwargs)
self.p_idx += 1
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)):
n_state = len(Q)
n_meas = len(R)
sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=1.)
ukf = UKF(dim_x=n_state,
dim_z=n_meas,
fx=f_kal,
hx=h_kal,
dt=dt,
points=sigmas)
ukf.Q = Q
ukf.R = R
self.ukf = ukf
self.isFirst = True
def ukf_process(x, P, sigma_range, sigma_bearing, dt=1.0):
points = MerweScaledSigmaPoints(n=3, alpha=0.001, beta=2, kappa=0,
subtract=residual)
# build the UKF based on previous functions
ukf = UKF(dim_x=3, dim_z=3, fx=move, hx=Hx,
dt=dt, points=points, x_mean_fn=state_mean,
z_mean_fn=z_mean, residual_x=residual,
residual_z=residual)
# assign the parameters of ukf
ukf.x = np.array(x)
ukf.P = P
ukf.R = np.diag([sigma_range**2, sigma_range**2, sigma_bearing**2])
ukf.Q = np.eye(3)*0.0001
return ukf
def __init__(self, trueTrajectory, dt, route, Q=np.eye(2), R=np.eye(2)):
#from filterpy.kalman import KalmanFilter as KF
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints
n_state = len(Q)
n_meas = len(R)
sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=0.)
ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_v, hx=h_kal,
dt=dt, points=sigmas)
ukf.Q = Q
ukf.R = R
self.ukf = ukf
self.isFirst = True
self.route = route
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 test_1d():
def fx(x, dt):
F = np.array([[1., dt],
[0, 1]])
return np.dot(F, x)
def hx(x):
return np.array([[x[0]]])
ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)
ckf.x = np.array([[1.], [2.]])
ckf.P = np.array([[1, 1.1],
[1.1, 3]])
ckf.R = np.eye(1) * .05
ckf.Q = np.array([[0., 0], [0., .001]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1],
[1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
for i in range(50):
z = np.array([[i+randn()*0.1]])
#xx, pp, Sx = predict(f, x, P, Q)
#x, P = update(h, z, xx, pp, R)
ckf.predict()
ckf.update(z)
kf.predict()
kf.update(z[0])
assert abs(ckf.x[0] -kf.x[0]) < 1e-10
assert abs(ckf.x[1] -kf.x[1]) < 1e-10
plt.show()
def test_1d():
def fx(x, dt):
F = np.array([[1., dt],
[0, 1]])
return np.dot(F, x)
def hx(x):
return x[0:1]
ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)
ckf.x = np.array([[1.], [2.]])
ckf.P = np.array([[1, 1.1],
[1.1, 3]])
ckf.R = np.eye(1) * .05
ckf.Q = np.array([[0., 0], [0., .001]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1],
[1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
s = Saver(kf)
for i in range(50):
z = np.array([[i+randn()*0.1]])
ckf.predict()
ckf.update(z)
kf.predict()
kf.update(z[0])
assert abs(ckf.x[0] - kf.x[0]) < 1e-10
assert abs(ckf.x[1] - kf.x[1]) < 1e-10
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()
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)):
n_state = len(Q)
n_meas = len(R)
sigmas = SigmaPoints(n_state, alpha=.5, beta=2., kappa=0.)
ukf = UKF(dim_x=n_state,
dim_z=n_meas,
fx=f_kal_accel,
hx=h_kal_accel,
dt=dt,
points=sigmas,
x_mean_fn=state_mean,
residual_x=res_x,
residual_z=res_x)
ukf.Q = Q
ukf.R = R
self.ukf = ukf
self.isFirst = True
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 test_1d():
def fx(x, dt):
F = np.array([[1., dt], [0, 1]])
return np.dot(F, x)
def hx(x):
return x[0:1]
ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)
ckf.x = np.array([[1.], [2.]])
ckf.P = np.array([[1, 1.1], [1.1, 3]])
ckf.R = np.eye(1) * .05
ckf.Q = np.array([[0., 0], [0., .001]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1], [1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
s = Saver(kf)
for i in range(50):
z = np.array([[i + randn() * 0.1]])
ckf.predict()
ckf.update(z)
kf.predict()
kf.update(z[0])
assert abs(ckf.x[0] - kf.x[0]) < 1e-10
assert abs(ckf.x[1] - kf.x[1]) < 1e-10
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()
def __init__(self, trueTrajectory, dt, route, Q=np.eye(2), R=np.eye(2)):
#from filterpy.kalman import KalmanFilter as KF
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints
n_state = len(Q)
n_meas = len(R)
sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=0.)
ukf = UKF(dim_x=n_state,
dim_z=n_meas,
fx=f_kal_v,
hx=h_kal,
dt=dt,
points=sigmas)
ukf.Q = Q
ukf.R = R
self.ukf = ukf
self.isFirst = True
self.route = route
def test_1d():
def fx(x, dt):
F = np.array([[1., dt], [0, 1]])
return np.dot(F, x)
def hx(x):
return x[0:1]
ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)
ckf.x = np.array([[1.], [2.]])
ckf.P = np.array([[1, 1.1], [1.1, 3]])
ckf.R = np.eye(1) * .05
ckf.Q = np.array([[0., 0], [0., .001]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1], [1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
s = Saver(kf)
for i in range(50):
z = np.array([[i + randn() * 0.1]])
#xx, pp, Sx = predict(f, x, P, Q)
#x, P = update(h, z, xx, pp, R)
ckf.predict()
ckf.update(z)
kf.predict()
kf.update(z[0])
assert abs(ckf.x[0] - kf.x[0]) < 1e-10
assert abs(ckf.x[1] - kf.x[1]) < 1e-10
s.save()
s.to_array()
def test_linear_1d():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array([[1., dt],
[0, 1]], dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1],
[1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
z = np.array([2.])
kf.predict()
kf.update(z)
zs = []
for i in range(50):
z = np.array([i+randn()*0.1])
zs.append(z)
kf.predict()
kf.update(z)
print('K', kf.K.T)
print('x', kf.x)
def estimateUKF(camPoses):
points = MerweScaledSigmaPoints(3, .1, 2., 0.)
filter = UKF(3,
3,
0,
h,
f,
points,
sqrt_fn=None,
x_mean_fn=state_mean,
z_mean_fn=state_mean,
residual_x=residual,
residual_z=residual)
filter.P = np.diag([0.04, 0.04, 0.003])
filter.Q = np.diag([(0.2)**2, (0.2)**2, (1 * 3.14 / 180)**2])
filter.R = np.diag([100, 100, 0.25])
Uposes = [[], []]
for i in range(len(speed)):
x = filter.x
Uposes[0].append(x[0])
Uposes[1].append(x[1])
filter.predict(fx_args=[speed[i], angle[i] * 0.01745])
filter.update(z=[camPoses[0][i], camPoses[1][i], camPoses[2][i]])
return Uposes
"""
px, py = landmark
dist = sqrt((px - x[0])**2 + (py - x[1])**2)
angle = atan2(py - x[1], px - x[0])
return array([dist, normalize_angle(angle - x[2])])
points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0)
#points = JulierSigmaPoints(n=3, kappa=3)
ukf= UKF(dim_x=3, dim_z=2, fx=fx, hx=Hx, dt=dt, points=points,
x_mean_fn=state_mean, z_mean_fn=z_mean,
residual_x=residual_x, residual_z=residual_h)
ukf.x = array([2, 6, .3])
ukf.P = np.diag([.1, .1, .2])
ukf.R = np.diag([sigma_r**2, sigma_h**2])
ukf.Q = None#np.eye(3)*.00000
u = array([1.1, 0.])
xp = ukf.x.copy()
plt.cla()
plt.scatter(m[:, 0], m[:, 1])
cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)]
cmds.extend([cmds[-1]]*50)
v = cmds[-1][0]
cmds.extend([[v, a] for a in np.linspace(0, np.radians(2), 15)])
dt = 20 # ms
points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2., kappa=1.)
ukf = UKF(testx.shape[0],
testy.shape[0],
dt,
fx=f_force,
hx=h_forceToNeural,
points=points) #(x_dimm, z_dimm,dt, hx, fx, sigmaPoints)
ukf.x = [0.05, 0, 0] # initial gauss
# Optimize : P
fstd = 0.5
ratestd = 0.01
yankstd = 0.001
P = np.diag([fstd**2, ratestd**2, yankstd**2])
ukf.P = P
ukf.Q = Q
ukf.R = R
predict, fvar = [], []
for t in np.arange(target.shape[0]):
ukf.predict()
ukf.update(testy[:, t])
fvar.append(ukf.P[0, 0])
predict.append(ukf.x[0]) #(f,f',f'')
criterion = nn.MSELoss()
mse_loss = criterion(torch.from_numpy(np.asarray(predict)),
torch.from_numpy(np.asarray(target)))
figname = plot_dir + "ukfResult"
fig, ax = plt.subplots(2, 1, figsize=(12, 6))
sigmas = JulierSigmaPoints(n=2, kappa=1)
def fx(x, dt):
xout = np.empty_like(x)
xout[0] = x[1] * dt + x[0]
xout[1] = x[1]
return xout
def hx(x):
return x[:1] # return position [x]
ukf = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=1., hx=hx, fx=fx, points=sigmas)
ukf.P *= 10
ukf.R *= .5
ukf.Q = Q_discrete_white_noise(2, dt=1., var=0.03)
while(1):
try:
dt = time() - dt
mb.time += dt
dt = time()
mb.telemetry()
ts.append(round(mb.time, 3))
gyros.append(mb.gyroz)
magxs.append(mb.magx)
magys.append(mb.magy)
def track_head(camera_matrix, markerss, world_markers):
M = camera_matrix
f_x, c_x = M[0, 0], M[0, -1]
f_y, c_y = M[1, 1], M[1, -1]
h = 1
w = 16 / 9
dt = 1 / 30 # TODO: Use the actual timestamps
pos = np.array([0.0, 0.0, -2.0])
rot = np.array([0.0, 0.0, 0.0])
dpos = np.array([0.0, 0.0, 0.0])
drot = np.array([0.0, 0.0, 0.0])
state = np.array([
pos,
rot,
#dpos, drot
])
state_shape = state.shape
M = np.prod(state_shape)
points = JulierSigmaPoints(M)
def project(state, world_positions):
return project_plane_markers(world_positions,
state.reshape(state_shape)).reshape(-1)
kf = UnscentedKalmanFilter(
dt=dt,
dim_x=M,
dim_z=len(world_markers) * 2,
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
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)
all_world_positions = []
for w in world_markers.values():
for v in w:
all_world_positions.append(v)
all_world_positions = np.array(all_world_positions)
for row in markerss:
markers = row['markers']
world_positions = []
screen_positions = []
for m in markers:
try:
world = world_markers[str(m['id'])]
except KeyError:
continue
if m['id_confidence'] < 0.7: continue
for w, s in zip(world, m['verts']):
world_positions.append(w)
screen_positions.append(s)
world_positions = np.array(world_positions).reshape(-1, 2)
screen_positions = np.array(screen_positions).reshape(-1, 2)
screen_positions[:, 0] -= c_x
screen_positions[:, 0] /= f_x
screen_positions[:, 1] -= c_y
screen_positions[:, 1] /= -f_y
kf.predict()
if len(world_positions) >= 0:
measurement = screen_positions.reshape(-1)
kf.update(measurement,
R=np.diag([z_var] * len(measurement)),
world_positions=world_positions)
yield kf.x.copy(), kf.P.copy()
platform is stationary, this is a very difficult problem because there are
an infinite number of solutions. The literature is filled with this example,
along with proposed solutions (usually, platform makes manuevers).
"""
dt = 0.1
y = 20
platform_pos = (0, 20)
sf = SUKF(2, 1, dt, alpha=1.0e-4, beta=2.0, kappa=1.0)
sf.Q = Q_discrete_white_noise(2, dt, 0.1)
f = UKF(2, 1, dt, kappa=0.0)
f.Q = Q_discrete_white_noise(2, dt, 0.1)
def fx(x, dt):
""" state transition function"""
# pos = pos + vel
# vel = vel
return array([x[0] + x[1], x[1]])
def hx(x):
""" measurement function - convert position to bearing"""
return math.atan2(platform_pos[1], x[0] - platform_pos[0])
def pendulum(self):
messagebox.showinfo('Message title',
'End simulation by clicking Esc button')
# visulaization parameters
height = 600
width = 600
# simulation
center = None
# Pendulum parameters and variables
# initial conditions
theta = np.pi / 4 # the angle
omega = 0 # the angular velocity
# parametrs
g = 9.8 # m/s^2
L = .2 # m
m = 0.05 # kg
b = 0.001 # friction constant
# Numerical integration parameters
framerate = 60.0 # in frames per second
dt = 1.0 / framerate # Set dt to match the framerate of the webcam or animation
t = time.clock()
# Drawing parametres
thickness = 3
# Noise parameters
Sigma = 30 * np.array([[1, 0], [0, 1]])
#Kalman inferred state variables
theta_kf = theta
omega_kf = omega
#theta_kf_old = theta_kf
#Keep looping
# Create background image
frame = np.zeros((height, width, 3), np.uint8)
center_old = (300, 300)
center_noisy_old = (300, 300)
center_kf_old = (300, 300)
L_kf = 200
# Create background image
frame = np.zeros((height, width, 3), np.uint8)
cv2.circle(frame, (300, 300), 10, (0, 255, 255), -1)
##################
# ukf functions #
##################
#function to return the nonlinear state transition vatiables (theta, omega)
def fx(X, dt):
theta = X[0]
omega = X[1]
theta = theta + omega * dt
omega = omega - dt * g / L_kf * np.sin(theta) - dt * b / (
m * L_kf * L_kf) * omega
return np.c_[theta, omega]
# The update step converts the sigmas into measurement space via the h(x) ,return theta and omega function[https://share.cocalc.com/share/7557a5ac1c870f1ec8f01271959b16b49df9d087/Kalman-and-Bayesian-Filters-in-Python/10-Unscented-Kalman-Filter.ipynb?viewer=share]
def hx(X):
return X
points = MerweScaledSigmaPoints(2, alpha=1e-3, beta=2., kappa=4)
#points = JulierSigmaPoints(n=2, kappa=1)
kf = UKF(dim_x=2, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([theta_kf, omega_kf]) # initial state
kf.R = Sigma # a measurement noise matrix
kf.Q = np.diag(
[4, 4]) # process noise the smae shape as the state variables 2X2
######################
# end ukf functions #
######################
global readings_noisy, readings_after_ukf, theta_theoritical
readings_noisy = []
readings_after_ukf = []
theta_theoritical = []
while True:
cv2.circle(frame, (300, 300), 10, (0, 255, 255), -1)
# == Simulation model ==
# Update state
theta = theta + dt * omega
theta_theoritical.append(theta)
omega = omega - dt * g / L * np.sin(theta) - dt * b / (m * L *
L) * omega
# Map the state to a nearby pixel location
center = np.array((int(300 + 200 * np.sin(theta)),
int(300 + 200 * np.cos(theta))))
center_noisy = tuple(
center + np.matmul(Sigma, np.random.randn(2)).astype(int))
# Draw the pendulum
cv2.circle(frame, tuple(center_old), 10, (0, 0, 0), -1)
cv2.circle(frame, center_noisy_old, 10, (0, 0, 0), -1)
cv2.circle(frame, tuple(center), 10, (0, 255, 255), -1)
cv2.circle(frame, center_noisy, 10, (0, 0, 255), -1)
center_old = center
center_noisy_old = center_noisy
####################################################################
#### here starts the unscented kalman filter implementation #
####################################################################
center_noisy_kf = center_noisy
readings_noisy.append(
np.arctan(
(center_noisy_kf[0] - 300) / (center_noisy_kf[1] - 300)))
print('theoritical theta and omega', theta, omega)
#unscented kalman filter pridection
kf.predict()
#print('predicted theta and omega',kf.x[0],kf.x[1])
theta_kf = kf.x[0]
omega_kf = kf.x[1]
print('predicted theta and omega', theta_kf, omega_kf)
#center noisy kf update
# unscented kalman filter updating the state variables
kf.update([(np.arctan(
(center_noisy_kf[0] - 300) / (center_noisy_kf[1] - 300))), 0])
#print("after updtae",theta_kf)
#maping the updated theta to the nearest pixels
center_kf = np.array((int(300 + L_kf * np.sin(kf.x[0])),
int(300 + L_kf * np.cos(kf.x[0]))))
readings_after_ukf.append(
np.arctan((center_kf[0] - 300) / (center_kf[1] - 300)))
####################################################################
#### here finishes the unscented kalman filter implementation #
####################################################################
# Map the state to a nearby pixel location
cv2.circle(frame, tuple(center_kf_old), 10, (0, 0, 0), -1)
center_kf_old = center_kf
cv2.circle(frame, tuple(center_kf), 10, (255, 0, 255), -1)
# show the frame to our screen
cv2.imshow("Frame", frame)
key = cv2.waitKey(int(dt * 400)) & 0xFF
#if the 'Esc' key is pressed, stop the loop
if key == 27:
break
# Wait with calculating next animation step to match the intended framerate
t_ready = time.clock()
d_t_animation = t + dt - t_ready
t += dt
if d_t_animation > 0:
time.sleep(d_t_animation)
# close all windows
cv2.destroyAllWindows()
def residual(a, b):
y = a - b
y[2] = normalize_angle(y[2])
return y
def f(x,dt,u):
"""Estimate the non-linear state of the system"""
#print ((u[0]/self.L)*math.tan(u[1]))
return np.array([x[0]+u[0]*math.cos(x[2]),
x[1]+u[0]*math.sin(x[2]),
x[2]+((u[0]/1)*math.tan(u[1]))])
def h(z):
return z
points = MerweScaledSigmaPoints(3,.001,2.,0.)
filter = UKF(3,3,0,h, f, points, sqrt_fn=None, x_mean_fn=state_mean, z_mean_fn=state_mean, residual_x=residual, residual_z=residual)
filter.P = np.diag([0.04,0.04,1])
filter.Q = np.diag([(0.2)**2,(0.2)**2,(1*3.14/180)**2])
filter.R = np.diag([100,100,0.25])
robot = vehicle.Vehicle(1,50) #create a robot
robot.setOdometry(True) #configure its odometer
robot.setOdometryVariance([0.2,1])
speed,angle = [],[]
for a in xrange(4): #create a retangular path
for i in xrange(400):
angle.append(0)
for i in xrange(9):
angle.append(10)
for i in xrange(len(angle)): #set the speed to a constant along the path
speed.append(1)
robot.sim_Path(speed,angle) #run in a rectangular path
def test_circle():
from filterpy.kalman import KalmanFilter
from math import radians
def hx(x):
radius = x[0]
angle = x[1]
x = cos(radians(angle)) * radius
y = sin(radians(angle)) * radius
return np.array([x, y])
def fx(x, dt):
return np.array([x[0], x[1] + x[2], x[2]])
std_noise = .1
sp = JulierSigmaPoints(n=3, kappa=0.)
f = UKF(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, points=sp)
f.x = np.array([50., 90., 0])
f.P *= 100
f.R = np.eye(2) * (std_noise**2)
f.Q = np.eye(3) * .001
f.Q[0, 0] = 0
f.Q[2, 2] = 0
kf = KalmanFilter(dim_x=6, dim_z=2)
kf.x = np.array([50., 0., 0, 0, .0, 0.])
F = np.array([[1., 1., .5, 0., 0., 0.], [0., 1., 1., 0., 0., 0.],
[0., 0., 1., 0., 0., 0.], [0., 0., 0., 1., 1., .5],
[0., 0., 0., 0., 1., 1.], [0., 0., 0., 0., 0., 1.]])
kf.F = F
kf.P *= 100
kf.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
kf.R = f.R
kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)
kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001)
measurements = []
results = []
zs = []
kfxs = []
for t in range(0, 12000):
a = t / 30 + 90
x = cos(radians(a)) * 50. + randn() * std_noise
y = sin(radians(a)) * 50. + randn() * std_noise
# create measurement = t plus white noise
z = np.array([x, y])
zs.append(z)
f.predict()
f.update(z)
kf.predict()
kf.update(z)
# save data
results.append(hx(f.x))
kfxs.append(kf.x)
#print(f.x)
results = np.asarray(results)
zs = np.asarray(zs)
kfxs = np.asarray(kfxs)
print(results)
if DO_PLOT:
plt.plot(zs[:, 0], zs[:, 1], c='r', label='z')
plt.plot(results[:, 0], results[:, 1], c='k', label='UKF')
plt.plot(kfxs[:, 0], kfxs[:, 3], c='g', label='KF')
plt.legend(loc='best')
plt.axis('equal')
left_reader = lr.LogReader(l_logname,l_first_data_time_ms) right_reader = lr.LogReader(r_logname,r_first_data_time_ms) track = tr.TrackingReader(track_fname,right_reader,track_data_log_offset,F,30,1,vid_fname=vid_fname) pos_init = left_reader.get_ekf_loc_1d(START_TIME) vel_init = left_reader.get_ekf_vel(START_TIME) att_init = np.deg2rad(left_reader.get_ekf_att(START_TIME)) att_vel_init = np.zeros(3) state_init = np.hstack((pos_init,vel_init,att_init,att_vel_init)) ukf = UKF(dim_x=12, dim_z=15, dt=1.0/30, fx=fx, hx=hx) ukf.P = np.diag([5,5,2, 2,2,2, .017,.017,.017, .1,.1,.1]) ukf.x = state_init ukf.Q = np.diag([.5,.5,.5, .5,.5,.5, .1,.1,.1, .1,.1,.1]) T = np.array([16.878, -7.1368, 0]) #Translation vector joining two inertial frames time = np.arange(START_TIME,END_TIME,dt) print time.shape,time[0],time[-1] d = np.linspace(20,40,time.shape[0]) zs = np.zeros((time.shape[0],15)) Rs = np.zeros((time.shape[0],15,15)) means = np.zeros((time.shape[0],12)) covs = np.zeros((time.shape[0],12,12)) cam_locs = np.zeros((time.shape[0],3)) ekf_locs = np.zeros((time.shape[0],3)) for i in range(1,time.shape[0]): t = time[i]
def test_linear_rts():
""" for a linear model the Kalman filter and UKF should produce nearly
identical results.
Test code mostly due to user gboehl as reported in GitHub issue #97, though
I converted it from an AR(1) process to constant velocity kinematic
model.
"""
dt = 1.0
F = np.array([[1., dt], [.0, 1]])
H = np.array([[1., .0]])
def t_func(x, dt):
F = np.array([[1., dt], [.0, 1]])
return np.dot(F, x)
def o_func(x):
return np.dot(H, x)
sig_t = .1 # peocess
sig_o = .00000001 # measurement
N = 50
X_true, X_obs = [], []
for i in range(N):
X_true.append([i + 1, 1.])
X_obs.append(i + 1 + np.random.normal(scale=sig_o))
X_true = np.array(X_true)
X_obs = np.array(X_obs)
oc = np.ones((1, 1)) * sig_o**2
tc = np.zeros((2, 2))
tc[1, 1] = sig_t**2
tc = Q_discrete_white_noise(dim=2, dt=dt, var=sig_t**2)
points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2., kappa=1)
ukf = UKF(dim_x=2, dim_z=1, dt=dt, hx=o_func, fx=t_func, points=points)
ukf.x = np.array([0., 1.])
ukf.R = oc[:]
ukf.Q = tc[:]
s = Saver(ukf)
s.save()
s.to_array()
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([[0., 1]]).T
kf.R = oc[:]
kf.Q = tc[:]
kf.H = H[:]
kf.F = F[:]
mu_ukf, cov_ukf = ukf.batch_filter(X_obs)
x_ukf, _, _ = ukf.rts_smoother(mu_ukf, cov_ukf)
mu_kf, cov_kf, _, _ = kf.batch_filter(X_obs)
x_kf, _, _, _ = kf.rts_smoother(mu_kf, cov_kf)
# check results of filtering are correct
kfx = mu_kf[:, 0, 0]
ukfx = mu_ukf[:, 0]
kfxx = mu_kf[:, 1, 0]
ukfxx = mu_ukf[:, 1]
dx = kfx - ukfx
dxx = kfxx - ukfxx
# error in position should be smaller then error in velocity, hence
# atol is different for the two tests.
assert np.allclose(dx, 0, atol=1e-7)
assert np.allclose(dxx, 0, atol=1e-6)
# now ensure the RTS smoothers gave nearly identical results
kfx = x_kf[:, 0, 0]
ukfx = x_ukf[:, 0]
kfxx = x_kf[:, 1, 0]
ukfxx = x_ukf[:, 1]
dx = kfx - ukfx
dxx = kfxx - ukfxx
assert np.allclose(dx, 0, atol=1e-7)
assert np.allclose(dxx, 0, atol=1e-6)
return ukf
def test_circle():
from filterpy.kalman import KalmanFilter
from math import radians
def hx(x):
radius = x[0]
angle = x[1]
x = cos(radians(angle)) * radius
y = sin(radians(angle)) * radius
return np.array([x, y])
def fx(x, dt):
return np.array([x[0], x[1]+x[2], x[2]])
std_noise = .1
f = UKF(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, kappa=0)
f.x = np.array([50., 90., 0])
f.P *= 100
f.R = np.eye(2)*(std_noise**2)
f.Q = np.eye(3)*.001
f.Q[0,0]=0
f.Q[2,2]=0
kf = KalmanFilter(dim_x=6, dim_z=2)
kf.x = np.array([50., 0., 0, 0, .0, 0.])
F = np.array([[1., 1., .5, 0., 0., 0.],
[0., 1., 1., 0., 0., 0.],
[0., 0., 1., 0., 0., 0.],
[0., 0., 0., 1., 1., .5],
[0., 0., 0., 0., 1., 1.],
[0., 0., 0., 0., 0., 1.]])
kf.F = F
kf.P*= 100
kf.H = np.array([[1,0,0,0,0,0],
[0,0,0,1,0,0]])
kf.R = f.R
kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)
kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001)
measurements = []
results = []
zs = []
kfxs = []
for t in range (0,12000):
a = t / 30 + 90
x = cos(radians(a)) * 50.+ randn() * std_noise
y = sin(radians(a)) * 50. + randn() * std_noise
# create measurement = t plus white noise
z = np.array([x,y])
zs.append(z)
f.predict()
f.update(z)
kf.predict()
kf.update(z)
# save data
results.append (hx(f.x))
kfxs.append(kf.x)
#print(f.x)
results = np.asarray(results)
zs = np.asarray(zs)
kfxs = np.asarray(kfxs)
print(results)
if DO_PLOT:
plt.plot(zs[:,0], zs[:,1], c='r', label='z')
plt.plot(results[:,0], results[:,1], c='k', label='UKF')
plt.plot(kfxs[:,0], kfxs[:,3], c='g', label='KF')
plt.legend(loc='best')
plt.axis('equal')
"""
px = landmark[0]
py = landmark[1]
dist = np.sqrt((px - x[0])**2 + (py - x[1])**2)
Hx = array([dist, atan2(py - x[1], px - x[0]) - x[2]])
return Hx
points = MerweScaledSigmaPoints(n=3, alpha=1.e-3, beta=2, kappa=0)
ukf= UKF(dim_x=3, dim_z=2, fx=fx, hx=Hx, dt=dt, points=points,
x_mean_fn=state_mean, z_mean_fn=z_mean,
residual_x=residual_x, residual_z=residual_h)
ukf.x = array([2, 6, .3])
ukf.P = np.diag([.1, .1, .2])
ukf.R = np.diag([sigma_r**2, sigma_h**2])
ukf.Q = np.zeros((3,3))
u = array([1.1, .01])
xp = ukf.x.copy()
plt.figure()
plt.scatter(m[:, 0], m[:, 1])
for i in range(200):
xp = move(xp, u, dt/10., wheelbase) # simulate robot
plt.plot(xp[0], xp[1], ',', color='g')
if i % 10 == 0:
ukf.predict(fx_args=u)
def ukf_filter_without_register(x, filt_times, filepath, measurement_raw,
timestamp_data):
measurement = copy.deepcopy(measurement_raw)
# In reverse Order
if filt_times % 2 == 0:
measurement = measurement[::-1, :]
timestamp_data = timestamp_data[::-1, :]
for i in range(1, measurement.shape[0]):
measurement[measurement.shape[0] - i,
0:3] = -measurement[measurement.shape[0] - i - 1, 0:3]
# measurement[i, 0:3] = -measurement[i, 0:3]
dt = 0.1
points = MerweScaledSigmaPoints(11, alpha=.1, beta=2., kappa=-1)
ukf = UKF(dim_x=11, dim_z=3, dt=dt, fx=ukf_f, hx=ukf_h, points=points)
ukf.x = x[0:11] # initial state
x_global = x[11:19]
ukf.P *= 0.2 # initial uncertainty
ukf.R = 1e-5 * np.diag(np.asarray([1, 1, 1]))
if filt_times == 1:
ukf.P = 1e-4 * np.diag(
np.asarray([0.0001, 0.0001, 1, 1, 1, 1, 10, 10, 1, 1, 1]))
ukf.Q = 1e-8 * np.diag(
np.asarray([0.0001, 0.0001, 1, 1, 1, 1, 10, 10, 1, 1, 1]))
else:
ukf.P = 1e-4 * np.diag(
np.asarray([0.001, 0.001, 0.001, 1, 1, 1, 10, 10, 1, 1, 1]))
ukf.Q = 1e-8 * np.diag(
np.asarray([0.001, 0.001, 0.001, 1, 1, 1, 10, 10, 1, 1, 1]))
# shrink_list = [3, 5, 7]
# shrink_index = bisect(shrink_list, filt_times)
# ukf.P[6:8,:] /= (10**shrink_index)
# ukf.Q[6:8,:] /= (10**shrink_index)
# P[8:11,:] = P[8:11,:]/(10**shrink_index)
# Q[8:11,:] = Q[8:11,:]/(10**shrink_index)
# P[3:7] = P[3:7]/(10**shrink_index)
# Q[3:7] = Q[3:7]/(10**shrink_index)
x_log = np.zeros((measurement.shape[0], 19))
x_log[0, 0:11] = ukf.x
x_log[0, 11:19] = x_global
for i in range(measurement.shape[0] - 1):
dtime = abs(timestamp_data[i + 1, 1] - timestamp_data[i, 1])
ukf.predict(dt=dtime)
# z_measure[3:7] = target_quat_raw
z_measure = measurement[i + 1, 3:7]
ukf, x_global, _ = ukf_update(ukf, z_measure, x_global)
x_global[0:4] = x_global[0:4] / np.linalg.norm(x_global[0:4])
x_global[4:8] = x_global[4:8] / np.linalg.norm(x_global[4:8])
x_log[i + 1, 0:11] = ukf.x
x_log[i + 1, 11:19] = x_global
np.savetxt(f"{filepath}ukf_log_all_{filt_times}.csv", x_log, delimiter=',')
x[0:11] = ukf.x
x[11:19] = x_global
return x
sigma_r = .3
sigma_h = .1#radians(.5)#np.radians(1)
#sigma_steer = radians(10)
dt = 0.1
wheelbase = 0.5
points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0, subtract=residual_x)
#points = JulierSigmaPoints(n=3, kappa=3)
ukf= UKF(dim_x=3, dim_z=2*len(m), fx=fx, hx=Hx, dt=dt, points=points,
x_mean_fn=state_mean, z_mean_fn=z_mean,
residual_x=residual_x, residual_z=residual_h)
ukf.x = array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_r**2, sigma_h**2]* len(m))
ukf.Q =np.eye(3)*.00001
u = array([1.1, 0.])
xp = ukf.x.copy()
plt.cla()
plt.scatter(m[:, 0], m[:, 1])
cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)]
cmds.extend([cmds[-1]]*50)
v = cmds[-1][0]
cmds.extend([[v, a] for a in np.linspace(0, np.radians(2), 15)])
