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 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_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_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):
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)
class LocalizationNode(object):
# model in meters
COEFFS = [-0.1416, 2.311, -14.76, 36.79]
EVERY_MINUTE = 0.1667
EVERY_SECOND = 1
def __init__(self):
rospy.init_node("position_publisher", anonymous=True)
self.rate = rospy.Rate(LocalizationNode.EVERY_SECOND)
self.extractParams()
self.frame = "/target_" + str(self.tag)
self.publisher = rospy.Publisher(self.frame, PointStamped, queue_size=10)
self.model = Poly3(LocalizationNode.COEFFS)
self.selector = BasestationSelector(self.basestations)
self.ekf = EKF(self.tag, 1, 0.1, self.basestations, self.model, self.selector)
self.miss_counter = 2
def extractParams(self):
self.tag = rospy.get_param("/localization_node/tag")
stations = rospy.get_param("/localization_node/basestations")
self.buildBasestation(stations)
def buildBasestation(self, stations):
self.basestations = []
for station in stations:
self.basestations.append(Basestation(station[0], float(station[1]), float(station[2])))
def publishPosition(self, position):
msg = PointStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame
msg.point = Point(position[0], position[1], 0)
self.publisher.publish(msg)
def localizationLoop(self):
while not rospy.is_shutdown():
try:
if self.miss_counter >= 2: # after two misses it is assumed the target is not in range
self.rate = rospy.Rate(LocalizationNode.EVERY_MINUTE) # once in a minute
position = self.ekf.setInitialPositionToCloserBasestation()
else:
position = self.ekf.ekfIteration()
self.publishPosition(position)
self.rate = rospy.Rate(LocalizationNode.EVERY_SECOND) # normal rate
self.miss_counter = 0
except NoMeasurementException: # no measurements means the target is not in range
self.miss_counter += 1
self.rate.sleep()
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 __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 test():
from math import pi
import numpy
# pose in frame A
a = EKF.column_vector(1, 2, pi / 2)
# pose in frame B
b = EKF.column_vector(1, 1, 0)
# actual origin of B in A
B = EKF.column_vector(2, 1, pi / 2)
# calculated origin of B in A
BB = EKF.get_offset(a, b)
# use it to convert b into a
aa = EKF.coord_transform(b, BB)
# assert that things match
assert numpy.equal(B, BB).all()
assert numpy.equal(a, aa).all()
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 test():
from math import pi
import numpy
# pose in frame A
a = EKF.column_vector(1, 2, pi / 2)
# pose in frame B
b = EKF.column_vector(1, 1, 0)
# actual origin of B in A
B = EKF.column_vector(2, 1, pi / 2)
# calculated origin of B in A
BB = EKF.get_offset(a, b)
# use it to convert b into a
aa = EKF.coord_transform(b, BB)
# assert that things match
assert numpy.equal(B, BB).all()
assert numpy.equal(a, aa).all()
def __init__(self, b1, b2, b3, dt):
EKF.__init__(self, 6, self.f, self.F, self.h, self.H)
self.b1 = b1
self.b2 = b2
self.b3 = b3
self.dt = dt
# self.Q = PROCESS_NOISE_VARIANCE * np.eye(6)
w_psd = PROCESS_NOISE_VARIANCE
self.Q = np.array([[0, 0, 0, -0.5 * dt**2 * w_psd, 0, 0],
[0, 0, 0, 0, -0.5 * dt**2 * w_psd, 0],
[0, 0, 0, 0, 0, -0.5 * dt**2 * w_psd],
[0, 0, 0, -0.5 * dt**3 * w_psd + dt * w_psd, 0, 0],
[0, 0, 0, 0, -0.5 * dt**3 * w_psd + dt * w_psd, 0],
[0, 0, 0, 0, 0, -0.5 * dt**3 * w_psd + dt * w_psd]
]) + w_psd * np.eye(self.n)
def __init__(self, b1, b2, b3, dt):
EKF.__init__(self, 6, self.f, self.F, self.h, self.H)
self.b1 = b1
self.b2 = b2
self.b3 = b3
self.dt = dt
# self.Q = PROCESS_NOISE_VARIANCE * np.eye(6)
w_psd = PROCESS_NOISE_VARIANCE
self.Q = np.array([
[0, 0, 0, -0.5*dt**2*w_psd, 0, 0],
[0, 0, 0, 0, -0.5*dt**2*w_psd, 0],
[0, 0, 0, 0, 0, -0.5*dt**2*w_psd],
[0, 0, 0, -0.5*dt**3*w_psd + dt*w_psd, 0, 0],
[0, 0, 0, 0, -0.5*dt**3*w_psd + dt*w_psd, 0],
[0, 0, 0, 0, 0, -0.5*dt**3*w_psd + dt*w_psd]
]) + w_psd * np.eye(self.n)
def __init__(self):
rospy.init_node("position_publisher", anonymous=True)
self.rate = rospy.Rate(LocalizationNode.EVERY_SECOND)
self.extractParams()
self.frame = "/target_" + str(self.tag)
self.publisher = rospy.Publisher(self.frame, PointStamped, queue_size=10)
self.model = Poly3(LocalizationNode.COEFFS)
self.selector = BasestationSelector(self.basestations)
self.ekf = EKF(self.tag, 1, 0.1, self.basestations, self.model, self.selector)
self.miss_counter = 2
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_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)
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 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 setUp(self):
self.empty_ekf = EKF(landmark_threshold=0)
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]
#!/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
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):
def predict(self, u):
EKF.predict(self, u, self.Q, self.dt)
z[0, i] = r + np.random.normal(0, params.sigma_r)
z[1, i] = theta + np.random.normal(0, params.sigma_theta)
z[1, i] = unwrap(z[1, i])
return z
if __name__ == "__main__":
read_file = True
if read_file:
t, v, w, x, y, psi = readFile()
vc, wc = generateVelocities(t)
Car = CarAnimation()
ekf = EKF(params.dt)
x_hist = []
mu_hist = []
err_hist = []
x_covar_hist = []
y_covar_hist = []
psi_covar_hist = []
K_hist = []
x0 = params.x0
y0 = params.y0
phi0 = params.theta0
state = np.array([x[0], y[0], psi[0]])
dead_reckon = np.array([x0, y0, phi0])
mu = np.array([x0, y0, phi0])
[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()
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
class KalmanFilterProxy:
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 run(self):
# Application creation
app = web.Application()
self._sio.attach(app)
# Run app itself
web.run_app(app, host=self._host, port=self._port)
@staticmethod
def _on_sim_connect(sid, environ):
print('Connected: ', sid)
@staticmethod
def _on_sim_disconnect(sid):
print('Disconnected: ', sid)
async def _on_telemetry(self, sid, data):
# Visualize data received data
print('Telemetry received: ', data)
# Communication
return_msg = dict()
if data is not None:
meas_list = str.split(data['sensor_measurement'])
# Parse data accordingly to sensor type
if meas_list[0] == "L":
measurement = Measurement(
sensor=Sensor.LIDAR,
data=np.array([float(x) for x in meas_list[1:3]]),
ts_us=int(meas_list[3]))
groundtruth = [float(x) for x in meas_list[4:8]]
elif meas_list[0] == "R":
measurement = Measurement(
sensor=Sensor.RADAR,
data=np.array([float(x) for x in meas_list[1:4]]),
ts_us=int(meas_list[4]))
groundtruth = [float(x) for x in meas_list[5:9]]
else:
raise ValueError('Not correct sensor type')
self.ekf.process_measurement(measurement)
estimation = self.ekf.get_estimation()
# Stack history of estimation vs groundtruth
self.estimations_history.append(estimation)
self.groundtruth_history.append(groundtruth)
# Calculate RMSE
estimation_rmse = rmse(self.estimations_history,
self.groundtruth_history)
# Create message
return_msg['estimate_x'] = float(estimation[0])
return_msg['estimate_y'] = float(estimation[1])
return_msg['rmse_x'] = estimation_rmse[0]
return_msg['rmse_y'] = estimation_rmse[1]
return_msg['rmse_vx'] = estimation_rmse[2]
return_msg['rmse_vy'] = estimation_rmse[3]
# Message sending
print("Sending data: ", return_msg)
await self._sio.emit('estimate_marker', return_msg)
else:
# If data is not valid, send dummy response
await self._sio.emit('manual', {})
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)
class TestEKF(unittest.TestCase):
def setUp(self):
self.empty_ekf = EKF(landmark_threshold=0)
def tearDown(self):
del self.empty_ekf
def test_init(self):
self.assertIsInstance(self.empty_ekf, EKF)
self.assertEqual(self.empty_ekf.landmark_types, [])
self.assertEqual(self.empty_ekf.landmark_counts, [])
np.testing.assert_array_equal(self.empty_ekf.system_state,
np.array([[0], [0], [0]]))
np.testing.assert_array_equal(
self.empty_ekf.covariance,
np.array([[0.95, 0, 0], [0, 0.95, 0], [0, 0, 0.95]]))
def test_noop_move(self):
# Run the move function, but without actually moving.
self.empty_ekf.update(radians(0), 0, [])
np.testing.assert_array_equal(self.empty_ekf.pos(),
np.array([[0], [0], [0]]))
np.testing.assert_array_equal(
self.empty_ekf.covariance,
np.array([[0.95, 0, 0], [0, 0.95, 0], [0, 0, 0.95]]))
def test_simple_move(self):
# Turn left 30 degrees and advance 10 units of distance.
self.empty_ekf.update(radians(30), 10, [])
np.testing.assert_almost_equal(
self.empty_ekf.pos(), np.array([[8.66025404], [5], [0.52359878]]))
np.testing.assert_almost_equal(
self.empty_ekf.covariance,
np.array([[75.95, -38.9711432, -8.0005164],
[-38.9711432, 25.95, 4.8808997],
[-8.0005164, 4.8808997, 0.9637078]]))
def test_linear_move_and_observe(self):
# Add a landmark at the start.
self.empty_ekf.update(radians(0), 0, [(20, 0, 'statue')])
np.testing.assert_array_equal(self.empty_ekf.pos(),
np.array([[0], [0], [0]]))
np.testing.assert_array_equal(self.empty_ekf.system_state,
np.array([[0], [0], [0], [20], [0]]))
np.testing.assert_array_equal(
self.empty_ekf.covariance,
np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95],
[0, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0],
[0, 0.95, 0, 0, 0.95]]))
# Move 5 units, but imply some error by landmark drift.
self.empty_ekf.update(radians(0), 5, [(19, 0, 'statue')])
np.testing.assert_almost_equal(
self.empty_ekf.pos(), np.array([[5.8274554], [0], [-0.042244909]]))
np.testing.assert_almost_equal(
self.empty_ekf.system_state,
np.array([[5.8274554], [0], [-0.042244909], [19.9014031], [0]]))
np.testing.assert_almost_equal(
self.empty_ekf.covariance,
np.array([[25.95, 0, -4.75, 0.95, 0], [0, 0.95, 0, 0, 0.95],
[-4.75, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0],
[0, 0.95, 0, 0, 0.95]]))
def test_rotate_and_observe(self):
# Add a landmark at the start.
self.empty_ekf.update(radians(0), 0, [(20, 0, 'statue')])
np.testing.assert_array_equal(self.empty_ekf.pos(),
np.array([[0], [0], [0]]))
np.testing.assert_array_equal(self.empty_ekf.system_state,
np.array([[0], [0], [0], [20], [0]]))
np.testing.assert_array_equal(
self.empty_ekf.covariance,
np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95],
[0, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0],
[0, 0.95, 0, 0, 0.95]]))
# Turn 30 degrees, but imply some error by landmark drift.
self.empty_ekf.update(
radians(30), 0,
[(20 * cos(radians(-2)), 20 * sin(radians(-2)), 'statue')])
np.testing.assert_almost_equal(self.empty_ekf.pos(),
np.array([[0], [0], [0.55788443]]))
np.testing.assert_almost_equal(
self.empty_ekf.system_state,
np.array([[0], [0], [0.55788443], [20], [0]]))
np.testing.assert_almost_equal(
self.empty_ekf.covariance,
np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95],
[0, 0, 0.9637078, 0, 0], [0.95, 0, 0, 1.15, 0],
[0, 0.95, 0, 0, 0.95]]))
def test_add_two_landmarks(self):
# Add first landmark.
self.empty_ekf.update(radians(0), 0, [(20, 0, 'statue')])
np.testing.assert_array_equal(self.empty_ekf.pos(),
np.array([[0], [0], [0]]))
np.testing.assert_array_equal(self.empty_ekf.system_state,
np.array([[0], [0], [0], [20], [0]]))
np.testing.assert_array_equal(
self.empty_ekf.covariance,
np.array([[0.95, 0, 0, 0.95, 0], [0, 0.95, 0, 0, 0.95],
[0, 0, 0.95, 0, 0], [0.95, 0, 0, 1.15, 0],
[0, 0.95, 0, 0, 0.95]]))
# Add second landmark.
self.empty_ekf.update(radians(0), 0, [(0, 20, 'fountain')])
np.testing.assert_array_equal(self.empty_ekf.pos(),
np.array([[0], [0], [0]]))
np.testing.assert_array_equal(
self.empty_ekf.system_state,
np.array([[0], [0], [0], [20], [0], [0], [20]]))
np.testing.assert_array_equal(
self.empty_ekf.covariance,
np.array([[0.95, 0, 0, 0.95, 0, 0.95, 0],
[0, 0.95, 0, 0, 0.95, 0, 0.95], [0, 0, 0.95, 0, 0, 0, 0],
[0.95, 0, 0, 1.15, 0, 0.95, 0],
[0, 0.95, 0, 0, 0.95, 0, 0.95],
[0.95, 0, 0, 0.95, 0, 1.15, 0],
[0, 0.95, 0, 0, 0.95, 0, 0.95]]))
#!/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.,
], [
class Fusion:
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 imu_callback(self, msg):
imu = ImuData()
imu.timestamp = msg.header.stamp.to_sec()
imu.acc = np.array([
msg.linear_acceleration.x, msg.linear_acceleration.y,
msg.linear_acceleration.z
])
imu.gyr = np.array([
msg.angular_velocity.x, msg.angular_velocity.y,
msg.angular_velocity.z
])
if (not self.initialized):
self.imu_buff.append(imu)
return
self.ekf.predict(self.last_imu, imu)
self.last_imu = imu
self.pub_state()
def gnss_callback(self, msg):
if (msg.status.status != 2):
print("Error with GPS!!")
return
gps = GPSData()
gps.timestamp = msg.header.stamp.to_sec()
gps.lla = np.array([msg.latitude, msg.longitude, msg.altitude])
gps.cov = np.array(msg.position_covariance).reshape((3, 3))
if (not self.initialized):
if (len(self.imu_buff) < 100):
print("not enough imu data!!")
return
self.last_imu = self.imu_buff[99]
if (abs(gps.timestamp - self.last_imu.timestamp) > 0.5):
print("GPS and imu not synchronized!!")
return
self.ekf.state.timestamp = self.last_imu.timestamp
if (not self.init_rot_from_imu_data()):
return
self.init_lla_ = gps.lla
self.initialized = True
return
self.p_G_Gps_ = gps.lla2enu(self.init_lla_, gps.lla)
p_GI = self.ekf.state.p_GI
r_GI = self.ekf.state.r_GI
residual = self.p_G_Gps_ - (p_GI + r_GI.dot(self.I_p_Gps_))
H = np.zeros((3, 15))
H[:3, :3] = np.eye(3)
H[:3, 6:9] = -r_GI.dot(skew_matrix(self.I_p_Gps_))
V = gps.cov
self.ekf.update_measurement(H, V, residual)
def init_rot_from_imu_data(self):
## 初始化姿态，加速的方差不能太大
sum_acc = np.array([0., 0., 0.])
for imu_data in self.imu_buff:
sum_acc += imu_data.acc
mean_acc = sum_acc / len(self.imu_buff)
sum_err2 = np.array([0., 0., 0.])
for imu_data in self.imu_buff:
sum_err2 += np.power(imu_data.acc - mean_acc, 2)
std_acc = np.power(sum_err2 / len(self.imu_buff), 0.5)
if (np.max(std_acc) > 3.0):
print("acc std is too big !!")
return False
## 这里获得旋转矩阵的原理是？
z_axis = mean_acc / np.linalg.norm(mean_acc)
z_axis = z_axis.reshape((3, 1))
x_axis = np.array([1, 0, 0]).reshape(
(3, 1)) - z_axis.dot(z_axis.T).dot(
np.array([1, 0, 0]).reshape((3, 1)))
x_axis = x_axis / np.linalg.norm(x_axis)
y_axis = np.cross(z_axis.reshape(3), x_axis.reshape(3)).reshape(3, 1)
y_axis = y_axis / np.linalg.norm(y_axis)
r_IG = np.zeros((3, 3))
r_IG[:3, 0] = x_axis.reshape(3)
r_IG[:3, 1] = y_axis.reshape(3)
r_IG[:3, 2] = z_axis.reshape(3)
self.ekf.state.r_GI = r_IG.T ## 初始化姿态
return True
def pub_state(self):
if (self.p_G_Gps_ is None):
return
odom_msg = Odometry()
odom_msg.header.frame_id = fixed_id
odom_msg.header.stamp = rospy.Time.now()
odom_msg.pose.pose = Pose(
Point(self.ekf.state.p_GI[0], self.ekf.state.p_GI[1],
self.ekf.state.p_GI[2]), Quaternion(0, 0, 0, 1))
odom_msg.twist.twist = Twist(
Vector3(self.ekf.state.v_GI[0], self.ekf.state.v_GI[1],
self.ekf.state.v_GI[2]), Vector3(0, 0, 0))
self.pub_odom.publish(odom_msg)
pose_msg = PoseStamped()
pose_msg.header = odom_msg.header
pose_msg.pose = odom_msg.pose.pose
self.nav_path.header = pose_msg.header
self.nav_path.poses.append(pose_msg)
self.pub_path.publish(self.nav_path)
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
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):
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:, :]