def test_motion_model_circular_right_1(self):
'''
Move from origin (0,0,0) to the RIGHT
With v = 1, on a circle with R = 2.0 meters with dT = 1 seconds for 1 cycle.
x = Rsin(v*dT/R) = 0.47
y = Rcos(v*dT/R) = 0.94
y needs a shifting with R after that
'''
x = 0.0
y = 0.0
o = 0.0
v = 1.0
a = 0.0
s = 0.0 # R = 2
dT = 1
u = np.array([[a], [s]])
kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT)
R = get_icr_radius(s)
x = kf.motion_model(u)
self.assertLessEqual(kf.posx, R)
self.assertLessEqual(kf.posy, 0)
self.assertGreaterEqual(kf.posx, -R)
self.assertGreaterEqual(kf.posy, -2 * R)
if self.debug is True:
print(kf.x)
def __init__(self, t):
self.dt = t
self.R = np.diag([params.sigma_r**2, params.sigma_theta**2])
self.lm_filters = [
[EKF(params.dt, i) for i in range(params.num_lms)]
for i in range(params.M)
] #List of lists. Inside list is EKF for each LM. Outer list is each particle
def __init__(self):
self.icp = ICP()
self.ekf = EKF()
self.extraction = Extraction()
# odom robot init states
self.robot_x = rospy.get_param('/icp/robot_x',0)
self.robot_y = rospy.get_param('/icp/robot_y',0)
self.robot_theta = rospy.get_param('/icp/robot_theta',0)
self.sensor_sta = [self.robot_x,self.robot_y,self.robot_theta]
self.isFirstScan = True
self.src_pc = []
self.tar_pc = []
# State Vector [x y yaw]
self.xOdom = np.zeros((3,1))
self.xEst = np.zeros((3,1))
self.PEst = np.eye(3)
# map observation
self.obstacle = []
# radius
self.obstacle_r = 10
# init map
self.updateMap()
# ros topic
self.laser_sub = rospy.Subscriber('/course_agv/laser/scan',LaserScan,self.laserCallback)
self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3)
self.odom_pub = rospy.Publisher('icp_odom',Odometry,queue_size=3)
self.odom_broadcaster = tf.TransformBroadcaster()
self.landMark_pub = rospy.Publisher('/landMarks',MarkerArray,queue_size=1)
def test_motion_model_circular_left_4(self):
'''
Move from origin (0,0,0) to the LEFT
With v = 1, on a circle with R = 1.75 meters with dT = 4 seconds for 1 cycle.
'''
x = 0.0
y = 0.0
o = 0.0
v = 1.0
a = 0.0
s = 14.0 # R = 1.75
dT = 4
u = np.array([[a], [s]])
kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT)
R = get_icr_radius(s)
x = kf.motion_model(u)
self.assertLessEqual(kf.posx, R)
self.assertLessEqual(kf.posy, 2 * R)
self.assertGreaterEqual(kf.posx, -R)
self.assertGreaterEqual(kf.posy, 0)
if self.debug is True:
print(kf.x)
def test_motion_model_circular_loop_left_5times(self):
'''
Move from origin (0,0,0) to the LEFT
With v = 1, on a circle with R = 1.75 meters with dT = 1 seconds with 5 cycles
'''
x = 0.0
y = 0.0
o = 0.0
v = 1.0
a = 0.0
s = 14.0 # R = 1.75
u = np.array([[a], [s]])
dT = 1.0
kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT)
R = get_icr_radius(s)
for i in range(1, 6):
x = kf.motion_model(u)
# if self.debug is True:
# print(kf.x)
self.assertLessEqual(kf.posx, R)
self.assertLessEqual(kf.posy, 2 * R)
self.assertGreaterEqual(kf.posx, -R)
self.assertGreaterEqual(kf.posy, 0)
if self.debug is True:
print(kf.x)
def test_motion_model_circular_right_0(self):
'''
Move from origin (0,0,0) to the RIGHT
With v = PI, on a circle with R = 2.0 meters with dT = 1 seconds for 1 cycle.
x = [2, -2, pi, pi]
'''
x = 0.0
y = 0.0
o = 0.0
v = 0.0
a = 0.0
s = 0.0 # R = 2
dT = 1
u = np.array([[a], [s]])
R = get_icr_radius(s)
v = np.pi * R / 2
kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT)
x = kf.motion_model(u)
self.assertLessEqual(kf.posx, R)
self.assertLessEqual(kf.posy, 0)
self.assertGreaterEqual(kf.posx, -R)
self.assertGreaterEqual(kf.posy, -2 * R)
self.assertAlmostEqual(kf.posx, R, None, None, 0.2)
self.assertAlmostEqual(kf.posy, -R, None, None, 0.2)
if self.debug is True:
print(kf.x)
def __init__(self, acc_n=1e-2, gyr_n=1e-4, acc_w=1e-6, gyr_w=1e-8):
self.initialized = False
self.init_lla_ = None
self.I_p_Gps_ = np.array([.0, .0, .0])
self.imu_buff = deque([], maxlen=100)
self.ekf = EKF(acc_n, gyr_n, acc_w, gyr_w)
rospy.Subscriber("imu/data", Imu, self.imu_callback)
rospy.Subscriber("/fix", NavSatFix, self.gnss_callback)
self.pub_path = rospy.Publisher("nav_path", Path, queue_size=10)
self.pub_odom = rospy.Publisher("nav_odom", Odometry, queue_size=10)
self.nav_path = Path()
self.last_imu = None
self.p_G_Gps_ = None
def main():
global ekf, pose_pub
rospy.init_node("localization")
ekf = EKF()
pose_pub = rospy.Publisher("pose", Pose)
rospy.Subscriber("odom", Odometry, odom_callback)
# odom_combined comes from robot_pose_ekf, different message type from
# regular odom, but seems to have the same basic contents, so we can use
# the same callback (yay dynamic typing!)
#rospy.Subscriber("odom_combined", PoseWithCovarianceStamped, odom_callback)
#rospy.Subscriber("/corobot_pose_ekf/odom_semicombined", PoseWithCovarianceStamped, odom_callback)
#rospy.Subscriber("laser_pose", Pose, laser_callback)
rospy.Subscriber("qrcode_pose", Pose, qrcode_callback)
rospy.spin()
def test_init_ekf(self):
'''
Create an instance of the EKF class and initialize it.
'''
x = 0.0
y = 0.0
o = 0.0
v = 0.1
kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=1)
self.assertEqual(kf.posx, x)
self.assertEqual(kf.posy, y)
self.assertEqual(kf.yaw, o)
self.assertEqual(kf.vel, v)
def __init__(self, host='localhost', port=4567):
# Connection details
self._host = host
self._port = port
self.ekf = EKF()
# Server callbacks definition
self._sio = socketio.AsyncServer()
self._sio.on('connect', self._on_sim_connect)
self._sio.on('disconnect', self._on_sim_disconnect)
self._sio.on('telemetry', self._on_telemetry)
# History stacking
self.estimations_history = []
self.groundtruth_history = []
def main():
global pose_pub, ekf, particles, map, num_particles
rospy.init_node("localization")
rospy.wait_for_service('get_map')
map_srv = rospy.ServiceProxy('get_map', GetCoMap)
map = map_srv().map
roslib.load_manifest("corobot_localization")
ekf = EKF()
pose_pub = rospy.Publisher("pose", Pose)
num_particles = 500
particles = []
if len(particles) == 0:
rospy.Subscriber("qrcode_pose", Pose, pf_initialize)
if len(particles) > 0:
rospy.Subscriber("odom", Odometry, prediction)
rospy.Subscriber("scan", LaserScan, update_model)
rospy.spin()
def test_motion_model_straight(self):
'''
Check motion model for straight forward movement.
'''
x = 0.0
y = 0.0
o = 0.0
v = 0.1
a = 0.0
# 7.0 means straight forward
s = 7.0
dT = 1
u = np.array([[a], [s]])
kf = EKF(initial_x=x, initial_y=y, initial_o=o, initial_v=v, dT=dT)
x = kf.motion_model(u)
self.assertEqual(kf.posx, 0.1)
self.assertEqual(kf.posy, 0.0)
self.assertEqual(kf.yaw, o)
self.assertEqual(kf.vel, v)
count_list.append(count)
# Extracting simulation data and storing it in a list for plotting
S_truth, S1_meas, S2_meas = nl_simulation(count)
alt_truth_list.append(S_truth[0])
v_truth_list.append(S_truth[1])
alt_meas1_list.append(S1_meas[0])
v_meas1_list.append(S1_meas[1])
alt_meas2_list.append(S2_meas[0])
v_meas2_list.append(S2_meas[1])
# Initializing KF for two gps sensor measurements
if count == 0:
S0 = S1_meas
P0 = np.eye((2))
two_sensor_ekf = EKF(S0, P0)
# Running KF with 2 gps sensor measurements
F = [[1, pi / 180], [0, 1]]
H = np.eye((2))
Q = np.power(q, 2) * np.eye((2))
R = np.power(r, 2) * np.eye((2))
two_sensor_ekf.ekf(S1_meas, F, H, Q, R)
# print("alt truth %f ekf alt_est %f " % (S_truth[0], two_sensor_ekf.S_est[0]))
# print("v truth %f ekf v_est %f " % (S_truth[1], two_sensor_ekf.S_est[1]))
ekf_alt_est_list.append(two_sensor_ekf.S_est[0])
count += 1
print("----plotting NL EKF----")
plot_two_sensors(count_list, alt_truth_list, alt_meas1_list,
initial_transform = vehicle.actor_list[0].get_transform()
initial_rotation = initial_transform.rotation
initial_location = initial_transform.location
## Convert rotation and location to right-handed
r_right_handed = R.from_matrix(
carla_rotation_to_numpy_rotation_matrix(initial_rotation))
t_right_handed = np.array(
[initial_location.x, initial_location.y * -1, initial_location.z]).T
## Convert the right-handed rotation to a quaternion, roll it to get the form w,x,y,z from x,y,z,w
initial_quat = np.roll(r_right_handed.as_quat(), 1)
## Initialize the EKF system #TODO check initial values
vio_ekf = EKF(np.array([0, 0, 0]),
np.array([0, 0, 0]),
initial_quat,
debug=False)
vio_ekf.use_new_data = False
# --- Set instrument noise parameters
vio_ekf.setSigmaAccel(0.0)
vio_ekf.setSigmaGyro(0.0)
### Location transform ###############################################
H_local_to_global = np.eye(4)
H_local_to_global[:3, :3] = r_right_handed.as_matrix()
H_local_to_global[:3, 3] = t_right_handed
H_global_to_local = np.linalg.inv(H_local_to_global)
r_right_handed = R.from_matrix(H_global_to_local[:3, :3])
t_right_handed = H_global_to_local[:3, 3]
def setUp(self):
self.empty_ekf = EKF(landmark_threshold=0)
trajectory_vo = [np.array([0, 0, 0])]
# initialize first ground truth position
# transform from global frame to robot start frame
initial_pos = H_global_to_start @ np.append(t_right_handed, 1)
trajectory_gt = [initial_pos]
# EKF Initialization #TODO #########################################################
# Convert the right-handed rotation to a quaternion, roll it to get the form w,x,y,z from x,y,z,w
initial_quat_wxyz = np.roll(r_right_handed.as_quat(), 1)
# initial_quat_wxyz = np.roll(R.from_matrix(car_start_r).as_quat(), 1)
## Initialize the EKF system #TODO check initial values
# vio_ekf = EKF(np.array([0,0,0]), np.array([0, 0, 0]), initial_quat_wxyz, debug=False)
vio_ekf = EKF(np.array([0, 0, 0]),
np.array([0, 0, 0]),
np.array([1, 0, 0, 0]),
debug=False)
vio_ekf.use_new_data = True
vio_ekf.setSigmaAccel(1.)
vio_ekf.setSigmaGyro(0.5)
vio_ekf.setSigmaVO(100.)
accel_list = []
gyro_list = []
# Main Loop #############################################################
max_length = 500
first = True
step = 0
while True:
step += 1
[twr.Getx()[1],
twr.Getx()[1] + body_radius * np.sin(twr.Getx()[2])])
robot_head.set_xdata(headx)
robot_head.set_ydata(heady)
fig.canvas.draw()
plt.pause(0.01)
if __name__ == "__main__":
# Two-Wheeled Robot Init
twr = TWR() # TWR model object
# Kalman Filter Init
ekf = EKF(twr.c, twr.nl)
body_radius = 0.3
fig, lines, lines_est, msensor, robot_body, robot_head = InitPlot(
twr, body_radius)
mu = ekf.mu
K = ekf.K
two_sig_x = np.array([[2 * np.sqrt(ekf.cov.item((0, 0)))],
[-2 * np.sqrt(ekf.cov.item((0, 0)))]])
two_sig_y = np.array([[2 * np.sqrt(ekf.cov.item((1, 1)))],
[-2 * np.sqrt(ekf.cov.item((1, 1)))]])
two_sig_theta = np.array([[2 * np.sqrt(ekf.cov.item((2, 2)))],
[-2 * np.sqrt(ekf.cov.item((2, 2)))]])
for i in range(int(twr.t_end / twr.dt)):
# truth model updates
twr.Propagate()
#!/usr/bin/env python
import rospy
import sys
import os
import numpy as np
from std_msgs.msg import String
from nav_msgs.msg import Odometry
from marker_msgs.msg import MarkerDetection
from tf.transformations import euler_from_quaternion
sys.path.insert(1, '/home/ros/catkin_ws/src/moro_g12/src/')
from ekf import EKF
from csv_writer import csv_writer
previous_timestamp = None
ekf = EKF(3, 2, 2)
beacons = [(7.3, 3), (1, 1), (9, 9), (1, 8), (5.8, 8)] # xy coordinates
def get_rotation(quaternion):
global roll, pitch, yaw
orientation_list = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
return wrap_to_pi(yaw)
def odom_callback(msg):
timestamp = msg.header.stamp
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
theta = get_rotation(msg.pose.pose.orientation) # radians
robot_sim.add_landmark(4.0, -4.0)
robot_sim.add_landmark(-2.0, -2.0)
robot_sim.add_landmark(-4.0, 4.0)
robot_sim.set_odom_noises(0.33, 0.1, 0.1, 0.33)
robot_sim.set_max_measurement_range(max_measurement_range)
robot_sim.set_measurement_variances(measurement_range_variance_sim,
measurement_angle_variance_sim)
robot_sim.set_plot_sizes(max_measurement_range, max_measurement_range)
robot_sim.set_random_measurement_rate(0.05)
robot_sim.set_sim_time_step(0.1)
# ekf parameters
measurement_range_variance = 0.3 * 0.3
measurement_angle_variance = 5.0 * math.pi / 180.0 * 5.0 * math.pi / 180.0
ekf = EKF(start_x, start_y, start_yaw)
ekf.add_landmark(2.0, 2.0)
ekf.add_landmark(4.0, -4.0)
ekf.add_landmark(-2.0, -2.0)
ekf.add_landmark(-4.0, 4.0)
ekf.set_odom_noises(5.0, 2.0, 2.0, 8.0)
ekf.set_measurement_variances(measurement_range_variance,
measurement_angle_variance)
ekf.set_min_trace(0.001)
ekf.set_plot_sizes(5.0, 5.0)
while True:
# simulate robot behaviors
robot_sim.update_pose(0.2, -0.2)
delta_dist = robot_sim.sim_v * robot_sim.sim_time_step
delta_yaw = robot_sim.sim_w * robot_sim.sim_time_step
def gsf_fill_dict(self, key, x_0, P_0):
self.ekf_dict[key] = EKF(x_0, P_0)
#!/usr/bin/env python3
import os, sys
sys.path.append("/home/jc/Downloads/test_ws/src/imu_gnss_fusion/src")
from ekf import EKF
import numpy as np
class ImuData:
def __init__(self):
self.timestamp = 0.0
self.acc = np.array([.0, .0, .0])
self.gyr = np.array([.0, .0, .0])
ekf = EKF(acc_n=1e-2, gyr_n=1e-4, acc_w=1e-6, gyr_w=1e-8)
last_imu = ImuData()
last_imu.timestamp = 1
curr_imu = ImuData()
curr_imu.timestamp = 1.1
curr_imu.acc = np.array([0.1, 0, 0.])
curr_imu.gyr = np.array([0.0, 0., 0])
ekf.predict(last_imu, curr_imu)
H = np.array([[1., 0., 0., 0., 0., 0., -0., -0., -0., 0., 0., 0., 0., 0., 0.],
[0., 1., 0., 0., 0., 0., -0., -0., -0., 0., 0., 0., 0., 0., 0.],
[0., 0., 1., 0., 0., 0., -0., -0., -0., 0., 0., 0., 0., 0., 0.]])
V = np.array([[
0.002304,
0.,
0.,
], [
import animation
reload(animation)
from animation import Animation
import plotData
reload(plotData)
from plotData import plotData
import ekf
reload(ekf)
from ekf import EKF
dynamics = Dynamics()
animation = Animation()
dataPlot = plotData()
ekf = EKF()
t = P.t_start
while t < P.t_end:
t_next_plot = t + P.t_plot
while t < t_next_plot:
t = t + P.Ts
vc = 1 + 0.5 * np.cos(2 * np.pi * (0.2) * t)
omegac = -0.2 + 2 * np.cos(2 * np.pi * (0.6) * t)
noise_v = vc + np.random.normal(
0, np.sqrt(P.alpha1 * vc**2 + P.alpha2 * omegac**2))
noise_omega = omegac + np.random.normal(
0, np.sqrt(P.alpha3 * vc**2 + P.alpha4 * omegac**2))
u = [noise_v, noise_omega]
dynamics.propagateDynamics(u)
training_size, T))
all_y = []
all_x = []
ukf_mses = []
ekf_mses = []
lstm_mses = []
lstm_stacked_mses = []
bar = ProgressBar()
for s in bar(range(num_sims)):
#x_0 = np.random.normal(0, x_var, 1)
x_0 = np.array([0])
sim = UNGM(x_0, R, Q, x_var)
ukf = UKF(sim.f, sim.F, sim.h, sim.H, sim.Q, sim.R, 5., first_x_0, 1)
ekf = EKF(sim.f, sim.F, sim.h, sim.H, sim.Q, sim.R, first_x_0, 1)
for t in range(T):
x, y = sim.process_next()
ukf.predict()
ukf.update(y)
ekf.predict()
ekf.update(y)
ukf_mses.append(MSE(ukf.get_all_predictions(), sim.get_all_x()))
ekf_mses.append(MSE(ekf.get_all_predictions(), sim.get_all_x()))
all_x.append(np.array(sim.get_all_x()))
all_y.append(np.array(sim.get_all_y()))
X = np.array(all_y)[:, :-1, :]
y = np.array(all_x)[:, 1:, :]
import random
#from matplotlib.pyplot import *
#from mpl_toolkits.mplot3d import Axes3D
from path_finding import PATH
from median_filter_special import myfilter
from cost_matrix import COSTMtrix
from cost_matrix import Window_LEN, Overall_shiftting_WinLen, Standard_LEN
from scipy.ndimage import gaussian_filter1d
from time import time
import scipy.io
from parrallel_thread import Dual_thread_Overall_shift_NURD
from shift_deploy import Shift_Predict
from basic_trans import Basic_oper
from Path_post_pcs import PATH_POST
from ekf import EKF
myekf = EKF()
device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
Resample_size = Window_LEN
R_len = 20
#read_start = 100
read_start = 0
Debug_flag = True
global intergral_flag
intergral_flag = 0
Branch_flag = 0 # 0 fusion, 1 A, 2 B
if (Save_signal_flag == True):
ind = np.argwhere(np.abs(z[1]) < params.fov)
z = z[:, ind][:, :, 0]
return z, ind
if __name__ == "__main__":
t = np.arange(0, params.tf, params.dt)
vc, wc = generateVelocities(t)
v = vc + np.sqrt(params.alpha1 * vc**2 +
params.alpha2 * wc**2) * np.random.randn(vc.size)
w = wc + np.sqrt(params.alpha3 * vc**2 +
params.alpha4 * wc**2) * np.random.randn(wc.size)
Car = CarAnimation()
ekf = EKF(params.dt)
x_hist = []
mu_hist = []
err_hist = []
x_covar_hist = []
y_covar_hist = []
psi_covar_hist = []
K_hist = []
state = np.zeros(3)
dead_reckon = np.zeros(3)
mu = ekf.mu
Sigma = ekf.Sigma
for i in range(t.size):
# GPS and local heading
hdg = radians(0)
psi_k = radians(0)
# Driving simulation
last_xk = [
radians(35.210467),
radians(-97.441811), hdg, 0, 0, psi_k, 0, 0, 0, 0
]
# Commanded wheel velocities
ctrl_signal = [3.14, 6.3]
# Init the ekf
ekf = EKF(last_xk)
print(ekf.motion.get_jac())
exit(0)
dt = 1.0 / 20.0
sim_time = 4
sim_steps = int(round(sim_time / dt))
points = []
# Simulate the EKF
start_time = time.time()
for _ in range(0, sim_steps):
# last_xk = motion.run_filter(last_xk, ctrl_signal, dt)
last_xk = ekf.run_filter(last_xk, ctrl_signal, dt)
