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