def update_c_in_us(c_in_us):
dht_sensor = Adafruit_DHT.DHT11
dht_pin = 14
initialized = False
while True:
relative_humidity, temperature = Adafruit_DHT.read_retry(dht_sensor, dht_pin)
if relative_humidity is not None and temperature is not None:
c = speed_of_sound.calculate_c(temperature, relative_humidity) / 1e6
if not initialized:
x_est, error_est = kalman.kalman(c, c, 0.1, 0.05, 0.001)
initialized = True
else:
x_est, error_est = kalman.kalman(c, x_est, error_est, 0.05, 0.001)
c_in_us.value = x_est
print('Temp=%.1fC Humidity=%.1f%% Mach 1=%.1fm/s ' % (temperature, relative_humidity, c_in_us.value * 1e6))
time.sleep(2)
def kalman_average(cls, price_series):
"""Invoking kalman algorithm that builds improved version of moving average.
It accepts Series object with the price series.
It returns Series of averages for the period."""
obs = np.array([price_series.values])
steps = len(price_series)
result = ka.kalman(obs, nsteps=steps)
result = pd.Series(result)
return result
def __init__(self):
Thread.__init__(self)
self.time_start = time.time()
################################################
# Sensors
#######################
self.acc = lsm303d()
self.bar = lps25h()
self.gps = ls20031()
self.gyr = l3gd20h()
#######################
self.gyr.set_sample_rate()
################################################
################################################
# Kalman filter and temporal integration of gyro
##################
self.dt = 0.
self.g = zeros(3)
# Grav vector
self.g0 = zeros(3)
# Initial grav vector
self.Rtheta0 = 0.
self.Rphi0 = 0.
self.kalman = [kalman() for _ in range(3)]
#################
self.d_gravity_threashold = 0.02
# In case that the deviation of the magnitude
# of acceleration vector is greater than this,
# the data from acclerometer will not
# contribute to the Kalman filter.
################################################
################################################
# V/S calculation
#####################
self.alti_old = 0.
self.alti_old_t = 0.
################################################
################################################
# Deg <---> Rad conversions
###########################
self.rad_to_deg = 180 / pi
self.deg_to_rad = pi / 180
################################################
################################################
# Boolean flags
###########################
self.is_calibrated = False
self.is_to_break = False
################################################
return
def __init__( self ):
Thread.__init__( self );
self.time_start = time.time( );
################################################
# Sensors
#######################
self.acc = lsm303d( );
self.bar = lps25h ( );
self.gps = ls20031( );
self.gyr = l3gd20h( );
#######################
self.gyr.set_sample_rate( );
################################################
################################################
# Kalman filter and temporal integration of gyro
##################
self.dt = 0.;
self.g = zeros( 3 ); # Grav vector
self.g0 = zeros( 3 ); # Initial grav vector
self.Rtheta0 = 0.;
self.Rphi0 = 0.;
self.kalman = [ kalman( ) for _ in range( 3 ) ];
#################
self.d_gravity_threashold = 0.02;
# In case that the deviation of the magnitude
# of acceleration vector is greater than this,
# the data from acclerometer will not
# contribute to the Kalman filter.
################################################
################################################
# V/S calculation
#####################
self.alti_old = 0.;
self.alti_old_t = 0.;
################################################
################################################
# Deg <---> Rad conversions
###########################
self.rad_to_deg = 180 / pi;
self.deg_to_rad = pi / 180;
################################################
################################################
# Boolean flags
###########################
self.is_calibrated = False;
self.is_to_break = False;
################################################
return;
def gps_only_retro(gps_filename):
# Load data from csv file.
sensors = SensorData(gps_filename=gps_filename)
# Plot the GPS data
plt.scatter(sensors.gps.x, sensors.gps.y, c='r', edgecolors="none")
# Run the Kalman filter
output_matrix = kalman.kalman(sensors)
# # Plot the Kalman output
plt.scatter([output_matrix[:, 0]], [output_matrix[:, 1]],
edgecolors="none")
# Show everything
plt.show()
def __kalman_c(self, x, P, measurement, R, Q):
"""
Parameters:
x: initial state of coefficients (c0, c1)
P: initial uncertainty convariance matrix
measurement: line fit coefficients
R: line fit errors
motion: external motion added to state vector x
Q: motion noise (same shape as P)
"""
motion = np.matrix(np.zeros(self.order)).T
return kalman(x,
P,
measurement,
R,
motion,
Q,
F=np.matrix(np.matrix(np.eye(self.order))),
H=np.matrix(np.matrix(np.eye(self.order))))
def __init__(self, diameter, color, circular_contour):
self.radius = diameter / 2
self.color = color
self.contour = circular_contour
self.thresh_rbg = np.load('threshholds_ball.npy')
self.thresh = []
self.kalman = kalman()
if color is 'red':
self.thresh.extend(([[self.thresh_rbg[0][0][0]], [self.thresh_rbg[0][0][1]]], [[self.thresh_rbg[1][0][0]], [self.thresh_rbg[1][0][1]]]))
elif color is 'green':
self.thresh.extend(([[[self.thresh_rbg[0][1][0]], [self.thresh_rbg[0][1][1]]]]))
elif color is 'blue':
self.thresh.extend(([[[self.thresh_rbg[0][2][0]], [self.thresh_rbg[0][2][1]]]]))
else:
print('Ball.__init__() failed: Wrong Color: Use red, green or blue')
self.center_m = None
self.center_r = None
self.radius_image = None
self.position = None
self.visible = False
self.moving = False
self.last_positions = deque(maxlen=5)
import time
import RPi.GPIO as GPIO
import datetime as dt
import math
import sys
from multiprocessing import Process, Value, Array
import Adafruit_DHT
import speed_of_sound
import kalman
x_est, error_est = kalman.kalman(0.02, 0.02, 0.01, 0.005, 0.0001)
c_in_us = Value('d', speed_of_sound.calculate_c(22, 50) / 1e6)
def update_c_in_us(c_in_us):
dht_sensor = Adafruit_DHT.DHT11
dht_pin = 14
initialized = False
while True:
relative_humidity, temperature = Adafruit_DHT.read_retry(dht_sensor, dht_pin)
if relative_humidity is not None and temperature is not None:
c = speed_of_sound.calculate_c(temperature, relative_humidity) / 1e6
if not initialized:
x_est, error_est = kalman.kalman(c, c, 0.1, 0.05, 0.001)
initialized = True
else:
x_est, error_est = kalman.kalman(c, x_est, error_est, 0.05, 0.001)
c_in_us.value = x_est
import kalman import matplotlib.pyplot as plt from numpy import * from scipy.integrate import odeint from scipy.stats import norm Ts = 0.2 process_sigmasq = 1e-2 measurement_sigmasq = 0.5 filt = kalman.kalman( x=array([[0.0, 0.0, 0.0, 0.0, 1.0]]).T, P=2 * eye(5), ) class SineProcess: def __init__(self): self.stepsused = [] def F(self, state): x, _, y, _, omega = ravel(state) vd = -(omega**2) * Ts vw = -2 * omega * Ts return array([ [1.0, Ts, 0.0, 0.0, 0.0 ], [ vd, 1.0, 0.0, 0.0, vw * x], [0.0, 0.0, 1.0, Ts, 0.0 ], [0.0, 0.0, vd, 1.0, vw * y], [0.0, 0.0, 0.0, 0.0, 1.0 ]]) def integrate(self, coeff, *init):
sys.f(
Matrix(
[x[1],
(l_1_ * f(x[0],x[2]) - l_1_ * m * g * cos(x[0] + theta_) - (
k_f * x[1])) / (I + l_1_**2 * m_1),
A * r * (rho_e * u[0] - h(x[0],x[2]) * u[1])
])
)
sys.h(Matrix([x[0]]))
# Linearization!!!
A,B,C = sys.linearize(Matrix([0.02824, 0, 0.13233]), Matrix([5.0, 4.46]))
print "A=\n", A
print "B=\n", B
print "C=\n", C
from sympy import I
K = luenberger(A, C, [-10 + 4 * I, -10 - 4 * I, -10])
print "K = \n", K
V_2 = Matrix([1e-2])
V_1 = kalman(A, C, K, V_2)
print "V_1 = \n", V_1
print "V_2 = \n", V_2
def camProcess():
# brocker ip address (this brokeris running inside our aws server)
broker_address = "broker.mqttdashboard.com"
print("creating new instance")
# client Name
client = mqtt.Client("Platform_PC",
transport='websockets') #create new instance
client.on_message = on_message # attach function to callback
print("connecting to broker")
client.connect(broker_address, 8000, 60) #connect to broker
# starting the mqtt client loop
client.loop_start()
# subscribing to the current postion topic
client.subscribe("swarm/{}/currentPos".format(SWARM_ID))
client.subscribe(TOPIC_COM)
client.subscribe(TOPIC_SEVER_COM)
print("Publishing message to topic",
"swarm/{}/currentPos".format(SWARM_ID))
# destination point
# desX = 400
# desY = 50
global sharedData, broadcastPos
print("Cam Process Started")
while True:
ret, frame = cam.read()
# Detect the markers in the image8
markerCorners, markerIds, rejectedCandidates = cv2.aruco.detectMarkers(
frame, dictionary, parameters=parameters)
# current marker id set
markerSet = set()
for i in range(len(markerCorners)):
# ploting rectangles around the markers
pts = np.array([
markerCorners[i][0][0], markerCorners[i][0][1],
markerCorners[i][0][2], markerCorners[i][0][3]
], np.int32)
pts = pts.reshape((-1, 1, 2))
cv2.polylines(frame, [pts], True, (0, 255, 255))
# converting to center point
conData = convert(markerCorners[i][0])
frame = cv2.circle(frame, tuple(conData[0]), 1, (255, 0, 0), 2)
frame = cv2.circle(frame, tuple(markerCorners[i][0][1]), 1,
(0, 255, 0), 2)
frame = cv2.circle(frame, tuple(markerCorners[i][0][2]), 1,
(0, 0, 255), 2)
# add to the marker id set
markerSet.add(markerIds[i][0])
# adding data to the dictionary
if (markerIds[i][0] in robotData):
if kalmanEn:
# adding data to the kalman obj
k_obj = robotData[markerIds[i][0]][
2] # grabbing the kalman object
kalVal = k_obj(conData[0], conData[1][0], conData[1][1],
True) # calculating the kalman value
else:
if kalmanEn:
# add new key to the set
robotDataSet.add(markerIds[i][0])
# adding data to the kalman algo
k_obj = kalman(conData[0], conData[1][0], conData[1][1])
robotData[markerIds[i][0]][
2] = k_obj # adding kalman object to the array
# creating the kalman object
robotData[markerIds[i][0]] = [
0, 0, 0, 0, conData[0], True
] # [center_point,top_two_cord,kalman,top_two_cord,destination,idle]
# adding data to the dictionary
robotData[markerIds[i][0]][0] = conData[0]
robotData[markerIds[i][0]][1] = conData[1]
robotData[markerIds[i][0]][3] = [
tuple(markerCorners[i][0][1]),
tuple(markerCorners[i][0][2])
]
# adding data to be broadcasted
# TODO Changed
# broadcastPos[int(markerIds[i][0])] = positions(conData[0], [tuple(markerCorners[i][0][1]), tuple(markerCorners[i][0][2])], [desX,desY], 0)
if kalmanEn:
# updating the not detected objects through kalman algo
differentSet = robotDataSet - markerSet
for id in differentSet:
k_obj = robotData[id][2] # grabbing the kalman object
k_obj([0, 0], [0, 0], [0, 0],
False) # calculating the kalman value
kalVal = k_obj.x.transpose()
# setting data to the dataset
robotData[id][0] = [kalVal[0], kalVal[1]]
robotData[id][1] = [[kalVal[2], kalVal[3]],
[kalVal[4], kalVal[5]]]
# adding data to be broadcasted
broadcastPos[id] = positions(
[kalVal[0], kalVal[1]],
[[kalVal[2], kalVal[3]], [kalVal[4], kalVal[5]]],
[desX, desY], 0)
# calculate destinations
broadcastPos = destinationCalculation(robotData, broadcastPos, frame,
client)
try:
# print(broadcastPos[1][0], desX, desY)
# add destination
if (19 in robotData and True):
desX = robotData[19][0][0]
desY = robotData[19][0][1]
except:
pass
#print(jsonEncodedData)
# addig to the shared variable
sharedData[0] = broadcastPos
if cv2WindowEn:
cv2.imshow('Cam', frame)
ret, buffer = cv2.imencode('.jpg', frame)
img = buffer.tobytes()
sharedData[1] = img
#saving to the file
#outVid.write(frame)
if (cv2.waitKey(1) == ord('q')):
break
cam.release()
#outVid.release()
cv2.destroyAllWindows()
process = kalman.process_model(F=array([
[1, dt],
[0, 1],
]),
Q=array([
[(dt**4) / 4, (dt**3) / 2],
[(dt**3) / 2, dt**2],
]) * (accel_sigma**2))
measure = kalman.observation_model(
H=[[1.0, 0.0]],
R=[[measurement_sigma**2]],
)
filt = kalman.kalman(
x=zeros((2, 1)),
P=zeros((2, 2)),
)
accel_rv = norm(0, accel_sigma)
measurement_rv = norm(0, measurement_sigma)
accelerations = (accel_rv.rvs() for _ in xrange(1000))
true_positions = []
true_velocities = []
estimated_positions = []
estimated_velocities = []
true_position = 0.0
true_velocity = 0.0
for acceleration in accelerations:
filt.predict(process)
[1, dt],
[0, 1],
]),
Q=array([
[(dt**4)/4, (dt**3)/2],
[(dt**3)/2, dt**2],
]) * (accel_sigma ** 2)
)
measure = kalman.observation_model(
H=[[1.0, 0.0]],
R=[[measurement_sigma ** 2]],
)
filt = kalman.kalman(
x=zeros((2,1)),
P=zeros((2,2)),
)
accel_rv = norm(0, accel_sigma)
measurement_rv = norm(0, measurement_sigma)
accelerations = (accel_rv.rvs() for _ in xrange(1000))
true_positions = []
true_velocities = []
estimated_positions = []
estimated_velocities = []
true_position = 0.0
true_velocity = 0.0
for acceleration in accelerations:
filt.predict(process)
def camProcess():
# destination point
desX = 400
desY = 50
global sharedData
print("Cam Process Started")
while True:
ret, frame = cam.read()
# Detect the markers in the image8
markerCorners, markerIds, rejectedCandidates = cv2.aruco.detectMarkers(frame, dictionary, parameters=parameters)
# current marker id set
markerSet = set()
for i in range(len(markerCorners)):
# ploting rectangles around the markers
pts = np.array([markerCorners[i][0][0],markerCorners[i][0][1],markerCorners[i][0][2],markerCorners[i][0][3]], np.int32)
pts = pts.reshape((-1,1,2))
cv2.polylines(frame,[pts],True,(0,255,255))
# converting to center point
conData = convert(markerCorners[i][0])
frame = cv2.circle(frame, tuple(conData[0]), 1, (255,0,0), 2)
frame = cv2.circle(frame, tuple(markerCorners[i][0][1]), 1, (0,255,0), 2)
frame = cv2.circle(frame, tuple(markerCorners[i][0][2]), 1, (0,0,255), 2)
# add to the marker id set
markerSet.add(markerIds[i][0])
# adding data to the dictionary
if (markerIds[i][0] in robotData):
robotData[markerIds[i][0]][0] = conData[0]
robotData[markerIds[i][0]][1] = conData[1]
# adding data to the kalman obj
k_obj = robotData[markerIds[i][0]][2] # grabbing the kalman object
kalVal = k_obj(conData[0], conData[1][0] , conData[1][1], True) # calculating the kalman value
else:
# add new key to the set
robotDataSet.add(markerIds[i][0])
k_obj = kalman(conData[0], conData[1][0], conData[1][1]) # creating the kalman object
robotData[markerIds[i][0]] = [0,0,0,0]
robotData[markerIds[i][0]][0] = conData[0]
robotData[markerIds[i][0]][1] = conData[1]
robotData[markerIds[i][0]][2] = k_obj # adding kalman object to the array
# adding data to be broadcasted
# TODO Changed
broadcastPos[int(markerIds[i][0])] = positions(conData[0], [tuple(markerCorners[i][0][1]), tuple(markerCorners[i][0][2])], [desX,desY], 0)
# updating the not detected objects through kalman algo
differentSet = robotDataSet - markerSet
for id in differentSet:
k_obj = robotData[id][2] # grabbing the kalman object
k_obj([0,0],[0,0],[0,0],False) # calculating the kalman value
kalVal = k_obj.x.transpose()
# setting data to the dataset
robotData[id][0] = [kalVal[0], kalVal[1]]
robotData[id][1] = [[kalVal[2], kalVal[3]] , [kalVal[4], kalVal[5]]]
# adding data to be broadcasted
#broadcastPos[id] = positions([kalVal[0], kalVal[1]], [[kalVal[2], kalVal[3]] , [kalVal[4], kalVal[5]]], [desX, desY], 0)
# print(robotData)
try:
# print(broadcastPos[1][0], desX, desY)
# add destination
if (19 in robotData and True):
desX = robotData[19][0][0]
desY = robotData[19][0][1]
except:
pass
#print(jsonEncodedData)
# addig to the shared variable
sharedData[0] = broadcastPos
cv2.imshow('Cam', frame)
#saving to the file
#outVid.write(frame)
if (cv2.waitKey(1) == ord('q')):
break
cam.release()
#outVid.release()
cv2.destroyAllWindows()
# LINEAIR
# Y-axis: Depth (how far away is the ball)
# determined by the difference in x-values of the camera's
# LINEAIR
# Z-axis: Vertical Height (How high is the ball above the ground),
# determined by the y-values
# PARABOLEA
if xo is not None:
list3dX.append(xo)
list3dY.append(yo)
list3dZ.append(zo)
print(xo, yo, zo)
######## KALMAN FILTER Position #########
mu, P, pred = kalman(mu, P, F, Q, B, a, np.array([xo, yo, zo]), H, R)
# Storing of matrices
res = [(mu, P)]
xe = [mu[0] for mu, _ in res] # Estimated position
ye = [mu[1] for mu, _ in res]
ze = [mu[2] for mu, _ in res]
xu = [2 * np.sqrt(P[0, 0])
for _, P in res] # uncertainty of estimated position
yu = [2 * np.sqrt(P[1, 1])
for _, P in res] # uncertainty of estimated position
zu = [2 * np.sqrt(P[1, 1])
for _, P in res] # uncertainty of estimated position
# Storing of estimations
list3dXe.append(xe[0])
import sys import kalman from numpy import * dt = 1.0 / 2500.0 process_sigmasq = 3.0 measurement_sigmasq = 1.0 t = kalman.kalman( x=zeros((9,1)), P=array([ [10., 0.0, 0.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], [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 10., 0.0, 0.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, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10., 0.0, 0.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, 0.0, 0.0, 1.0]]), ) process = kalman.process_model( F=array([ [1.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, dt, 0.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, 0.0, 0.0, 1.0, dt, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 1.0, dt, 0.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, 0.0, 0.0, 1.0, dt, 0.0],
from matplotlib import pyplot as plt
plt.figure()
# plt.xlim((-100,100))
# plt.ylim((-100,100))
A = np.array([[1, 1], [0, 1]])
B = np.array([[0.5], [1]])
C = np.array([[1, 0]])
# process noise
w = 1
#measurement noise
v = 3
#v = np.array([[1e-4, 1e-5],[1e-4,1e-5]])
kal = kalman(A, B, C, dim=1, w=w, v=v)
u = 0 #input acceleration
post = np.array([[0], [0]])
x = np.array([[0], [0]])
curVar = np.array([[1, 1], [1, 1]])
GPSPollFreq = 1
count = 0
while count < 21:
if count % GPSPollFreq == 0:
measTaken = 1
else:
measTaken = 0
#simulate GPS randomly not working
p = np.random.rand()
import sys
import kalman
from numpy import *
dt = 1.0 / 2500.0
process_sigmasq = 3.0
measurement_sigmasq = 1.0
t = kalman.kalman(
x=zeros((9, 1)),
P=array([[10., 0.0, 0.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],
[0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 10., 0.0, 0.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, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10., 0.0, 0.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, 0.0, 0.0, 1.0]]),
)
process = kalman.process_model(
F=array([[1.0, dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 1.0, dt, 0.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, 0.0, 0.0, 1.0, dt, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 1.0, dt, 0.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, 0.0, 0.0, 1.0, dt, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, dt],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]),
import kalman import matplotlib.pyplot as plt from numpy import * from scipy.integrate import odeint from scipy.stats import norm Ts = 0.2 process_sigmasq = 1e-2 measurement_sigmasq = 0.5 filt = kalman.kalman( x=array([[0.0, 1.0, 1.0]]).T, P=array([ [2.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 2.0]]), ) class SineProcess: def __init__(self): self.stepsused = [] def F(self, state): x = state[0][0] omega = state[2][0] return array([ [1.0, Ts, 0.0], [-(omega**2) * Ts, 1.0, -2 * omega * x * Ts], [0.0, 0.0, 1.0]]) def step(self, state):
def main():
input_gps_csv_file = "data/gps.csv" # define the file name
input_acc_csv_file = "data/data.csv"
input_dist_csv_file = "data/distance.csv"
# read the file and extract the required data
lat, lon, time_gps = CSVReadGPS(input_gps_csv_file)
accX, accY, time_acc = CSVReadAcc(input_acc_csv_file)
dist, time_ultra = CSVReadUltra(input_dist_csv_file)
# loop in the lists to estimate the velocities
num_gps_val = len(lat) # Number of GPS data points captured
num_acc_val = len(accX) # Number of Accelerometer data points captured
num_dist_val = len(dist) # number of ultrasound readings collected
#### Calculate the covariances of data
# GPS based velocity estimate
velNorth = []
velWest = []
velNorth.append(0) # intial Velocity
velWest.append(0) # intial Velocity
for i in range(1, num_gps_val):
y_t = GPSVel(time_gps[i], lat[i], lon[i], time_gps[i - 1], lat[i - 1],
lon[i - 1])
velNorth.append(y_t[0])
velWest.append(y_t[1])
# Accelerometer based Velocity Estimate
velX = []
velY = []
velX.append(0) # intial Velocity
velY.append(0) # intial Velocity
for i in range(1, num_acc_val):
acc_time_prev = time_acc[i - 1]
acc_time_cur = time_acc[i]
acc_time_cur_temp = acc_time_cur.split(":")
acc_time_cur_temp = [float(item) for item in acc_time_cur_temp]
acc_time_prev_temp = acc_time_prev.split(":")
acc_time_prev_temp = [float(item) for item in acc_time_prev_temp]
del_acc_t = (acc_time_cur_temp[0] - acc_time_prev_temp[0]) * 3600 + (
acc_time_cur_temp[1] - acc_time_prev_temp[1]) * 60 + (
acc_time_cur_temp[2] - acc_time_prev_temp[2])
# Measurements from the accelerometer
a_t = np.array([2, 1], dtype=np.float32)
a_t[0] = accX[i] # X-direction
a_t[1] = accY[i] # Y-direction
velX.append(a_t[0] * del_acc_t + velX[i - 1]) # V = U + a*t
velY.append(a_t[1] * del_acc_t + velY[i - 1])
cov_gps_velN = np.cov(velNorth)
cov_gps_velW = np.cov(velWest)
cov_acc_velX = np.cov(velX)
cov_acc_velY = np.cov(velY)
cov_dist = np.cov(dist)
print("Covariances Calculated")
# Accelerometer Data acquisition frequency is higher than GPS. GPS: update, Accelerometer: Prediction
# Intialize the filter parameters:
# Velocity Estimate
x_init = np.array([velX[0], velY[0]])
p_init = np.array([1, 0, 1, 0]) # initialize with high covariance value
q_matrix = np.array([cov_acc_velX, 0, 0, cov_acc_velY], dtype=np.float32)
r_matrix = np.array([cov_gps_velN, 0, 0, cov_gps_velW], dtype=np.float32)
acc_data_count = 1 # counter for data index in accelermeter
ultra_data_count = 1 # counter for data index in ultrasound prediction step
# intialize temporary variables
acc_data_count_temp = 0
ultra_data_count_temp = 0
xd_init = dist[0]
pd_init = 1e10
r_ultra = cov_dist
x_prior = np.empty([2, 1])
p_prior = np.empty([2, 2])
xd_prior = 0
pd_prior = 0
x_post = np.empty([2, 1])
p_post = np.empty([2, 2])
xd_post = 0
pd_post = 0
u_t = np.empty([2, 1])
y_t = np.empty([2, 1])
# store the state vector and covariance matrix
#x_post_vec = np.empty([num_gps_val,2], dtype = np.float32)
x1_post_vec = []
p1_post_vec = []
x2_post_vec = []
p2_post_vec = []
x_post_arr = np.zeros((24, 4))
p_post_arr = np.zeros((24, 4))
time_arr = np.zeros((24, 1))
xd_post_arr = np.zeros((3, 1))
pd_post_arr = np.zeros((3, 1))
d_time_arr = np.zeros((3, 1))
print("Q_matrix: ", q_matrix.reshape(2, 2))
print("R_matrix: ", r_matrix.reshape(2, 2))
# Filter Implementation
for i in range(1, num_gps_val):
y_t = GPSVel(time_gps[i], lat[i], lon[i], time_gps[i - 1], lat[i - 1],
lon[i - 1])
gps_time_cur_temp = time_gps[i].split(":")
gps_time_cur_temp = [float(item) for item in gps_time_cur_temp]
gps_time_cur_temp = GMTCDTconv(gps_time_cur_temp)
gps_time_prev_temp = time_gps[i - 1].split(":")
gps_time_prev_temp = [float(item) for item in gps_time_prev_temp]
gps_time_prev_temp = GMTCDTconv(gps_time_prev_temp)
gps_del_t = (gps_time_cur_temp[0] - gps_time_prev_temp[0]) * 3600 + (
gps_time_cur_temp[1] - gps_time_prev_temp[1]) * 60 + (
gps_time_cur_temp[2] - gps_time_prev_temp[2])
# accumulating previous estimates
if i == 1:
x_prev_est = x_init
p_prev = p_init
else:
x_prev_est = x_post
p_prev = p_post
acc_data_count = acc_data_count_temp + acc_data_count
acc_data_count_temp = 0
# Prediction Steps
for j in range(acc_data_count, num_acc_val):
if acc_data_count_temp > 0: # Account for multiple prediction steps
x_prev_est = x_prior
p_prev = p_prior
acc_data_count_temp = acc_data_count_temp + 1 # update the counter
acc_time_cur = time_acc[j]
acc_time_cur_temp = acc_time_cur.split(":")
acc_time_cur_temp = [float(item) for item in acc_time_cur_temp]
time_diff = (
acc_time_cur_temp[0] - gps_time_cur_temp[0]) * 3600 + (
acc_time_cur_temp[1] - gps_time_cur_temp[1]) * 60 + (
acc_time_cur_temp[2] - gps_time_cur_temp[2])
if time_diff < 0: # proceed to the prediction step for all the prediction steps before the GPS readings
acc_time_prev = time_acc[j - 1]
#time difference
acc_time_prev_temp = acc_time_prev.split(":")
acc_time_prev_temp = [
float(item) for item in acc_time_prev_temp
]
del_acc_t = (
acc_time_cur_temp[0] - acc_time_prev_temp[0]) * 3600 + (
acc_time_cur_temp[1] - acc_time_prev_temp[1]) * 60 + (
acc_time_cur_temp[2] - acc_time_prev_temp[2])
# Measurements from the accelerometer
u_t = np.array([2, 1], dtype=np.float32)
u_t[0] = accX[j] # X-direction
u_t[1] = accY[j] # Y-direction
# Kalman Filter Prediction
x_prior, p_prior = KF(x_prev_est, u_t, del_acc_t, 0, q_matrix,
0, p_prev, True, False)
acc_data_count = acc_data_count + 1
else:
break # end of prediction loop
# run the update only if there is a prediction steps
if acc_data_count_temp > 0:
# call the KF update
x_post, p_post = KF(x_prior, u_t, del_acc_t, y_t, 0, r_matrix,
p_prior, False, True)
print("P: ", x_post)
######################################################################################################################################################################pyt
#print(p_post)
x1_post_vec.append(x_post[0][0])
p1_post_vec.append(p_post[0])
x2_post_vec.append(x_post[0][1])
p2_post_vec.append(p_post[3])
if i < 24:
cur_time = (gps_time_cur_temp[0]) * 3600 + (
gps_time_cur_temp[1]) * 60 + (gps_time_cur_temp[2])
time_arr[i] = 84600 - cur_time
x_post_new = x_post.flatten()
if x_post_new.shape[0] == 2:
x_post_arr[i, :2] = x_post_new
p_post_arr[i, :] = p_post.flatten()
else:
x_post_arr[i, :] = x_post.flatten()
p_post_arr[i, :] = p_post.flatten()
####
# Distance Estimation
if i == 1:
xd_prev_est = xd_init
pd_prev = pd_init
kfd = kalman(pd_init, r_ultra, xd_prev_est,
pd_prev) # filter initialization
else:
xd_prev_est = xd_post
pd_prev = pd_post
kfd = kalman(p_post[3], r_ultra, xd_prev_est,
pd_prev) # filter initialization
# prediction using calculated velocity
xd_prior, pd_prior = kfd.update_meas(0, gps_del_t, x_post[0][1],
False)
# Update step: iterate over the different measurement data and use the value which is recorded closest to the current GPS time step
ultra_data_count_temp = 0
gps_ultra_diff = 0
for j in range(ultra_data_count + 1, num_dist_val):
# Time of the measurement
dis_time_cur = time_ultra[j]
dis_time_cur_temp = dis_time_cur.split(":")
dis_time_cur_temp = [float(item) for item in dis_time_cur_temp]
#print(dis_time_cur_temp)
#print(gps_time_cur_temp)
# keep iterating until the difference is positive
gps_ultra_diff = (
dis_time_cur_temp[0] - gps_time_cur_temp[0]) * 3600 + (
dis_time_cur_temp[1] - gps_time_cur_temp[1]) * 60 + (
dis_time_cur_temp[2] - gps_time_cur_temp[2])
if gps_ultra_diff < 0:
ultra_data_count_temp = ultra_data_count_temp + 1
else:
break
# break the for loop if measurement when ahead of prediction
# end of for loop
ultra_data_count_tem = ultra_data_count
ultra_data_count = ultra_data_count + ultra_data_count_temp - 1
if ultra_data_count_tem == ultra_data_count:
continue
if ultra_data_count_temp > 0: # only perform update if a measurement is detected in the given range
#print(ultra_data_count)
xd_post, pd_post = kfd.update_meas(dist[ultra_data_count],
gps_del_t, x_post[0][1],
True)
if i < 3:
time_curr = (gps_time_cur_temp[0]) * 3600 + (
gps_time_cur_temp[1]) * 60 + (gps_time_cur_temp[2])
d_time_arr[i] = 84600 - time_curr
xd_post_arr[i] = xd_post
pd_post_arr[i] = pd_post
mass = 1.81 # kg (~ 4 pounds)
radius = 0.062 / 2 # meters
height = 0.033 # meters (bump height)
wheelbumpdynamics(mass, radius, x_post_arr[0, 0], height)
print('================================================')
# End of Distance Estimation
else:
continue
# end of for loop
#print(p1_post_vec)
#print("R_ultra: ", r_ultra)
#print(pd_post)
#print(p2_post_vec)
plot_results(x_post_arr, p_post_arr, time_arr, xd_post_arr, pd_post_arr,
d_time_arr)
plotter = PlotterInit()
for t in t_true:
req = gen_custom_request('transform', t, size=0.05)
plotter.request(req)
for t in t_noise:
req = gen_custom_request('transform', t, size=0.02)
plotter.request(req)
true2noise = []
for i in xrange(len(t_true)):
true2noise.append(np.c_[t_true[i][0:3,3], t_noise[i][0:3,3]].T)
lreq = gen_custom_request('lines', true2noise, color=(1,1,0), line_width=3)
plotter.request(lreq)
kf = kalman()
kf.init_filter(0, np.zeros(12), 0.01*np.eye(12))
ts = (1/30.) * np.arange(N)
ke = []
for i in xrange(1,len(ts)):
kf.observe_ar(t_noise[i], ts[i])
ke.append(kf.x_filt)
kf_Ts = x_to_tf(ke)
for t in kf_Ts:
req = gen_custom_request('transform', t, size=0.03)
plotter.request(req)
true2est = []
for i in xrange(len(t_true)-1):
true2est.append(np.c_[t_true[i+1][0:3,3], kf_Ts[i][0:3,3]].T)
import kalman
import matplotlib.pyplot as plt
from numpy import *
from scipy.integrate import odeint
from scipy.stats import norm
Ts = 0.2
process_sigmasq = 1e-2
measurement_sigmasq = 0.5
filt = kalman.kalman(
x=array([[0.0, 0.0, 0.0, 0.0, 1.0]]).T,
P=2 * eye(5),
)
class SineProcess:
def __init__(self):
self.stepsused = []
def F(self, state):
x, _, y, _, omega = ravel(state)
vd = -(omega**2) * Ts
vw = -2 * omega * Ts
return array([[1.0, Ts, 0.0, 0.0, 0.0], [vd, 1.0, 0.0, 0.0, vw * x],
[0.0, 0.0, 1.0, Ts, 0.0], [0.0, 0.0, vd, 1.0, vw * y],
[0.0, 0.0, 0.0, 0.0, 1.0]])
def integrate(self, coeff, *init):
def func(y, t):
return [coeff * y[1], y[0]]
and streamfps.frames() >
50): # Start the predicting at a particular point
######## CALCULATE POSITION ########
# Calculate bounding box
#print(streamfps.frames())
(x, y, w, h) = cv.boundingRect(biggreen)
# Calculate x and y of the current observation
xo = x + int(w / 2)
yo = y + int(h / 2)
error = w # Error: somewhere in this region is the center
######## KALMAN FILTER Position #########
if yo < error:
mu, P, pred = kalman(mu, P, F, Q, B, a, None, H, R)
m = "None"
mm = False
else:
mu, P, pred = kalman(mu, P, F, Q, B, a, np.array([xo, yo]), H, R)
m = "normal"
mm = True
if (mm):
listCenterX.append(xo)
listCenterY.append(yo)
listPoints.append((xo, yo))
res += [(mu, P)]
# Calculating estimated position and uncertainty based on the state/covariance matrices
xe = [mu[0] for mu, _ in res]
import kalman
import matplotlib.pyplot as plt
from numpy import *
from scipy.integrate import odeint
from scipy.stats import norm
Ts = 0.2
process_sigmasq = 1e-2
measurement_sigmasq = 0.5
filt = kalman.kalman(
x=array([[0.0, 1.0, 1.0]]).T,
P=array([[2.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 2.0]]),
)
class SineProcess:
def __init__(self):
self.stepsused = []
def F(self, state):
x = state[0][0]
omega = state[2][0]
return array([[1.0, Ts, 0.0],
[-(omega**2) * Ts, 1.0, -2 * omega * x * Ts],
[0.0, 0.0, 1.0]])
def step(self, state):
x, xdot, omega = state[0][0], state[1][0], state[2][0]
coeff = -(omega**2)
