def __init__(self, bzrc):
self._kalman = Kalman()
self.aliveTime = time.time()
self.bzrc = bzrc
self.constants = self.bzrc.get_constants()
# print self.constants
self.commands = []
self.potentialFields = []
self.updates = []
self.flag_sphere = 400
self.obstacle_sphere = 1000
self.enemy_sphere = 100
self.explore_sphere = 50
self.prev_x = {}
self.prev_y = {}
self.stuck_ticks = {}
for x in xrange(20):
self.prev_x[x] = 0
self.prev_y[x] = 0
self.stuck_ticks[x] = 0
self.path_fields = []
self.grid = Grid(int(self.constants['worldsize']),
int(self.constants['worldsize']),
self.initialGridConstant, self.initialGridConstant)
def __init__(self, bzrc, tank_index):
#print tank_index
self.agent_index = tank_index
self._kalman = Kalman()
self.aliveTime = time.time()
self.bzrc = bzrc
self.constants = self.bzrc.get_constants()
self.mytanks = self.bzrc.get_mytanks()
self.targetTank,self.targ = self.closest_enemy(self.mytanks[self.agent_index], self.bzrc.get_othertanks())
self._kalman.resetArrays(self.targetTank.x, self.targetTank.y)
# print self.constants
self.commands = []
self.potentialFields = []
self.updates = []
self.flag_sphere = 400
self.obstacle_sphere = 1000
self.enemy_sphere = 100
self.explore_sphere = 50
self.prev_x = {}
self.prev_y = {}
self.stuck_ticks = {}
for x in xrange(20):
self.prev_x[x] = 0
self.prev_y[x] = 0
self.stuck_ticks[x] = 0
self.path_fields = []
self.grid = Grid(int(self.constants['worldsize']),
int(self.constants['worldsize']),
self.initialGridConstant,
self.initialGridConstant)
def __init__(self, interval=1):
""" Constructor
:type interval: int
:param interval: Check interval, in seconds
"""
self.sensorfusion = Kalman()
print("Connecting to sensortag...")
self.tag = SensorTag(macAddress)
print("connected.")
self.tag.accelerometer.enable()
self.tag.magnetometer.enable()
self.tag.gyroscope.enable()
self.tag.battery.enable()
self.pitch = 0
self.angular_velocity = 0
self.linear_velocity = 0
self.acceleration = 0
time.sleep(1.0) # Loading sensors
self.prev_time = time.time()
thread = threading.Thread(target=self.run, args=())
thread.daemon = True # Daemonize thread
thread.start() # Start the execution
def main():
sensorfusion = Kalman()
print("Connecting to sensortag...")
tag = SensorTag(macAddress)
print("connected.")
tag.accelerometer.enable()
tag.magnetometer.enable()
tag.gyroscope.enable()
tag.battery.enable()
time.sleep(1.0) # Loading sensors
prev_time = time.time()
while True:
accelerometer_readings = tag.accelerometer.read()
gyroscope_readings = tag.gyroscope.read()
magnetometer_readings = tag.magnetometer.read()
ax, ay, az = accelerometer_readings
gx, gy, gz = gyroscope_readings
mx, my, mz = magnetometer_readings
curr_time = time.time()
dt = curr_time - prev_time
sensorfusion.computeAndUpdateRollPitchYaw(ax, ay, az, gx, gy, gz, mx,
my, mz, dt)
print(f"dt: {dt} pitch: {sensorfusion.pitch}")
prev_time = curr_time
print("Battery: ", tag.battery.read())
def __init__(
self,
set_point,
water_level,
maximum_flowrate,
initial_flowrate=0,
learning_rate=0.1,
last_layer_activation_function='tanh',
initial_error_covariance_matrix=initial_error_covariance_matrix,
transition_matrix=transition_matrix,
input_matrix=input_matrix,
input_vector=input_vector,
measurement_matrix=measurement_matrix,
disturbance=disturbance,
uncertainty=uncertainty):
initial_state = np.asarray([initial_flowrate]).T
self.kalman = Kalman(
initial_state=initial_state,
initial_error_covariance_matrix=initial_error_covariance_matrix,
transition_matrix=transition_matrix,
measurement_matrix=measurement_matrix,
input_matrix=input_matrix,
input_vector=input_vector,
measurement_noise=uncertainty,
process_noise=disturbance)
self.pidnn = PIDNN(
set_point=set_point,
actual_value=water_level,
prev_output=initial_flowrate,
learning_rate=learning_rate,
last_layer_activation_function=last_layer_activation_function)
self.maximum_flowrate = maximum_flowrate
def __init__(self):
rospy.init_node("IK_node", anonymous=False)
self.input_sub = rospy.Subscriber("/vx250/input_arm_pose", ArmPose,
self.callback)
self.marker_pub = rospy.Publisher("/vx250/joint_state_est",
Marker,
queue_size=1)
self.angles_pub = rospy.Publisher("/vx250/arm_controller/command",
JointTrajectory,
queue_size=1)
self.measurement = ArmState()
self.states = {
'Green':
Kalman(
np.ones((6, 1)) / 1000,
np.diag([
2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(),
2.5 * randn(), 2.5 * randn()
]), 1 / LOOP_RATE),
'Red':
Kalman(
np.ones((6, 1)) / 1000,
np.diag([
2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(),
2.5 * randn(), 2.5 * randn()
]), 1 / LOOP_RATE),
'Blue':
Kalman(
np.ones((6, 1)) / 1000,
np.diag([
2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(),
2.5 * randn(), 2.5 * randn()
]), 1 / LOOP_RATE),
'Yellow':
Kalman(
np.ones((6, 1)) / 1000,
np.diag([
2.5 * randn(), 2.5 * randn(), 2.5 * randn(), 2.5 * randn(),
2.5 * randn(), 2.5 * randn()
]), 1 / LOOP_RATE)
}
self.prev_states = self.states
self.rate = rospy.Rate(LOOP_RATE)
def __init__(self, trackId, rect, img, mask):
self.trackId = trackId
self.lastClass = 'unknown'
self.lastKeypoints = None
self.rect = []
self.searchWindow = rect
self.template = []
self.tracklet = []
self.trackletRects = []
self.direction = Enum(['LEFT', 'RIGHT', 'UNKNOWN'])
self.actions = []
self.actions.append('unknown')
self.classHistory = []
self.active = True
self.occluded = False
self.foundCount = 0
self.numTemplates = 6
self.outDimens = None
#self.trackOutputStream = None
self.doTrackWrite = False
self.savedFrames = []
self.outFps = 30
height, width = img.shape[:2]
self.imgBounds = (width, height)
x, y, w, h = rect
x, y, w, h = rect = self.resizeRect(rect, (1.0, 1.0), (0.0, 0.0))
#x,y,w,h = rect = self.resizeRect(rect,(1.0,0.75),(-0.15,-0.2))
#crop syntax is [starty:endy, startx:endx]
template = img[y:y + h, x:x + w]
#maskFiltered = mask[y:y+h, x:x+w]
# 3 Kalman filters per research document
self.kalmanFast = Kalman((x + (w / 2), y + (h / 2)))
self.kalmanArea = Kalman((w, h)) # ,0.003,0.3)
self.kalmanSlow = Kalman((x + (w / 2), y + (h / 2)), 0.003, 0.03)
self.updateTemplate(rect, template)
#x,y,w,h = rect = self.resizeRect(rect,(1.5,1.3),(0,0.5))
self.trackletSize = (0, 0)
self.updateTracklet(rect, img)
def init_demo_kalman():
n = m = 2
A = np.array([[0.9, 0.2], [-0.3, 0.5]])
H = np.array([[1.0, 0.1], [-0.2, 1.1]])
R = 0.5 * np.eye(m)
Q = 1.4 * np.eye(n)
x0 = [-0.7, 0.9]
sys = LinearSystem(n, m, x0, A, H, R, Q)
kalman = Kalman(n, m, sys)
return kalman
def __init__(self, robot_id):
# Does this need to be a node? Maybe it could be a tf?
rospy.init_node('Filter' + str(robot_id))
self.pub = rospy.Publisher("Filter" + str(robot_id) + "/state",
Odometry,
queue_size=1)
self.sub = rospy.Subscriber("Sensor/measurement" + str(robot_id),
Odometry, self.new_measurement)
self.kf = Kalman()
self.state_out = Odometry()
self.state = np.array([[0], [0], [0], [0], [0], [0]])
self.startTime = rospy.get_time()
self.oldTime = 0.0
def __init__(self, initial_point, trackIdCount):
"""Initialize variables used by Track class
Args:
prediction: predicted centroids of clusters to be tracked
trackIdCount: identification of each track object
Return:
None
"""
self.track_id = trackIdCount # identification of each track object
self.KF = Kalman(initial_point) # KF instance to track this object
self.prediction = np.asarray(
initial_point) # predicted centroids (x,y)
self.skipped_frames = 0 # number of frames skipped undetected
self.last_detection_assigment = 0 # number of frames skipped undetected
self.age = 0 # the number of frames this tracker has lived for
self.trace = [] # trace path
def _filter_xs(self, s_0, s_ts, pi_0, sigma_0, As, Cs, Qs, Rs):
"""
Computes x_1|1, P_1|1, ..., x_T|T, P_T|T given observations, parameters and values of switching
variable at each time.
:returns:
-n_LF x T numpy array
Smoothed means
-T x n_LF x n_LF numpy array
Smoothed covariances
-T-1 x n_LF x n_LF numpy array
Smoothed lagged covariances (ie, cov[x_t, x_t-1])
"""
# Initialize Kalman filter using values of parameters at t = 0, even though they're never used
kf = Kalman(mu_0=pi_0.copy(), sigma_0=sigma_0.copy(), A=As[s_0], B=None, C=Cs[s_0], D=None,
Q=Qs[s_0], R=Rs[s_0])
# x_t|t, P_t|t, t = 1, ..., T
x_filts = np.zeros([self.T, self.n_LF])
P_filts = np.zeros([self.T, self.n_LF, self.n_LF])
# x_t|t-1, P_t|t-1. t = 1, ..., T. Indexed by t.
x_pred = np.zeros([self.T, self.n_LF])
P_pred = np.zeros([self.T, self.n_LF, self.n_LF])
# Filtering step
for t in range(0, self.T): # corresponds to t = 1, ..., T
# Change parameters. Never need to use A_{s_0}, etc.
kf.A = As[s_ts[t]]
kf.C = Cs[s_ts[t]]
kf.Q = Qs[s_ts[t]]
kf.R = Rs[s_ts[t]]
# Compute x_{t|t-1}, P_{t|t-1}
kf.predict()
x_pred[t] = kf.mu
P_pred[t] = kf.sigma
# Compute x_{t|t}, P_{t|t}
kf.update(self.y_ts[t])
x_filts[t] = kf.mu
P_filts[t] = kf.sigma
# TODO: run smoother to fill in missing data!!!
return x_filts, P_filts, x_pred, P_pred
def __init__(self):
self.A_k = None
self.B_k = None
self.dt = 0.02 # simulation time step
self.t_rc = 0.005 # membrane RC time constant
self.t_ref = 0.001 # refractory period
self.tau = 0.014 # synapse time constant for standard first-order lowpass filter synapse
self.N_A = 1000 # number of neurons in first population
self.rate_A = 200, 400 # range of maximum firing rates for population A
self.model = nengo.Network(label="Kalman SNN")
self.sim = None
self.kalman = Kalman()
self.output = None
self.testX = None
self.count = 0
self.chN = None
ay_offset = np.mean(ay_data[0:offset_num])
az_offset = np.mean(az_data[0:offset_num])
gx_offset = np.mean(gx_data[0:offset_num])
gy_offset = np.mean(gy_data[0:offset_num])
gz_offset = np.mean(gz_data[0:offset_num])
# q = 1000
# r = 0.001
Q_list = [1000, 0.001]
R_list = [1000, 0.001]
for q, r in zip(Q_list, R_list):
fi_kf = Kalman(F=np.array([[1, 1], [0, 1]]),
P = np.array([[1000, 0], [0, 1000]]),
Q = np.array([[q, 0], [0, q]]),
R = np.array([[r]]),
H = np.array([[1, 0]]),
x = np.array([[0], [0]]),
u = np.array([[0], [0]]))
theta_kf = Kalman(F=np.array([[1, 1], [0, 1]]),
P = np.array([[1000, 0], [0, 1000]]),
Q = np.array([[q, 0], [0, q]]),
R = np.array([[r]]),
H = np.array([[1, 0]]),
x = np.array([[0], [0]]),
u = np.array([[0], [0]]))
should_init_kf = True
fi_raw_list = []
theta_raw_list = []
priori, posteriori
'''
# Dynamics
from process_dynamics import *
from kalman import Kalman
# initial_state = [x_coord, y_coord, x_velocity, y_velocity, x_acceleration, y_acceleration]
initial_state = np.asarray([coords[0][0], coords[0][1], 0, 0, 0, 0]).T
k = Kalman(initial_state=initial_state,
initial_error_covariance_matrix=initial_error_covariance_matrix,
transition_matrix=transition_matrix,
measurement_matrix=measurement_matrix,
input_matrix=input_matrix,
input_vector=input_vector,
measurement_noise=uncertainty,
process_noise=disturbance)
# Visualization
length = 1366
breadth = 768
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
fig, ax = plt.subplots()
xdata, ydata = [], []
# Create a socket object soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM) time.sleep(3) # arduino will reset on serial open # let it calibrate, has to be still # Initialize orientation vectors acc = [0.0, 0.0, 0.0] mag = [0.0, 0.0, 0.0] # Reset rpy reset_condition = True # Kalman objects kalman_x = Kalman() kalman_y = Kalman() kalman_z = Kalman() ##################################### # YOU NEED TO ADJUST THESE (TASK 4) # ##################################### # Set Kalman sensor noise stdevs kalman_x.setSensorNoise(0.2, 0.03) # 0.2, 0.03 kalman_y.setSensorNoise(0.2, 0.03) # 0.2, 0.03 kalman_z.setSensorNoise(0.6, 0.03) # 0.6, 0.03 # kalman_x.setSensorNoise(0.01, 0.01) # kalman_y.setSensorNoise(0.01, 0.01) # kalman_z.setSensorNoise(0.01, 0.01)
def test(noise=20,
time_max=10,
show_acceleration_plots=True,
filter_name='kalman'):
import sys
sys.path.append('../')
import numpy as np
import time
# Record Data
import time
import numpy as np
import pyautogui
def record_data(noise=20, time_max=10):
print("RECORDING DATA")
print("Move your cursor as if you were driving it.")
coords = []
coords_actual = []
timestamp = []
T = time.time()
while time.time() - T < time_max:
t = time.time()
while True:
if time.time() - t > .02:
timestamp.append(time.time() - T)
act = np.asarray(pyautogui.position())
pos = act + 2 * noise * np.asarray([
np.random.rand(), np.random.rand()
]) - noise * np.asarray(
[np.random.rand(), np.random.rand()])
coords.append(pos)
pos_actual = act
coords_actual.append(pos_actual)
break
print("RECORDING DATA COMPLETED")
return coords, coords_actual, timestamp
coords, coords_actual, timestamp = record_data(noise=noise,
time_max=time_max)
'''
FLOW:
INITIAL_STATE_VECTOR, INITIAL_ERROR_COVARIANCE_MATRIX
PRIORI CALCULATION, PRIORI_ERROR_COVARIANCE
OBSERVE_READINGS, EXPECTED_READINGS, EXPECTED_ERROR_COVARIANCE
KALMAN_GAIN
TAKES IN MEASUREMENT_NOISE, MEASUREMENT_MATRIX, NEW_ERROR_COVARIANCE
POSTERIORI CALCULATION
TAKES IN KALMAN_GAIN, OBSERVED_READINGS, PRIORI
POSTERIORI_ERROR_COVARIANCE
'''
'''
INPUT:
cursor coords
OUTPUT:
priori, posteriori
'''
# Dynamics
# initial_state = [x_coord, y_coord, x_velocity, y_velocity, x_acceleration, y_acceleration]
initial_state = np.asarray([coords[0][0], coords[0][1], 0, 0, 0, 0]).T
initial_error_covariance_matrix = 0 * np.eye(6)
dt = 0.02
transition_matrix = np.asarray([[1, 0, dt, 0, 0, 0], [0, 1, 0, dt, 0, 0],
[0, 0, 1, 0, dt, 0], [0, 0, 0, 1, 0, dt],
[0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]])
position_funciton = lambda position, velocity: position + velocity * dt
velocity_funciton = lambda velocity, acceleration: velocity + acceleration * dt
acceleration_function = lambda acceleration: acceleration
def transition_function(state_vector, input_vector):
# print('state_vector', state_vector.shape)
position = position_funciton(state_vector[0:2], state_vector[2:4])
velocity = velocity_funciton(state_vector[2:4], state_vector[4:6])
acceleration = acceleration_function(state_vector[4:6])
return_state = np.vstack((position, velocity, acceleration))
return np.reshape(return_state, (-1, 1)) + np.zeros(6).T.reshape(
state_vector.size, input_vector.shape[0]) * input_vector
def measurement_function(state_vector):
# print('state_vector', state_vector.shape)
return np.reshape(state_vector[0:2], (-1, 1))
# print(nd.Jacobian(transition_function)(initial_state))
input_matrix = np.zeros(6).T
input_vector = np.zeros((1, 1))
measurement_matrix = np.asarray([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]])
disturbance = 10 * np.eye(6)
uncertainty = noise * np.eye(2)
from kalman import Kalman
from extended_kalman import ExtendedKalman
kf = Kalman(
initial_state=initial_state,
initial_error_covariance_matrix=initial_error_covariance_matrix,
transition_matrix=transition_matrix,
measurement_matrix=measurement_matrix,
input_matrix=input_matrix,
input_vector=input_vector,
measurement_noise=uncertainty,
process_noise=disturbance)
ekf = ExtendedKalman(
initial_state=initial_state,
initial_error_covariance_matrix=initial_error_covariance_matrix,
transition_function=transition_function,
measurement_function=measurement_function,
input_matrix=input_matrix,
input_vector=input_vector,
measurement_noise=uncertainty,
process_noise=disturbance)
if filter_name == 'kalman':
k = kf
elif filter_name == 'extended':
k = ekf
# Visualization
length = 1920
breadth = 1080
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
fig, ax = plt.subplots()
cursor_x, cursor_y = [], []
actual, = plt.plot([], [], '-', label='actual')
xdata, ydata = [], []
recorded, = plt.plot([], [], '*', label='recorded')
priori_x, priori_y = [], []
priori_plot, = plt.plot([], [], 'g-', label='priori_plot')
posteriori_x, posteriori_y = [], []
posteriori_plot, = plt.plot([], [], 'r-', label='posteriori_plot')
if show_acceleration_plots:
fig_acc_x, acc_x = plt.subplots()
priori_acceleration_x = []
posteriori_acceleration_x = []
priori_acceleration_plot_x, = plt.plot([], [],
'g-',
label='priori_acc_x')
posteriori_acceleration_plot_x, = plt.plot([], [],
'r-',
label='posteriori_acc_x')
fig_acc_y, acc_y = plt.subplots()
priori_acceleration_y = []
posteriori_acceleration_y = []
priori_acceleration_plot_y, = plt.plot([], [],
'g-',
label='priori_acc_y')
posteriori_acceleration_plot_y, = plt.plot([], [],
'r-',
label='posteriori_acc_y')
timestamp_data = []
def init():
ax.set_xlim(0, length)
ax.set_ylim(0, breadth)
ax.legend()
if show_acceleration_plots:
acc_x.set_xlim(0, 10)
acc_x.set_ylim(-1000, 1000)
acc_x.legend()
acc_y.set_xlim(0, 10)
acc_y.set_ylim(-1000, 1000)
acc_y.legend()
return actual, recorded, priori_plot, posteriori_plot, priori_acceleration_plot_x, posteriori_acceleration_plot_x, priori_acceleration_plot_y, posteriori_acceleration_plot_y,
else:
return actual, recorded, priori_plot, posteriori_plot,
def update(frame):
n = frame
# print(xdata[-1], ydata[-1])
# print(n - 1000, n)
# print(np.asarray(coords[n - 100:n])[:, 0], 1080 - np.asarray(coords[n - 100:n])[:, 1])
pos_x = np.asarray(coords[n])[0]
pos_y = breadth - np.asarray(coords[n])[1]
# kpriori, kposteriori = kf(np.asarray([pos_x, pos_y]), None)
# epriori, eposteriori = ekf(np.asarray([pos_x, pos_y]), None)
priori, posteriori = k(np.asarray([pos_x, pos_y]), input_vector)
priori_x.append(priori[0])
priori_y.append(priori[1])
priori_plot.set_data(priori_x, priori_y)
xdata.append(pos_x)
ydata.append(pos_y)
recorded.set_data(xdata, ydata)
posteriori_x.append(posteriori[0])
posteriori_y.append(posteriori[1])
posteriori_plot.set_data(posteriori_x, posteriori_y)
cursor_x.append(np.asarray(coords_actual[n])[0])
cursor_y.append(breadth - np.asarray(coords_actual[n])[1])
actual.set_data(cursor_x, cursor_y)
if show_acceleration_plots:
timestamp_data.append(timestamp[n])
priori_acceleration_x.append(priori[4, 0])
priori_acceleration_plot_x.set_data(timestamp_data,
priori_acceleration_x)
posteriori_acceleration_x.append(posteriori[4, 0])
posteriori_acceleration_plot_x.set_data(timestamp_data,
posteriori_acceleration_x)
priori_acceleration_y.append(priori[5, 0])
priori_acceleration_plot_y.set_data(timestamp_data,
priori_acceleration_y)
posteriori_acceleration_y.append(posteriori[5, 0])
posteriori_acceleration_plot_y.set_data(timestamp_data,
posteriori_acceleration_y)
return actual, recorded, priori_plot, posteriori_plot, priori_acceleration_plot_x, posteriori_acceleration_plot_x, priori_acceleration_plot_y, posteriori_acceleration_plot_y,
else:
return actual, recorded, priori_plot, posteriori_plot,
ani = FuncAnimation(fig,
update,
frames=np.arange(1, len(coords), 2),
init_func=init,
blit=True,
interval=100)
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from kalman import Kalman
from scipy.stats import norm
## Parameters
theta = 10
A, G, Q, R = 1, 1, 0, 1
x_hat_0, Sigma_0 = 8, 1
## Initialize Kalman filter
kalman = Kalman(A, G, Q, R)
kalman.set_state(x_hat_0, Sigma_0)
N = 5
fig, ax = plt.subplots()
xgrid = np.linspace(theta - 5, theta + 2, 200)
for i in range(N):
# Record the current predicted mean and variance, and plot their densities
m, v = kalman.current_x_hat, kalman.current_Sigma
m, v = float(m), float(v)
ax.plot(xgrid,
norm.pdf(xgrid, loc=m, scale=np.sqrt(v)),
label=r'$t=%d$' % i)
# Generate the noisy signal
y = theta + norm.rvs(size=1)
# Update the Kalman filter
kalman.update(y)
ax.set_title(r'First %d densities when $\theta = %.1f$' % (N, theta))
ax.legend(loc='upper left')
plt.show()
# Initialization of state matrices
X = array([[0.03], [0.025], [0.032], [0.040]])
print(X.shape)
P = diag((0.01, 0.01, 0.01, 0.01))
A = array([[1, 0, dt , 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0,\
1]])
Q = eye(X.shape[0])
B = eye(X.shape[0])
U = zeros((X.shape[0], 1))
# Measurement matrices
Y = array([[X[0,0] + abs(random.randn(1)[0])], [X[1,0] +\
abs(random.randn(1)[0])]])
H = array([[1, 0, 0, 0], [0, 1, 0, 0]])
R = eye(Y.shape[0])
# Number of iterations in Kalman Filter
N_iter = 50
# Applying the Kalman Filter
ys = []
xs = []
kal = Kalman()
for i in arange(0, N_iter):
(X, P) = kal.kf_predict(X, P, A, Q, B, U)
(X, P, K, IM, IS, LH) = kal.kf_update(X, P, Y, H, R)
Y = array([[X[0,0] + abs(0.1 * random.randn(1)[0])],[X[1, 0] +\
abs(0.1 * random.randn(1)[0])]])
xs.append(Y[0])
ys.append(Y[1])
plt.plot(xs, ys, '--r')
plt.show()
ePrev = 0.0 # previous error
FIX = -12.89
ZLoop = True
#-------------System Initialization --------------------
bus = smbus.SMBus(1) #Init I2C module
now = time.time()
m = MOTOR.servo() # Start Servoblaster deamon and reset servos
wi_net = Server() # Start net service for remote control
wi_net.start()
time0 = now
sensor = MPU6050(bus, address, "MPU6050") #Init IMU module
sensor.read_raw_data() # Reads current data from the sensor
k = Kalman()
#=========================================================
def PID_L(current, target):
global eInteg
global ePrev
error = target - current
pid = KP * error + KI * eInteg + KD * (error - ePrev)
eInteg = eInteg + error
ePrev = error
return pid
#---------------------------------------------------------
figdistr, axis = pl.subplots(3, 1)
figdistr.tight_layout()
init_area = 5
s = 2
# Desired altitude and heading
alt_d = 4
q1.yaw_d = -np.pi
q2.yaw_d = np.pi/2
q3.yaw_d = 0
# Kalman filter for position estimation
kf = Kalman()
for t in time:
# Simulation
X = np.append(q1.xyz[0:2], np.append(q2.xyz[0:2], q3.xyz[0:2]))
V = np.append(q1.v_ned[0:2], np.append(q2.v_ned[0:2], q3.v_ned[0:2]))
U = fc.u_acc(X, V)
q1.set_a_2D_alt_lya(U[0:2], -alt_d)
q2.set_a_2D_alt_lya(U[2:4], -alt_d)
q3.set_a_2D_alt_lya(U[4:6], -alt_d)
# relative velocities!
v2_relative = q2.v_ned - q1.v_ned
v3_relative = q3.v_ned - q1.v_ned
image_folder_path = "./images/"
measurements_path = "./measurements/ball_20.csv"
dt = 0.1
u = 0 # Acceleration
std_acc = 0.001 # Q
std_pos = 0.5 # R
image_names = os.listdir(image_folder_path)
image_names.sort()
measurements = np.genfromtxt(measurements_path, delimiter=",")
kalman = Kalman()
kalman.init(dt, u, std_acc, std_pos)
for i in range(len(image_names)):
image = cv2.imread(image_folder_path + image_names[i])
measurement = measurements[i].astype(np.int)
# Given measurement color BLUE
cv2.rectangle(image, (measurement[0], measurement[1]),
(measurement[0]+measurement[2], measurement[1]+measurement[3]), (255, 0, 0), 2)
(x, y, vx, vy) = kalman.predict()
# Predicted state color GREEN
cv2.rectangle(image, (x, y),
(x + measurement[2], y + measurement[3]), (0, 255, 0), 2)
update = np.array([[measurement[0]+(measurement[2]/2)], [measurement[1]+(measurement[3]/2)]])
def main():
print('Program Started')
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
else:
print('Failed connecting to remote API server')
returnCode, leftMotor = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx_leftMotor", sim.simx_opmode_oneshot_wait)
returnCode, rightMotor = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx_rightMotor", sim.simx_opmode_oneshot_wait)
robot = Robot(clientID, 2)
returnCode, robotHandle = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx", sim.simx_opmode_oneshot_wait)
returnCode, sensorData = sim.simxGetStringSignal(clientID, "scanRanges",
sim.simx_opmode_streaming)
l_rot_prev, r_rot_prev = 0, 0
# Initial state and initial covariance matrix
prevPosition = np.matrix(getPosition(clientID, robotHandle)).T
odPrevPos = np.matrix(getPosition(clientID, robotHandle)).T
prevErrorPosition = np.zeros((3, 3))
odPrevError = np.zeros((3, 3))
a = 0
g = 0.3
count = 0
while sim.simxGetConnectionId(clientID) != -1:
v = np.zeros((2, 1))
# speedMotors = robot.breit_controller(clientID)
# sim.simxSetJointTargetVelocity(clientID, leftMotor, speedMotors[0], sim.simx_opmode_streaming)
# sim.simxSetJointTargetVelocity(clientID, rightMotor, speedMotors[1], sim.simx_opmode_streaming)
if count == 100:
# Dead reckoning
dPhiL, dPhiR, l_rot_prev, r_rot_prev = readOdometry(
clientID, leftMotor, rightMotor, l_rot_prev, r_rot_prev)
# Initialize Kalman
kalman_filter = Kalman(dPhiL, dPhiR)
odometry = Kalman(dPhiL, dPhiR)
predPosition, predError = kalman_filter.prediction(
prevPosition, prevErrorPosition)
truePos = np.matrix(getPosition(clientID, robotHandle)).T
odPosition, odError = odometry.prediction(odPrevPos, odPrevError)
odPosition[2] = truePos[2]
odPrevPos = odPosition
odPrevError = odError
# Observations
observedFeatures = readObservations(clientID)
x, y = lidar.arrangeData(observedFeatures)
lidarInputs, nLidarInputs = lidar.split_and_merge(x, y)
d_ant = 10000000000
S = np.zeros((2, 2))
H = np.zeros((2, 3))
predPosition[2] = truePos[2]
for i in range(nLidarInputs):
for j in range(len(mapInputs)):
y, S, H = kalman_filter.getInnovation(
predPosition, predError, mapInputs[j, :],
lidarInputs[:, i])
d = y.T @ np.linalg.pinv(S) @ y
if d < g**2 and d < d_ant:
# print("=============== MATCH ================\n")
v = y
d_ant = d
estPosition, estError, y, S = kalman_filter.update(
predPosition, predError, v, S, H)
prevPosition = estPosition
prevErrorPosition = estError
print(
f'Odometry position (x) - Estimated position (x) = {float(odPosition[0] - estPosition[0])}\n'
)
print(
f'Odometry position (y) - Estimated position (y) = {float(odPosition[1] - estPosition[1])}\n'
)
count = 0
count += 1
gx_data = df['gx'].as_matrix()
gy_data = df['gy'].as_matrix()
gz_data = df['gz'].as_matrix()
offset_num = 500
ax_offset = np.mean(ax_data[0:offset_num])
ay_offset = np.mean(ay_data[0:offset_num])
az_offset = np.mean(az_data[0:offset_num])
gx_offset = np.mean(gx_data[0:offset_num])
gy_offset = np.mean(gy_data[0:offset_num])
gz_offset = np.mean(gz_data[0:offset_num])
fi_kf = Kalman(F=np.array([[1, 1], [0, 1]]),
P = np.array([[10, 0], [0, 10]]),
Q = np.array([[0.001, 0], [0, 0.001]]),
R = np.array([[1000]]),
H = np.array([[1, 0]]),
x = np.array([[0], [0]]),
u = np.array([[0], [0]]))
theta_kf = Kalman(F=np.array([[1, 1], [0, 1]]),
P = np.array([[1000, 0], [0, 1000]]),
Q = np.array([[0.1, 0], [0, 0.1]]),
R = np.array([[10]]),
H = np.array([[1, 0]]),
x = np.array([[0], [0]]),
u = np.array([[0], [0]]))
should_init_kf = True
fi_raw_list = []
theta_raw_list = []
from ImageTracking.ClassifyImages import ClassifyImages
# import the calibration as well
import cv2
import numpy as np
import os
import time
import sys
from kalman import Kalman
from constants import *
# ---- Get the kalman setup ----- #
kalman_l = Kalman()
kalman_r = Kalman()
# -- some init values for the main script -- #
frame = 0
objectFoundL = False
objectFoundR = False
objectVisibleL = False
objectVisibleR = False
track_l = kalman_l.state
track_r = kalman_r.state
sensedLs = []
sensedRs = []
predictedLs = []
predictedRs = []
NaiveBayesL = np.ones(3)
NaiveBayesR = np.ones(3)
stereo = cv2.StereoSGBM_create(minDisparity=0,
numDisparities=96,
blockSize=9,
def kalman_smooth(Y, U, V, pi0, sigma0, A, B, C, D, Q, R, n_LF):
"""
Runs Kalman filtering and smoothing step of dynamic factor model estimator.
Arguments
-Y: N x T
Observations
-U: L x T
State controls
-V: P x T
Observation controls
-pi0: n_LF x 1
-sigma0: n_LF x n_LF
-A: n_LF x n_LF
-B: n_LF x L
-C: N x n_LF
-D: N x M
-Q: n_LF x n_LF
-R: N x N
Returns
-n_LF x T numpy array
Smoothed means
-T x n_LF x n_LF numpy array
Smoothed covariances
-T-1 x n_LF x n_LF numpy array
Smoothed lagged covariances (ie, cov[x_t, x_t-1])
"""
N, T = Y.shape
# Initialize Kalman filter
kf = Kalman(mu_0=pi0.copy(),
sigma_0=sigma0.copy(),
A=A,
B=B,
C=C,
D=D,
Q=Q,
R=R)
# sigma_t|t, mu_t|t
sigma_filt = np.zeros([T, n_LF, n_LF])
sigma_filt[0] = sigma0.copy()
mu_filt = np.zeros([T, n_LF])
mu_filt[0] = pi0.copy()
# sigma_t|t-1, mu_t|t-1. NOTE: indexed by t-1!!!
sigma_pred = np.zeros([T - 1, n_LF, n_LF])
mu_pred = np.zeros([T - 1, n_LF])
# Filtering step
for t in range(1, T):
### Prediction step
kf.predict(U[:, t])
# Save mu_t|t-1 and sigma_t|t-1
sigma_pred[t - 1, :, :] = kf.sigma
mu_pred[t - 1] = kf.mu
### Update step
kf.C = C.copy()
kf.D = D.copy()
kf.R = R.copy()
# Need to change observation, observation control and observation noise matrices if observation is
# incomplete! See Shumway and Stoffer page 314.
# First, figure out which sensors don't have observations:
nan_sensors = np.where(np.isnan(Y[:, t]))[0]
# If some observations were present, go through with the update step
if nan_sensors.size < N:
# Zero those observations
y_t = np.nan_to_num(Y[:, t])
# Modify parameters
kf.C[nan_sensors, :] = 0.0
kf.D[nan_sensors, :] = 0.0
kf.R[nan_sensors, :] = 0.0
kf.R[:, nan_sensors] = 0.0
kf.R[nan_sensors, nan_sensors] = 1.0
kf.update(y_t, V[:, t])
# Save filtered mean, covariance
sigma_filt[t, :, :] = kf.sigma
mu_filt[t] = kf.mu
# sigma_t|T, mu_t|T
sigma_smooth = np.zeros((T, n_LF, n_LF))
mu_smooth = np.zeros((T, n_LF))
# Initialize: sigma_T|T = sigma_T|T(filtered)
sigma_smooth[-1] = sigma_filt[-1]
# mu_T|T = mu_T|T(filtered)
mu_smooth[-1] = mu_filt[-1]
# Lagged covariance. Indexed by t-1.
sigma_lag_smooth = np.zeros((T - 1, n_LF, n_LF))
# sigmaLag_{T,T-1} = (1 - K_T C) A V_{T-1|T-1}, where K_T is Kalman gain at last timestep.
# TODO: unclear what to do here if last observation contains missing components
K_T = np.dot(sigma_pred[-1], np.dot(kf.C.T, np.linalg.pinv(np.dot(kf.C, \
np.dot(sigma_pred[-1], kf.C.T)) + kf.R)))
sigma_lag_smooth[-1] = np.dot(
np.dot((np.identity(n_LF) - np.dot(K_T, kf.C)), kf.A), sigma_filt[-2])
# Backwards Kalman gain
J = np.zeros((T - 1, n_LF, n_LF))
# Smoothing step. Runs from t=T-1 to t=0.
for t in range(T - 2, -1, -1):
# Backward Kalman gain matrix
J[t] = np.dot(np.dot(sigma_filt[t], kf.A.T),
np.linalg.pinv(sigma_pred[t]))
# Smoothed mean
mu_smooth[t] = mu_filt[t] + np.dot(J[t], mu_smooth[t + 1] - mu_pred[t])
# Smoothed covariance. This is explicity symmetric.
sigma_smooth[t, :, :] = sigma_filt[t] + np.dot(
np.dot(J[t], sigma_smooth[t + 1] - sigma_pred[t]), J[t].T)
# Lagged smoothed covariance (NOT SYMMETRIC!)
for t in range(T - 3, -1, -1):
sigma_lag_smooth[t, :, :] = np.dot(sigma_filt[t+1], J[t].T) \
+ np.dot(np.dot(J[t+1], (sigma_lag_smooth[t+1] - np.dot(kf.A, sigma_filt[t+1]))), J[t].T)
# Fill in missing Y values
Y_imp = Y.copy()
nanRows, nanCols = np.where(np.isnan(Y_imp))
for s, t in zip(nanRows, nanCols):
Y_imp[s,
t] = np.dot(C[s, :], mu_smooth[t, :]) + np.dot(D[s, :], V[:, t])
return mu_smooth.T, sigma_smooth, sigma_lag_smooth, Y_imp, sigma_filt
if cv2.waitKey(1) & 0xFF == ord('q'): # Stop the video feed
break
cap.release() # Release the camera
cv2.destroyAllWindows() # Destroy the window
if serial_flag: # If the arduino is connected close the serial communication
ard.close()
from kalman import Kalman
m = 0.02
R = 0.05
L = 0.6
J = 2 / 5 * m * R**2
d = 0.21
g = 10
control_constant = -(m * g * d / L) / ((J / R) + m)
B = np.array([[0], [control_constant]])
process_variance = np.array([[100, 0], [0, 100]])
measurement_variance = np.array([[0.03, 0], [0, 0.03]])
process_noise = np.array([[1, 0], [0, 1]])
kalman = Kalman(initial_condition=np.array([[20], [0]]),
control_matrix=B,
process_variance=process_variance,
process_noise=process_noise,
measurement_variance=measurement_variance)
for measurement, control in zip(measurement_list, control_list):
kalman.make_prediction_and_update(control, measurement)
plt.plot(control_list)
plt.plot(measurement_list)
# - Position and velocity states and estimates vs time
# - Estimation error and error covariance vs time
# - Kalman gains vs time
if __name__ == "__main__":
# UUV parameters
x0 = np.zeros([1, 2]) # initial states
duration = 50 # model test duration
dt = 0.05 # time step
R = np.array([[0.01, 0], [0, 0.0001]]) # process covariance
Q = np.array([0.001]) # measurement covariance
uuv = UUV(x0, duration, dt, R, Q) # UUV model object
# Kalman Filter Init
kalman = Kalman(uuv.A, uuv.B, uuv.C, uuv.R, uuv.Q)
# plot data containers
two_sig_v = np.zeros([int(duration / dt) + 1, 2]) # two-sigma velocity boundary
two_sig_x = np.zeros([int(duration / dt) + 1, 2]) # two-sigma position boundary
mu = np.zeros([int(duration / dt) + 1, 2]) # state estimation vector
K = np.zeros([int(duration / dt) + 1, 2]) # Kalman gain vector
# Input Command Simulation
F = np.zeros([int(duration / dt)])
for i in range(int(duration / dt)):
if i < int(5 / dt):
F[i] = 500
elif i < int(25 / dt):
F[i] = 0
elif i < int(30 / dt):
def kalmanSmooth(Y, pi0, sigma0, A, C, Q, R, nLF):
"""
Runs Kalman filtering and smoothing step of dynamic factor model estimator.
Returns
-nLF x T numpy array
Smoothed means
-T x nLF x nLF numpy array
Smoothed covariances
-T-1 x nLF x nLF numpy array
Smoothed lagged covariances (ie, cov[x_t, x_t-1])
"""
N, T = Y.shape
# Initialize Kalman filter
kf = Kalman(mu_0=pi0.copy(), sigma_0=sigma0.copy(), A=A, B=np.zeros(2*[2*nLF]), C=C, D=None, Q=Q, R=R)
# sigma_t|t, mu_t|t
sigma_filt = np.zeros([T, nLF, nLF])
sigma_filt[0] = sigma0.copy()
mu_filt = np.zeros([T, nLF])
mu_filt[0] = pi0.copy()
# sigma_t|t-1, mu_t|t-1. NOTE: indexed by t-1!!!
sigma_pred = np.zeros([T-1, nLF, nLF])
mu_pred = np.zeros([T-1, nLF])
# Avoid printing repetitive errors
#printedPosSemidefErr = False
# Filtering step
for t in range(1, T):
kf.predict()
# Save mu_t|t-1 and sigma_t|t-1
sigma_pred[t-1, :, :] = kf.sigma
mu_pred[t-1] = kf.mu
# Update if we have a measurement. Nans are handled by Kalman.
kf.update(Y[:, t])
# Save filtered mean, covariance
sigma_filt[t, :, :] = kf.sigma
mu_filt[t] = kf.mu
# Make sure filtered covariance is positive semidefinite!
#eigs, _ = np.linalg.eig(sigma_filt[t])
#if len(np.where(eigs < 0)[0]) > 0 and not printedPosSemidefErr:
# print "\tsigma_filt[%i] is not positive semidefinite"%t
# printedPosSemidefErr = True
# Make sure filtered covariance is symmetric (with REALLY loose tolerance...)
#if np.allclose(sigma_filt[t], sigma_filt[t].T, rtol=1e-4) == False:
# print "\tsigma_filt[%i] is not symmetric..."%t
# sigma_t|T, mu_t|T
sigma_smooth = np.zeros((T, nLF, nLF))
mu_smooth = np.zeros((T, nLF))
# Initialize: sigma_T|T = sigma_T|T(filtered)
sigma_smooth[-1] = sigma_filt[-1]
# mu_T|T = mu_T|T(filtered)
mu_smooth[-1] = mu_filt[-1]
# Lagged covariance. Indexed by t-1.
sigmaLag_smooth = np.zeros((T-1, nLF, nLF))
# sigmaLag_{T,T-1} = (1 - K_T C) A V_{T-1|T-1}, where K_T is Kalman gain at last timestep.
K_T = np.dot(sigma_pred[-1], np.dot(kf.C.T, np.linalg.pinv(np.dot(kf.C, \
np.dot(sigma_pred[-1], kf.C.T)) + kf.R)))
sigmaLag_smooth[-1] = np.dot(np.dot((np.identity(nLF) - np.dot(K_T, kf.C)), kf.A), sigma_filt[-2])
# Backwards Kalman gain
J = np.zeros((T-1, nLF, nLF))
# Smoothing step. Runs from t=T-1 to t=0.
for t in range(T-2, -1, -1):
# Backward Kalman gain matrix
J[t] = np.dot(np.dot(sigma_filt[t], kf.A.T), np.linalg.pinv(sigma_pred[t]))
# Smoothed mean
mu_smooth[t] = mu_filt[t] + np.dot(J[t], mu_smooth[t+1] - mu_pred[t])
# Smoothed covariance. This is explicity symmetric.
sigma_smooth[t, :, :] = sigma_filt[t] + np.dot(np.dot(J[t], sigma_smooth[t+1] - sigma_pred[t]), J[t].T)
# Lagged smoothed covariance (NOT SYMMETRIC!). Pretty sure this is correct...
for t in range(T-3, -1, -1):
sigmaLag_smooth[t, :, :] = np.dot(sigma_filt[t+1], J[t].T) \
+ np.dot(np.dot(J[t+1], (sigmaLag_smooth[t+1] - np.dot(kf.A, sigma_filt[t+1]))), J[t].T)
# Fill in missing Y values
YImp = Y.copy()
nanRows, nanCols = np.where(np.isnan(YImp))
for s, t in zip(nanRows, nanCols):
YImp[s, t] = np.dot(C[s, :], mu_smooth[t, :])
return mu_smooth.T, sigma_smooth, sigmaLag_smooth, YImp, sigma_filt
import time
import numpy as np
from icm20948 import ICM20948
from kalman import Kalman
import csv
imu = ICM20948()
sensorfusion = Kalman()
imu.read_sensors()
imu.computeOrientation()
sensorfusion.roll = imu.roll
sensorfusion.pitch = imu.pitch
sensorfusion.yaw = imu.yaw
currTime = time.time()
while True:
imu.read_sensors()
imu.computeOrientation()
print(
f"roll: {round(imu.roll,3)}, pitch: {round(imu.pitch,3)}, yaw: {round(imu.yaw,3)}"
)
newTime = time.time()
dt = newTime - currTime
currTime = newTime
sensorfusion.computeAndUpdateRollPitchYaw(
imu.accel_data[0], imu.accel_data[1], imu.accel_data[2],
