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 filter(measurements):
dt = 0.1
# x = [x, x', x'' y, y', y'']
x = np.array([measurements[0][0], 0., 0., measurements[0][1], 0., 0.])
G = np.array([[0.19*(dt**2)],
[dt],
[1.],
[0.19*(dt**2)],
[dt],
[1.]])
Q = G*G.T*0.1**2
# Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
sigmas = MerweScaledSigmaPoints(n=6, alpha=1., beta=2., kappa=-3.)
bot_filter = UKF(dim_x=6, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=sigmas)
bot_filter.x = np.array([measurements[0][0], 0., 0, measurements[0][1], 0., 0.])
#bot_filter.F = F
bot_filter.H = np.array([[1., 0., 0., 1., 0., 0.]])
#bot_filter.Q = Q
bot_filter.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=1, var=0.0002)
bot_filter.Q[3:6, 3:6] = Q_discrete_white_noise(3, dt=1, var=0.0002)
bot_filter.P *= 500
bot_filter.R = np.diag([0.0001, 0.0001])
observable_meas = measurements[0:len(measurements)-60]
pos, cov = [], []
for z in observable_meas:
pos.append(bot_filter.x)
cov.append(bot_filter.P)
bot_filter.predict()
bot_filter.update(z)
for i in range(0,60):
bot_filter.predict()
pos.append(bot_filter.x)
return pos
def linear_filter(measurements):
dt = 1.0
# x = [x, x', y, y']
x = np.array([measurements[0][0], 0., measurements[0][1], 0.])
H = np.array([[1., 0., 1., 0.]])
# Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
sigmas = MerweScaledSigmaPoints(n=4, alpha=0.3, beta=2., kappa=-3.)
bot_filter = UKF(dim_x=4,
dim_z=2,
fx=f_linear,
hx=h_linear,
dt=dt,
points=sigmas)
bot_filter.x = np.array([measurements[0][0], 0., measurements[0][1], 0.])
#bot_filter.F = F
bot_filter.H = np.asarray(H)
#bot_filter.Q = Q
bot_filter.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.1)
bot_filter.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.1)
bot_filter.P *= 10
bot_filter.R = np.diag([0.0001, 0.0001])
observable_meas = measurements[0:len(measurements) - 60]
pos, cov = [], []
for z in observable_meas:
pos.append(bot_filter.x)
cov.append(bot_filter.P)
bot_filter.predict()
bot_filter.update(z)
for i in range(0, 60):
bot_filter.predict()
pos.append(bot_filter.x)
return pos
def filter(measurements):
dt = 0.1
# x = [x, x', x'' y, y', y'']
x = np.array([measurements[0][0], 0., 0., measurements[0][1], 0., 0.])
G = np.array([[0.19 * (dt**2)], [dt], [1.], [0.19 * (dt**2)], [dt], [1.]])
Q = G * G.T * 0.1**2
# Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
sigmas = MerweScaledSigmaPoints(n=6, alpha=1., beta=2., kappa=-3.)
bot_filter = UKF(dim_x=6, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=sigmas)
bot_filter.x = np.array(
[measurements[0][0], 0., 0, measurements[0][1], 0., 0.])
#bot_filter.F = F
bot_filter.H = np.array([[1., 0., 0., 1., 0., 0.]])
#bot_filter.Q = Q
bot_filter.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=1, var=0.0002)
bot_filter.Q[3:6, 3:6] = Q_discrete_white_noise(3, dt=1, var=0.0002)
bot_filter.P *= 500
bot_filter.R = np.diag([0.0001, 0.0001])
observable_meas = measurements[0:len(measurements) - 60]
pos, cov = [], []
for z in observable_meas:
pos.append(bot_filter.x)
cov.append(bot_filter.P)
bot_filter.predict()
bot_filter.update(z)
for i in range(0, 60):
bot_filter.predict()
pos.append(bot_filter.x)
return pos
def linear_filter(measurements):
dt = 1.0
# x = [x, x', y, y']
x = np.array([measurements[0][0], 0., measurements[0][1], 0.])
H = np.array([[1., 0., 1., 0.]])
# Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
sigmas = MerweScaledSigmaPoints(n=4, alpha=0.3, beta=2., kappa=-3.)
bot_filter = UKF(dim_x=4, dim_z=2, fx=f_linear, hx=h_linear, dt=dt, points=sigmas)
bot_filter.x = np.array([measurements[0][0], 0., measurements[0][1], 0.])
#bot_filter.F = F
bot_filter.H = np.asarray(H)
#bot_filter.Q = Q
bot_filter.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.1)
bot_filter.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.1)
bot_filter.P *= 10
bot_filter.R = np.diag([0.0001, 0.0001])
observable_meas = measurements[0:len(measurements)-60]
pos, cov = [], []
for z in observable_meas:
pos.append(bot_filter.x)
cov.append(bot_filter.P)
bot_filter.predict()
bot_filter.update(z)
for i in range(0,60):
bot_filter.predict()
pos.append(bot_filter.x)
return pos
