def FindBaselineSection(xi,xf,xAvg_,uAvg_):
"""
xi: starting index
xf: ending index (inclusive)
Finds the baseline for a section; returns the index for the end of the baseline
(last index in the baseline)
If there is no baseline to begin with the current index is returned
"""
dirn = klm.sign(xf-xi) # direction of change
ip = xi-dirn # previous index
ic = xi # current index
count = 0
while(klm.sign(ic-xf)!=dirn): # find out if we have finished
# Kalman filter to find baseline (with resets)
(xf1[ic], uf1[ic]) = klm.KalFilIter(xf1[ip],xAvg_*dirn,xm[ic], uf1[ip],uAvg_,um, 1,1,1)
# Kalman filtered baseline
#(xf2[ic], uf2[ic]) = klm.KalFilIter(xf2[ip],xAvg_*dirn,xf1[ic], uf2[ip],uAvg_,uf1[ic], 1,1,1)
(xf2[ic], uf2[ic]) = (xf1[ic], uf1[ic])
d_x2x2 = xf2[ic]-xf2[ip]
# increase count in direction of skew
dCount = klm.sign(d_x2x2-xAvg_*dirn)
if (dCount==-klm.sign(count)): # reset count if skew has changed
count = dCount
else:
count += dCount
print("~",ic,ip,count,d_x2x2,xm[ic],xf1[ic],xf2[ic])
if (abs(count)>=countTN):
return ic-countTN*dirn
ip = ic
ic += dirn
return ip
def __init__(self, dataFile, connection, SaveSensorData):
# Create an instance of the sensor object
self.sensor = ms5837.MS5837_30BA()
# Check if we were able to connect to the sensor
if (self.sensor._bus == None):
# There is no depth sensor connected
return None
else:
# A is the matrix we need to create that converts the last state to the new one, in our case it's just 1 because we get depth measurements directly
A = numpy.matrix([1])
# H is the matrix we need to create that converts sensors readings into state variables, since we get the readings directly from the sensor this is 1
H = numpy.matrix([1])
# B is the control matrix, since this is a 1D case and because we have no inputs that we can change we can set this to zero.
B = 0
# Q is the process covariance, since we want accurate values we set this to a very low value
Q = numpy.matrix([0.001])
# R is the measurement covariance, using a conservative estimate of 0.1 is fair
R = numpy.matrix([0.1])
# IC is the original prediction of the depth, setting this to normal room conditions makes sense
IC = self.currentDepth
# P is the initial prediction of the covariance, setting it to 1 is reasonable
P = numpy.matrix([1])
# We must initialize the sensor before reading it
self.sensor.init()
# Create the filter
self._filter = KalmanFilter(A, B, H, IC, P, Q, R)
# Create and start the process
p = Process(target=self.UpdateValue,
args=(connection, dataFile, SaveSensorData))
p.start()
class SinkKalmanFilter:
def __init__(self, sensor_id):
self.cache = []
self.sensor_id = sensor_id
self.readings = []
r_std, q_std = 1., 0.003
self.kf = KalmanFilter(dim_x=2, dim_z=1)
self.kf.x = np.array([[19], [0]]) # position, velocity
self.kf.F = np.array([[1, 1], [0, 1]])
self.kf.R = np.array([[r_std**2]]) # input noise
self.kf.H = np.array([[1., 0.]])
self.kf.P = np.diag([0.001**2, 0])
self.kf.Q = Q_discrete_white_noise(2, 1, q_std**2)
self.last_est = 0
def run(self, sensorReading):
est = []
est = self.kf.predict()
if sensorReading is None:
self.kf.update([self.last_est])
else:
self.kf.update([sensorReading])
self.last_est = est[0][0]
return est[0][0]
def getEstimates():
#This method goes through, grabs the polls and sends them through the Kalman fitler for each candidate and returns all the arrays of estimates as two dictionaries.
#First, we initialize our SQLite connection
cursor = conn.cursor()
#Next, we make sure that our polls are all up-to-date, and this also gives us a list of candidates for each party
dems, repubs = PollFetcher.fetchPolls()
demEstimates = {}
demUncertainties = {}
for d in dems:
#We go through every candidate and first get the poll results from the database
cursor.execute("SELECT EndDate, `%(cand)s`, Sample FROM DemPrimary;" %{"cand":d})
polls = cursor.fetchall()
#Next we send these polls through the KalmanFilter to get the estimates
dates, estimates, uncertainties = KalmanFilter.kalman(polls)
demEstimates[d] = [dates, estimates]
demUncertainties[d] = [dates, uncertainties]
#We now repeat the process for the Republicans
repubEstimates = {}
repubUncertainties = {}
for r in repubs:
cursor.execute("SELECT EndDate, `%(cand)s`, Sample FROM RepubPrimary;" %{"cand": r})
polls = cursor.fetchall()
dates, estimates, uncertainties = KalmanFilter.kalman(polls)
repubEstimates[r] = [dates, estimates]
repubUncertainties[r] = [dates, uncertainties]
#We now return our estimates and uncertainties
return demEstimates, demUncertainties, repubEstimates, repubUncertainties
def __init__(self, ip, port):
KalmanFinalAgent.__init__(self, ip, port)
self._initializePF()
self._initializeOcc()
self.kalmanFilter1 = KalmanFilter(self.NOISE)
self.kalmanFilter2 = KalmanFilter(self.NOISE)
class SubDepthSensor:
currentDepth = 0
# Relative to the MSL
def __init__(self, dataFile, connection, SaveSensorData):
# Create an instance of the sensor object
self.sensor = ms5837.MS5837_30BA()
# Check if we were able to connect to the sensor
if (self.sensor._bus == None):
# There is no depth sensor connected
return None
else:
# A is the matrix we need to create that converts the last state to the new one, in our case it's just 1 because we get depth measurements directly
A = numpy.matrix([1])
# H is the matrix we need to create that converts sensors readings into state variables, since we get the readings directly from the sensor this is 1
H = numpy.matrix([1])
# B is the control matrix, since this is a 1D case and because we have no inputs that we can change we can set this to zero.
B = 0
# Q is the process covariance, since we want accurate values we set this to a very low value
Q = numpy.matrix([0.001])
# R is the measurement covariance, using a conservative estimate of 0.1 is fair
R = numpy.matrix([0.1])
# IC is the original prediction of the depth, setting this to normal room conditions makes sense
IC = self.currentDepth
# P is the initial prediction of the covariance, setting it to 1 is reasonable
P = numpy.matrix([1])
# We must initialize the sensor before reading it
self.sensor.init()
# Create the filter
self._filter = KalmanFilter(A, B, H, IC, P, Q, R)
# Create and start the process
p = Process(target=self.UpdateValue,
args=(connection, dataFile, SaveSensorData))
p.start()
def UpdateValue(self, connection, dataFile, SaveSensorData):
# Create a method for the sensor to be turned off
readSensor = True
while (readSensor):
if (self.sensor.read()):
self._filter.Step(numpy.matrix([0]),
numpy.matrix([self.sensor.depth()]))
self.currentDepth = self._filter.GetCurrentState()[0, 0]
connection.send(self.currentDepth)
if (SaveSensorData):
dataFile.write(
str(time.time()) + "," + str(self.currentDepth) + "\n")
if (connection.poll()):
readSensor = False
dataFile.close()
class IMU(object):
DELAY_TIME = 0.02
GYRO_NOISE = 0.001 / 2
BIAS_NOISE = 0.003 / 2
ACCEL_NOISE = 0.01 / 2
def __init__(self, i2cBus=1):
self.gyro = Gyroscope(i2cBus, Gyroscope.DPS2000)
self.accel = Accelerometer(i2cBus, Accelerometer.SCALE_A_8G)
initOrientation = self.accel.getOrientation()
self.roll = initOrientation.roll
self.pitch = initOrientation.pitch
self.yaw = initOrientation.yaw
self.kalmanFilter = KalmanFilter(IMU.GYRO_NOISE, IMU.BIAS_NOISE, IMU.ACCEL_NOISE)
self.thread = threading.Thread(target=self.updateOrientation)
self.thread.daemon = True
self.thread.start()
def zero(self):
self.gyro.zero()
self.accel.zero()
def updateOrientation(self):
self.elapsedTime = IMU.DELAY_TIME
while True:
startTime = time.time()
gyroX, gyroY, gyroZ = self.gyro.getRawData()
accelOrientation = self.accel.getOrientation()
# kalman filter
self.roll = self.kalmanFilter.filterX(accelOrientation.roll, gyroX, self.elapsedTime)
self.pitch = self.kalmanFilter.filterY(accelOrientation.pitch, gyroY, self.elapsedTime)
# if abs(gyroX) > .5:
# self.roll = self.roll + (gyroX * self.elapsedTime)
# if abs(gyroY) > .5:
# self.pitch = self.pitch - (gyroY * self.elapsedTime)
# if abs(gyroZ) > .5:
# self.yaw = self.yaw + (gyroZ * self.elapsedTime)
# # complementary filter
# self.roll = self.roll * 0.95 + accelOrientation.roll * 0.05
# self.pitch = self.pitch * 0.95 + accelOrientation.pitch * 0.05
# # low pass filter
# self.roll = self.roll*.5 + accelOrientation.roll*.5
# self.pitch = self.pitch*.5 + accelOrientation.pitch*.5
# print (time.time() - startTime)
time.sleep(max(0, IMU.DELAY_TIME - (time.time() - startTime)))
self.elapsedTime = time.time() - startTime
def getOrientation(self):
# print self.roll, self.pitch, self.yaw
return Orientation(self.roll, self.pitch, self.yaw)
class IMU(object):
DELAY_TIME = .02
GYRO_NOISE = .001/2
BIAS_NOISE = .003/2
ACCEL_NOISE = .01/2
def __init__(self, i2cBus = 1):
self.gyro = Gyroscope(i2cBus, Gyroscope.DPS2000)
self.accel = Accelerometer(i2cBus, Accelerometer.SCALE_A_8G)
initOrientation = self.accel.getOrientation()
self.roll = initOrientation.roll
self.pitch = initOrientation.pitch
self.yaw = initOrientation.yaw
self.kalmanFilter = KalmanFilter(IMU.GYRO_NOISE, IMU.BIAS_NOISE, IMU.ACCEL_NOISE)
self.thread = threading.Thread(target=self.updateOrientation)
self.thread.daemon = True
self.thread.start()
def zero(self):
self.gyro.zero()
self.accel.zero()
def updateOrientation(self):
self.elapsedTime = IMU.DELAY_TIME
while True:
startTime = time.time()
gyroX, gyroY, gyroZ = self.gyro.getRawData()
accelOrientation = self.accel.getOrientation()
# kalman filter
self.roll = self.kalmanFilter.filterX(accelOrientation.roll, gyroX, self.elapsedTime)
self.pitch = self.kalmanFilter.filterY(accelOrientation.pitch, gyroY, self.elapsedTime)
# if abs(gyroX) > .5:
# self.roll = self.roll + (gyroX * self.elapsedTime)
# if abs(gyroY) > .5:
# self.pitch = self.pitch - (gyroY * self.elapsedTime)
# if abs(gyroZ) > .5:
# self.yaw = self.yaw + (gyroZ * self.elapsedTime)
# # complementary filter
# self.roll = self.roll * 0.95 + accelOrientation.roll * 0.05
# self.pitch = self.pitch * 0.95 + accelOrientation.pitch * 0.05
# # low pass filter
#self.roll = self.roll*.5 + accelOrientation.roll*.5
#self.pitch = self.pitch*.5 + accelOrientation.pitch*.5
#print (time.time() - startTime)
time.sleep(max(0, IMU.DELAY_TIME - (time.time() - startTime)))
self.elapsedTime = time.time() - startTime
def getOrientation(self):
#print self.roll, self.pitch, self.yaw
return Orientation(self.roll, self.pitch, self.yaw)
def __init__(self, i2cBus = 1):
self.gyro = Gyroscope(i2cBus, Gyroscope.DPS2000)
self.accel = Accelerometer(i2cBus, Accelerometer.SCALE_A_8G)
initOrientation = self.accel.getOrientation()
self.roll = initOrientation.roll
self.pitch = initOrientation.pitch
self.yaw = initOrientation.yaw
self.kalmanFilter = KalmanFilter(IMU.GYRO_NOISE, IMU.BIAS_NOISE, IMU.ACCEL_NOISE)
self.thread = threading.Thread(target=self.updateOrientation)
self.thread.daemon = True
self.thread.start()
def __init__(self, ip, port):
PFFinalAgent.__init__(self, ip, port)
# world details
self.SHOT_SPEED = float(self.constants['shotspeed'])
self.target1 = (0, 0, False)
self.target2 = (0, 0, False)
self.delta = 0.0
self.kalmanFilter1 = KalmanFilter(self.NOISE)
self.kalmanFilter2 = KalmanFilter(self.NOISE)
def __init__(self, ip, port):
PFAgent.__init__(self, ip, port)
# world details
self.SHOT_SPEED = float(self.constants['shotspeed'])
self.target = (0, 0, False)
self.delta = 0.0
self.kalmanFilter = KalmanFilter(self.NOISE)
#visualization init
init_window(self.worldHalfSize * 2, self.worldHalfSize * 2)
self.resetPredictionGrid()
def __init__(self, sensor_id):
self.cache = []
self.sensor_id = sensor_id
self.readings = []
r_std, q_std = 1., 0.003
self.kf = KalmanFilter(dim_x=2, dim_z=1)
self.kf.x = np.array([[19], [0]]) # position, velocity
self.kf.F = np.array([[1, 1], [0, 1]])
self.kf.R = np.array([[r_std**2]]) # input noise
self.kf.H = np.array([[1., 0.]])
self.kf.P = np.diag([0.001**2, 0])
self.kf.Q = Q_discrete_white_noise(2, 1, q_std**2)
self.last_est = 0
def drawData():
data = pd.read_csv('data_movie5.csv')
y = np.array(data.cm)
N = len(data.cm)
Ts = 0.02
t = data.time
x, P = kf.filter(y, N, Ts)
# Read video
video = cv2.VideoCapture("movie_movie5.mov")
# Exit if video not opened.
if not video.isOpened():
print("Could not open video")
sys.exit()
# Read first frame.
ok, frame = video.read()
if not ok:
print('Cannot read video file')
sys.exit()
counter = 0
n = 0
width = video.get(3) # float
while True:
# Read a new frame
ok, frame = video.read()
if not ok:
break
# Draw bounding box
if ok:
if (n == len(data.cm)):
break
if (counter > 445):
cm = -1 * ((x - 915) / (30 + np.abs(1 - x / 450)))
cv2.circle(frame, (int(x[n][0] * -(30) + 915), 300), 10,
(255, 0, 0), -1)
cv2.circle(frame, (int(data.cm[n] * -(30) + 915), 277), 10,
(0, 0, 255), -1)
n = n + 1
counter = counter + 1
else:
# Tracking failure
cv2.putText(frame, "Tracking failure detected", (100, 80),
cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)
# Display result
cv2.imshow("Tracking", frame)
# Exit if ESC pressed
k = cv2.waitKey(15) & 0xff
if k == 27: break
def simulator_setup(nbr_of_robots, start_positions):
# Create pose handlers
# subscribe pose handlers to Rosaria pose
# give correct offset if several
# Create KalmanFilters
# subscribe them to pose handlers
# Create robot handlers
# subscribe them to KalmanFilter
# make robots move towards their startPoint
robots = [RobotHandler(0), RobotHandler(1)]
for i in range(0, nbr_of_robots - 1):
offsetX = 0.0
offsetY = 0.0
if i == 1:
offsetY = 1.0
h = PoseHandler(i, offsetX, offsetY)
h_sub = rospy.Subscriber("RosAria" + str(i) + "/pose", Odometry,
h.measure)
kf = KalmanFilter(i)
kf_sub = rospy.Subscriber("Sensor" + str(i) + "/measurement", Odometry,
kf.new_measurement)
#robots[i] = RobotHandler(i)
r_sub = rospy.Subscriber("Filter" + str(i) + "/state", Odometry,
robots[i].update_state)
print len(robots)
return robots
def compare():
data = uf.ReadData('data_movie5', False)
y = np.array(data.cm)
t = np.array(data.time)
N = len(y)
Ts = 0.02
dataTruth = uf.ReadData('truth_movie5', False)
s_groundtruth = np.array(dataTruth.cm)
s_groundtruth = s_groundtruth + 0.6
v = np.diff(y)/Ts
a = np.diff(v)/Ts
v = np.insert(v, 0, 0)
a = np.insert(a, 0, 0)
a = np.insert(a, 0, 0)
#y = uf.preFilter(y)
#y = uf.preFilter2(y)
x, P = kalman.filter(y, N, Ts)
outdata = pd.DataFrame(columns=['time', 'truth', 'sensor', 'kalman'])
for i in range(len(t)):
outdata = outdata.append({'time': np.round(t[i] * 1000),
'truth': np.round(s_groundtruth[i], 4),
'sensor': np.round(y[i], 4),
'kalman': np.round(x[i, 0], 4)}, ignore_index = True)
outdata.to_csv('kalman_results_movie5.csv', index=False, sep=';')
uf.plot_filterresult(t, y, s_groundtruth, v, a, x, P)
def KF_demo_run(self):
model = self.model
truth_X = self.truth_X
meas_Z = self.meas_Z
estimated_X = self.estimated_X
m_update = self.m_update
P_update = self.P_update
targetstate = self.targetstate
for k in range(self.N_duration):
# simulate dynamic target state
# targetstate = model["F"].dot(targetstate) + np.sqrt(model["Q"]).dot(np.random.randn(targetstate.shape[0],targetstate.shape[1])) # generate ground truth for states of a target
W = model["sigma_v"] * model["B"].dot(np.random.randn(model["B"].shape[1], targetstate.shape[1])) # equivalent to 'np.sqrt(model["Q"]).dot(np.random.randn(targetstate.shape[0],targetstate.shape[1])) '
targetstate = model["F"].dot(targetstate) + W # generate ground truth for states of a target
truth_X.append(targetstate)
# simulate target measurement
# np.sqrt(model["R"]).dot(np.random.randn(model["R"].shape[1],1)) meas_zk = model["H"].dot(targetstate) + np.sqrt(model["R"]).dot(np.random.randn(model["R"].shape[1],1)) # generate measurement of a target
V = model["D"].dot(np.random.randn(model["D"].shape[1], targetstate.shape[1])) # equivalent to 'np.sqrt(model["R"]).dot(np.random.randn(model["R"].shape[1],1)) '
meas_zk = model["H"].dot(targetstate) + V # generate measurement of a target
meas_Z.append(meas_zk)
# Call KalmanFilter function
m_update, P_update = KalmanFilter(meas_zk, model, m_update, P_update)
estimated_X.append(m_update)
return truth_X, meas_Z, estimated_X
def initializeKalmanFilter():
A = np.array([[1, 0], [0, 1]])
B = np.array([[1, 0], [0, 1]])
Q = 20 * np.array([[1, 0], [0, 1]])
H = np.array([[1, 0], [0, 1]])
R = 10 * np.array([[1, 0], [0, 1]])
return kfilter.KalmanFilter(A, B, Q, H, R)
def __init__(self, gui=None, title=None):
super(FilterModule, self).__init__(gui=gui, title=title)
self.movingAverageModule = MovingAverageFilter(gui=gui,
mainModule=self)
self.kalmanFilterToy = KalmanFilterToy(gui=gui, mainModule=self)
self.kalmanFilter = KalmanFilter(gui=gui, mainModule=self)
self.font = ('Comic Sans MS', 11)
self.noisyImage = tk.PhotoImage(file="images/noisy_data.png")
def add_filter(self, id, site_name, filter_state=None):
if site_name not in self.filter_map[id].filters:
if filter_state is None:
self.filter_map[id].filters[
site_name] = KalmanFilter.KalmanFilter(
self.filter_map[id].configuration, site_name)
else:
self.filter_map[id].filters[site_name] = filter_state
def __init__(self, init_position, log):
'''
Initializes the parameters of the ship
- Parameters to calculate proper turn paths
- Initial sensor parameters for the control
- Initial start waypoint
- Navigation parameters (FollowDistance)
'''
self.Pos = init_position
self.retpos = self.Pos
self.NextSWP = self.Pos
self.Speed = 0
self.Sigma_max = 0.5
self.Kappa_max = 0.5
self.counter1 = 0
self.LastWP = 3
self.SWP = 0
self.SegmentCoords = 0
self.Current_SWP = 0
self.NextSWP_No = 0
self.NextSWP_validity = 0
self.FollowDistance = 2
self.v = 0
self.omega = 0
self.Theta = 0
self.x = numpy.matrix([[0], [0], [0]])
self.mark = 0
self.EndPath = 0
self.WPsEnded = 0
self.Filter = KF.Filter()
self.correction = 0
self.log = log
'''
The control matrices
'''
self.N = numpy.matrix([[1.2196566e+001, -2.2165773e-016],
[2.9639884e-017, 9.6263068e-001]])
self.F = numpy.matrix(
[[6.8421661e+000, -2.2165773e-016, -9.5435368e-017],
[2.9639884e-017, 9.6263068e-001, 1.4310741e+000]])
self.states = numpy.matrix([[0], [0], [0], [0], [0]])
def main():
data = readFile()
plotting(data)
inputValArray, Gyro_A = generateInput(data)
timestep = 0.02
wheelbase = 26
rob1 = myKalmanFilter.RobotEKF(dt=timestep, wheelbase=wheelbase,
std_acc=50.0, std_steer=np.radians(1))
X = rob1.x
# inputAcc = []
i = 0
moving = False
forwardSpeed = 0.0
speed = -20
direction = speed/abs(speed)
for inputV, gyroV in zip(inputValArray, Gyro_A):
print('Acc', inputV[0, 0]*direction)
rob1.predict(inputV)
if not moving and inputV[0, 0]*direction > 100:
forwardSpeed = speed
moving = True
print("Forward")
elif moving and inputV[0, 0]*direction < -100:
forwardSpeed = 0.0
moving = False
print("Brake")
# rob1.update(np.array([[0.0], [gyroV]]), R=np.matrix([[1**2, 0], [0, 0.0001**2]]))
rob1.update(np.array([[forwardSpeed], [gyroV]]),
R=np.matrix([[2**2, 0], [0, 100**2]]))
# else:
# rob1.update(np.array([[0.0], [gyroV]]), R=np.matrix([[1**2, 0], [0, 0.0001**2]]))
X_temp = rob1.x
X = np.concatenate((X, rob1.x), axis=1)
i += 1
plt.figure()
plt.title('Velocity')
plt.plot(X[2, :].tolist()[0], '--r')
plt.figure()
plt.title('Oriantation')
plt.plot(X[3, :].tolist()[0], '--r')
plt.figure()
plt.title('Angular velocity')
plt.plot(X[4, :].tolist()[0], '--r')
plt.plot(Gyro_A, 'g')
# print(Gyro_A)
plt.figure()
plt.title('Position')
plt.plot(X[0, :].tolist()[0], X[1, :].tolist()[0], '--r')
plt.show()
def __init__(self, init_position):
'''
Initializes the parameters of the ship
- Parameters to calculate proper turn paths
- Initial sensor parameters for the control
- Initial start waypoint
- Navigation parameters (FollowDistance)
'''
self.Pos = init_position
self.retpos = self.Pos
self.NextSWP = self.Pos
self.Speed = 0
self.Sigma_max = 0.3
self.Kappa_max = 0.3
self.counter1 = 0
self.LastWP = 3
self.SWP = 0
self.SegmentCoords = 0
self.Current_SWP = 0
self.NextSWP_No = 0
self.NextSWP_validity = 0
self.FollowDistance = 2
self.v = 0
self.omega = 0
self.Theta = 0
self.x = numpy.matrix([[0], [0], [0]])
self.mark = 0
self.EndPath = 0
self.WPsEnded = 0
self.Filter = KF.Filter()
self.correction = 0
'''
The control matrices
'''
self.N = numpy.matrix([[2.1366547e+001, 1.5569542e-016],
[8.3542070e-016, 2.2764131]])
self.F = numpy.matrix(
[[1.6012147e+001, 1.5569542e-016, 3.3824418e-016],
[8.3542070e-016, 2.2764131e+000, 2.2219859e+000]])
self.states = numpy.matrix([[0], [0], [0], [0], [0]])
def __init__(self, i2cBus = 1):
self.gyro = Gyroscope(i2cBus, Gyroscope.DPS2000)
self.accel = Accelerometer(i2cBus, Accelerometer.SCALE_A_8G)
initOrientation = self.accel.getOrientation()
self.roll = initOrientation.roll
self.pitch = initOrientation.pitch
self.yaw = initOrientation.yaw
self.kalmanFilter = KalmanFilter(IMU.GYRO_NOISE, IMU.BIAS_NOISE, IMU.ACCEL_NOISE)
self.thread = threading.Thread(target=self.updateOrientation)
self.thread.daemon = True
self.thread.start()
def runSimulation():
# Daten Erzeugung
#np.random.seed(1)
Ts = 0.02 #Abtastrate
N = 300 #Anzahl der Messungen
t = np.arange(N) * Ts #Zeitvektor
Q = 100000 #Varianz des Rucks
R = 0.68 #cm^2
a = np.arange(N)
a[0:50] = 100
a[50:60] = 50
a[60:70] = -50
a[70:120] = -200
a[120:140] = -50
a[140:150] = 50
a[150:210] = 100
a[210:220] = 50
a[220:230] = -50
a[230:280] = -100
a[280:290] = -50
a[290:300] = 50
b = np.sin(2 * np.pi * 0.25 * t) * 100
a = np.concatenate((a, b))
a = np.concatenate((b, a))
c = np.copy(a)
c = c * -1
a = np.concatenate((a, c))
a = np.concatenate((a, a))
N = a.size
r = np.random.randn(N) * np.sqrt(Q)
a = a + r.cumsum() * Ts
t, v, y, s_true = uf.generateData_CAM(a=a, Ts=Ts, N=N, R=R)
x, P = kalman.filter(y, N, Ts)
uf.plot_generatedData(t, y, s_true, v, a)
uf.plot_filterresult(t, y, s_true, v, a, x, P)
def real_world_setup(nbr_of_robots):
# Create UWBHandlers
# This is done from the commandline
# Create KalmanFilters
# Subscribe to UWBHandlers
# Create RobotHandlers
# Suscribe them to KalmanFilters
# Make them move towards start_positions
robots = []
for i in range(0, nbr_of_robots - 1):
kf = KalmanFilter(i)
kf_sub = rospy.Subscriber("Sensor" + str(i) + "/measurement", Odometry,
kf.new_measurement)
robots[i] = RobotHandler(i)
r_sub = rospy.Subscriber("Filter" + str(i) + "/state", Odometry,
robots[i].update_state)
return robots
def __init__(self, object_id, frame_number, indexes, H, pixelToMeters):
self.object_id = object_id
self.indexes = indexes
self.current_frame = frame_number
self.frames = [self.current_frame]
self.top_left = (min(self.indexes[1]), min(self.indexes[0]))
self.bottom_right = (max(self.indexes[1]), max(self.indexes[0]))
self.width = self.bottom_right[0] - self.top_left[0]
self.height = self.bottom_right[1] - self.top_left[1]
self.current_centroid = (sum(self.indexes[0]) / len(self.indexes[0]),
sum(self.indexes[1]) / len(self.indexes[1]))
self.centroids = [self.current_centroid]
self.kalman_filter = kf.KalmanFilter(self.object_id,
self.current_frame,
self.current_centroid)
self.found = True
self.speed = 40.0
self.speeds = [self.speed]
self.H = H
self.pixelToMeters = pixelToMeters
def __init__(self, id, obj_coord, nFrame):
self.id = id
self.nFrame = nFrame
self.visible = True
self.finished = False
minx = min(obj_coord[0])
maxx = max(obj_coord[0])
miny = min(obj_coord[1])
maxy = max(obj_coord[1])
self.bbox = np.array([minx, maxx, miny, maxy])
self.center = [(self.bbox[1]-self.bbox[0])/2+self.bbox[0], (self.bbox[3]-self.bbox[2])/2+self.bbox[2]]
self.posList = [self.center]
self.bboxList = [self.bbox]
self.framesList = [self.nFrame]
self.velocities = [50]
self.kalman = KalmanFilter(self.center)
class DepthSensor: currentDepth = 0; # Relative to the MSL self.sensor = ms5837.MS5837_30BA() # A is the matrix we need to create that converts the last state to the new one, in our case it's just 1 because we get depth measurements directly A = numpy.matrix([1]) # H is the matrix we need to create that converts sensors readings into state variables, since we get the readings directly from the sensor this is 1 H = numpy.matrix([1]) # B is the control matrix, since this is a 1D case and because we have no inputs that we can change we can set this to zero. B = 0 # Q is the process covariance, since we want accurate values we set this to a very low value Q = numpy.matrix([0.00001]) # R is the measurement covariance, using a conservative estimate of 0.1 is fair R = numpy.matrix([0.1]) # IC is the original prediction of the depth, setting this to normal room conditions makes sense IC = self.currentDepth # P is the initial prediction of the covariance, setting it to 1 is reasonable P = numpy.matrix([1]) # We must initialize the sensor before reading it self.sensor.init() # Create the filter self._filter = KalmanFilter(A,B,H,IC,P,Q,R)
(qVal[i]-qValsR[j-1])/(qValsR[j]-qValsR[j-1]))
qValsR.insert(j,qVals[i])
ppsE_T = [0]*len(ser_T) # expected time of serial arrival
covU_T = [0]*len(ser_T) # expected uncertainty
ardU_t = 0.5 # uncertainty in arduino times
#ardD_t = (ser_T[-1]-ser_T[0])/(len(ser_T)-1)-1000 # arduino drift per millisecond (from post-analysis)
ardD_t = 0
covU_T[0] = 50
ppsE_T[0] = ser_T[0]-ppsser_xR[qValsR.index(q_T[0])]
for i in range(len(ppsE_T)-1):
qValiCur = qValsR.index(q_T[i])
qValiNext = qValsR.index(q_T[1+i])
ppsE_T[1+i], covU_T[1+i] = klm.KalFilIter(ppsE_T[i], 1000+ardD_t-(ppsser_xR[qValiNext]-ppsser_xR[qValiCur]),
ser_T[1+i]-ppsser_xR[qValiNext], covU_T[i], ardU_t, ppsser_uR[qValiNext])
#print(1000+ardD_t-(ppsser_xR[qValiNext]-ppsser_xR[qValiCur]), (ser_T[1+i]-ppsser_xR[qValiNext])-(ser_T[i]-ppsser_xR[qValiCur]))
ppsser_dT = [ser_T[i]-pps_T[i] for i in range(len(ser_T))]
qValsN = [0]*len(qVals)
q_TN = [0]*len(q_T)
qMax = max(qVals)
qMin = min(qVals)
for i in range(len(qVals)):
qValsN[i] = (qVals[i]-qMin)/(qMax-qMin)
for i in range(len(q_T)):
q_TN[i] = (q_T[i]-qMin)/(qMax-qMin)
mplt.rcParams.update({'font.size': 15})
fig = plt.figure(figsize=(11,6))
import random
from KalmanFilter import *
from aisettings import *
kalman_filter = KalmanFilter(VAR, EST_VAR)
for i in range(5, 50):
num = random.randint(-10, 10)
kalman_filter.input_latest_noisy_measurement(num)
filtered_value = kalman_filter.get_latest_estimated_measurement()
print("Estimated:\t%s\t|\tActual:\t%s" % (num, filtered_value))
SystemCovNoise[1,1] = varDiffPrice
# Measurement error Covariance Matrix
# The covariance matrix of the measurement error observed at a given time t.
# We want to compute both price and return, so we obtain both
# therefore the noise measurement matrix is a 2x2 covariance matrix
MeasurCovNoise = np.eye(Nd) # Independent noise. This is not really true I think though. Can we check it ?
MeasurCovNoise[0,0] = varNoisePrice
MeasurCovNoise[1,1] = varNoiseDiff
######################################################################
############ USE THE KF !! #######
######################################################################
Ntst = 10
myKF = KF.KalmanFilter(A = A,B = B,C = C,
SystemCovNoise = SystemCovNoise, MeasurCovNoise = MeasurCovNoise)
Yhat,SigmaXXhatList,Ypred, SigmaXXpredList = myKF.fit(dates, Matrix_tr)
Ypredtest,SigmaXXpredtestList = myKF.predict(Ntst = Ntst)
######################################################################
## Extract the data to plot
RealPrice = Matrix_tr[:,[0]]
EstimatedPrice = Yhat[:,[0]]
PredictedPrice = Ypred[:,[0]]
sigmaXhatList = ul.fnp([ x[0,0] for x in SigmaXXhatList])
sigmaXpredList= ul.fnp([ x[0,0] for x in SigmaXXpredList])
sigmaXhatList = np.sqrt(sigmaXhatList )
SigmaXpredList = np.sqrt(sigmaXpredList)
sigmaYhatList = np.sqrt(sigmaXhatList**2 + MeasurCovNoise[0,0])
def range_of (self, pos):
""" returns tuple containing noisy range data to A and B
given a position 'pos'
"""
ra = sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)
rb = sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2)
return (ra + random.randn()*self.noise_factor,
rb + random.randn()*self.noise_factor)
pos_a = (100,-20)
pos_b = (-100, -20)
f1 = KalmanFilter(dim_x=4, dim_z=2)
f1.F = np.mat ([[0, 1, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
f1.B = 0.
f1.R *= 1.
f1.Q *= .1
f1.x = np.mat([1,0,1,0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
def KalBaseline(xm,up,um,A=1,B=1,H=1, countTY=5, countTN=10):
""" Input variables
returns (xf,uf)
x,u: filter variable and its uncertainty; -f,-m: filtered, measured; _: previous
A: mixing factor for last measurement contribution; B: mixing factor for dxp; H: convert between measurement, state
countTN: number of consecutive skewed values before the baseline is ended
countTY: minimum size of baseline
"""
""" Working variables
-t: temporary
d-: difference
"""
length = len(xm)
xf1 = [0]*length # Kalman filtered value, with resets
xf2 = [0]*length # Kalman filtered time 2nd order, with resets
xb = [0]*length # stores times for baseline
uf1 = [0]*length # uncertainty in filtered time
uf2 = [0]*length # uncertainty in filtered time
ub = [0]*length # uncertainty in baseline time
avg_x = (xm[-1]-xm[0])/(len(xm)-1) # average difference
avg_u = um/(len(xm)-1) # uncertainty in average value
# avg_x = 1000.52
# avg_u = 0
print(avg_x)
uf1[0] = um
uf2[0] = um
xf1[0] = xm[0]
xf2[0] = xm[0]
xb[0] = xf1[0]
ib = [] # stores list of indices[x,y] which form range
# of flat baselines (from x->y inclusive)
# cycle through data and use Kalman filter.
# Reset Kalman when there is a jump in the baseline (detected from consistent skew in filtered values)
# baseline takes average value for the period -- need to register start and end for region
# problem: not all fall into one region -- there are sometimes drifts between regions
jCur = 1
def FindBaselineSection(xi,xf,xAvg_,uAvg_):
"""
xi: starting index
xf: ending index (inclusive)
Finds the baseline for a section; returns the index for the end of the baseline
(last index in the baseline)
If there is no baseline to begin with the current index is returned
"""
dirn = klm.sign(xf-xi) # direction of change
ip = xi-dirn # previous index
ic = xi # current index
count = 0
while(klm.sign(ic-xf)!=dirn): # find out if we have finished
# Kalman filter to find baseline (with resets)
(xf1[ic], uf1[ic]) = klm.KalFilIter(xf1[ip],xAvg_*dirn,xm[ic], uf1[ip],uAvg_,um, 1,1,1)
# Kalman filtered baseline
#(xf2[ic], uf2[ic]) = klm.KalFilIter(xf2[ip],xAvg_*dirn,xf1[ic], uf2[ip],uAvg_,uf1[ic], 1,1,1)
(xf2[ic], uf2[ic]) = (xf1[ic], uf1[ic])
d_x2x2 = xf2[ic]-xf2[ip]
# increase count in direction of skew
dCount = klm.sign(d_x2x2-xAvg_*dirn)
if (dCount==-klm.sign(count)): # reset count if skew has changed
count = dCount
else:
count += dCount
print("~",ic,ip,count,d_x2x2,xm[ic],xf1[ic],xf2[ic])
if (abs(count)>=countTN):
return ic-countTN*dirn
ip = ic
ic += dirn
return ip
while(True):
print("jCur",jCur)
jPrev = jCur
xf1[jCur-1] = (xm[jCur]-avg_x+xm[jCur-1])/2 # set the previous filtered value to avg of last
xf2[jCur-1] = xf1[jCur-1]
print("set",jCur-1,xf1[jCur-1])
jCur = FindBaselineSection(jCur, length-1, avg_x, avg_u)# find index of end of current baseline
print("FB",jCur,jPrev)
if (jCur>jPrev+1): # check whether there a baseline
continueOn = True
ib.append([0,jCur]) # add the region to list
if (len(ib)>1): # check whether there is a previous region
ib[-1][0] = ib[-2][1]+1 # use previous region to bound the current baseline
avg_x_ = (xf2[ib[-1][1]]-xf2[ib[-1][0]])/(ib[-1][1]-ib[-1][0])
d_xf2_ = [xf2[i+1]-xf2[i] for i in range(ib[-1][0],ib[-1][1])]
avg_u_ = np.std(d_xf2_)
(avg_x_,avg_u_) = klm.KalFilIter(avg_x,0,avg_x_,avg_u,avg_u,avg_u_,1,1,1)
print("averages",avg_x,avg_u,";",avg_x_,avg_u_)
jPrev = FindBaselineSection(jCur-1,ib[-1][0], avg_x_, avg_u) # work backwards to find start of region
print("FB",jPrev)
if (jCur-jPrev<countTY):
ib = ib[:-1]
continueOn = False
if(continueOn):
for i_ in range(ib[-1][0],ib[-1][1]+1,1): # assign baseline values
xb[i_] = xf2[i_]
ub[i_] = uf2[i_]
avg_x_ = (xb[ib[-1][1]]-xb[ib[-1][0]])/(ib[-1][1]-ib[-1][0])
d_xb_ = [xb[i+1]-xb[i] for i in range(ib[-1][0],ib[-1][1])]
avg_u_ = np.std(d_xb_)
(avg_x,avg_u) = klm.KalFilIter(avg_x,0,avg_x_,avg_u,avg_u,avg_u_,1,1,1)
print("averages",avg_x,avg_u)
print("ib:",ib[-1])
#jCur += 1 # add one if we have the end of a baseline
jCur += 2
if (jCur >= length):
return (xb,ub,ib,xf1,xf2)
InitialStateError = 9999999999999.0
Proc_Noise_Sigma = 0.002
B = np.matrix("0")
H = np.matrix("1.0 0 1.0")
x_init = np.matrix("0 ; 0; 350.0")
P = np.matrix("{0} 0 0 ; 0 {0} 0 ; 0 0 {0}".format(InitialStateError))
Q = Proc_Noise_Sigma**2 * np.matrix("{0} {1} 0; {2} {3} 0 ; 0 0 0".format(Q00, Q10, Q01, Q11))
R = np.matrix(10.0)
N = 3
print("Constructing Filter")
kf = KalmanFilter.CKalmanFilter(N, np.array(A), np.array(P), np.array(Q), np.array(H).flatten(), np.array(x_init).flatten(), float(R))
print("Executing Filtering")
start_time = timeit.default_timer()
ResultData = kf.FilterData(data.signal, len(data.signal))
elapsed = timeit.default_timer() - start_time
print("C++ filtering took {}s".format(elapsed))
print("Constructing Python Filter")
KF2 = KalmanFilterPurePython.KalmanFilterLinear(A, B, H, x_init, P, Q, R)
print("Executing Python Filtering")
start_time = timeit.default_timer()
import matplotlib.pyplot as plt import KalmanFilter as kf import data.fakeData as fakeData if __name__ == '__main__': accelerations = fakeData.generateCurve((3, 1000)) true_velocity, noisy_velocity = fakeData.genVelocity((3, 1000)) dimension = 3 A = np.identity(dimension) P = np.identity(dimension) kf = kf.KalmanFilter(A, P, dimension) Q = np.zeros((3, 3)) ### TODO ### i = 0 state = [] while(i < len(noisy_velocity[0])): kf.predictState(accelerations[:,i], 1, Q) kf.getKalmanGain() kf.update(noisy_velocity[:,1]) i += 1 state.append(kf.v_k) state = np.transpose(np.array(state)) for x in range(dimension): plt.plot(state[x]) plt.show()
self._auto_run()
else:
self._manual_run()
if __name__ == '__main__':
print 'Testing FilterManager...'
origin_pos = np.array([200, 0, np.pi/2])
origin_cov = np.eye(3) * 3 # Initial Covariance
origin_cov[2,2] = 0.02
# Need to call this or gtk will never release locks
gobject.threads_init()
fm = FilterManager()
run_kf = False
run_pf = False
#run_kf = True
run_pf = True
if run_kf:
fm.add_filter(KalmanFilter.KalmanFilter(origin_pos, origin_cov))
if run_pf:
fm.add_filter(ParticleFilter.ParticleFilter(origin_pos, origin_cov))
tg = TestGUI(fm.get_draw())
#tt = TestThread(fm, tg, auto=True)
tt = TestThread(fm, tg, auto=False)
tt.start()
tg.mainloop()
tt.quit = True
