def __init__(self):
self.rate = rospy.Rate(update_interval)
self.bus = SMBus(1)
self.utmconv = utmconv()
self.recording = False
self.main_mode = ""
self.sub_mode = ""
self.lat = 0
self.lon = 0
self.alt = 0
self.mission_idx = 0
self.mission_len = 0
self.new_imu_reading = False
self.new_vel_reading = False
dt = 1/update_interval
self.state = np.zeros((4,1), float)
self.kalman = kalman.KalmanFilter(self.state, dt)
# self.state = np.zeros((6,1), float)
# self.kalman = kalman3.KalmanFilter(self.state, dt)
# self.state = np.zeros((9,1), float)
# self.kalman = kalman_acc3.KalmanFilter(self.state, dt)
self.local_position_ned = np.array([0,0,0])
self.vx = 0
self.vy = 0
self.vz = 0
self.ax = 0
self.ay = 0
self.az = 0
self.filtered_pos = np.empty((0,2), float)
self.local_position_landing = Point()
self.landing_target = np.array([0,0,0])
self.landing_coords = Point(1.17, 0.75, 0)
self.data = np.empty((0,4), float)
self.local_data = np.empty((0,3), float)
# self.filtered_data = np.empty((0,3), float)
self.filtered_data = np.empty((0,2), float)
self.rotation_matrix = np.array([[1,0],[0,1]])
rospack = rospkg.RosPack()
package_path = rospack.get_path("precision_landing")
self.data_path = package_path + "/data/"
rospy.Subscriber("/mavlink_pos", mavlink_lora_pos, self.on_drone_pos)
rospy.Subscriber("/telemetry/mission_info", telemetry_mission_info, self.on_mission_info)
rospy.Subscriber("/telemetry/heartbeat_status", telemetry_heartbeat_status, self.on_heartbeat_status)
rospy.Subscriber("/telemetry/imu_data_ned", telemetry_imu_ned, self.on_imu_data)
rospy.Subscriber("/telemetry/local_position_ned", telemetry_local_position_ned, self.on_local_pos)
self.sensor_data_pub = rospy.Publisher("/landing/sensor_data", precland_sensor_data, queue_size=0)
self.landing_target_pub = rospy.Publisher("/telemetry/set_landing_target", telemetry_landing_target, queue_size=0)
def __init__(self, x, y, label, team):
self.label = label
self.x = x
self.y = y
self.isAlreadyLabaled = True
self.team = team #Which team 1 or 2
self.kalmanFilter = kalman.KalmanFilter()
self.kalmanFilter.updatePosition((self.x, self.y))
self.historyRemember = 200
self.historyPoints = []
self.isAlreadyLabaled = False
def apply_kalman(split_dfs):
sys.path.append('./kalmanjs/contrib/python/')
import kalman
for run in split_dfs:
KFilter = kalman.KalmanFilter(0.008, 0.1)
cur_df = split_dfs[run]
num_vals = cur_df.shape[0]
for i, row in cur_df.iterrows():
row['rss'] = KFilter.filter(row['rss'])
full_df = pd.concat(split_dfs.values())
print(full_df.shape)
return full_df
def setKalman(self, now_cood, p_1, p_2, p_3, o_walk, o_range, r):
'''[Kalman Filterのパラメータ設定]
Args:
now_cood ([list]): [現在地の座標]
p_1 ([mat]): [基地局1の座標]
p_2 ([mat]): [基地局2の座標]
p_3 ([mat]): [基地局3の座標]
o_walk ([int]): [状態モデルの誤差分散]
o_range ([int]): [観測モデルの誤差分散]
'''
self.kf = kalman.KalmanFilter(now_cood[0], now_cood[1], p_1, p_2, p_3)
self.kf.setModel(o_walk, o_range, r)
def initialize_kalman_filter():
A = np.eye(3)
def B(x, u):
v, w = u
dx = v*math.cos(x[2] + w/2.)
dy = v*math.sin(x[2] + w/2.)
dtheta = w
return np.matrix([dx, dy, dtheta]).transpose()
D = np.eye(3)
R = np.matrix([[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.05]])
Q = R
return kalman.KalmanFilter(A, B, D, R, Q)
def kalman_filter(y, forecast_period, F=None, H=None, Q=None, R=None):
dt = 1.0/60
F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]])
H = np.array([1, 0, 0]).reshape(1, 3)
Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]])
R = np.array([0.5]).reshape(1, 1)
yfit = []
ypred = []
kf = kalman.KalmanFilter(F, Q, H, R)
kf.fit(y)
for mu in kf.mus:
yfit.append(H.dot(mu)[0])
for _ in range(forecast_period):
ypred.append(H.dot(kf.predict()))
yfit = np.asarray(yfit).reshape((-1, 1))
ypred = np.asarray(ypred).reshape((-1, 1))
return yfit, ypred
def robotFilter(robot, U_a, Y_a, X_a):
global errorA
global errorW
shapeY = Y_a.shape
init_X = np.zeros((6, 1))
robot.setStates(init_X)
P = np.zeros((6, 6))
dt = robot.dt
phi = 10.0
Q = np.matrix([[0, 0, 0, 0, 0, 0],
[0, 10000.0 * dt * phi, 0, -1.0 * dt * phi, 0, 0],
[0, 0, 5.e-2, 0, 0, 0],
[
0, -1.0 * dt * phi, 0, 1.00020100163329 * dt * phi,
-0.0100995766634669 * dt * phi,
-0.000100498325008375 * dt * phi
],
[
0, 0, 0, -0.0100995766634669 * dt * phi,
2.01002500336671 * dt * phi,
0.0200994975041875 * dt * phi
],
[
0, 0, 0, -0.000100498325008375 * dt * phi,
0.0200994975041875 * dt * phi, 1.0001 * dt * phi
]])
# Q=robot.F((0.0,0.0,0.0,0,0,0),(0,0))
R = np.matrix([[errorA**2, 0.0], [0.0, errorW**2]])
kfilter = kalman.KalmanFilter(robot, P, Q, R)
X_f = np.zeros((6, 1))
for index in range(shapeY[1]):
U = np.matrix([[U_a.item(0, index)], [U_a.item(1, index)]])
Y = np.matrix([[Y_a.item(0, index)], [Y_a.item(1, index)]])
kfilter.predict(U)
X = kfilter.update(Y)
X_f = np.concatenate((X_f, X), axis=1)
X_f_l = X_f.tolist()
plotting(X_f_l, X_a)
def main():
er_sum = 0
num = 0
#基地局からの距離
dis_deviceA = 0
dis_deviceB = 0
dis_deviceC = 0
c = np.mat([[1, 0], [0, 1]])
p_1 = np.mat([[AP_A_X], [AP_A_Y]])
p_2 = np.mat([[AP_B_X], [AP_B_Y]])
p_3 = np.mat([[AP_C_X], [AP_C_Y]])
draw = drawing.drawing()
draw = setDraw(draw)
'''
# 歩いていると仮定した場合
for i in range(POINT_NUM):
truePosition = selectPosition(i + 1)
draw.drawCircle(truePosition[0][0], truePosition[1][0], 100, "#050A11", True)'''
if filter == 1:
kf = kalman.KalmanFilter(TRUE_POSITION_1_X, TRUE_POSITION_1_Y, p_1, p_2, p_3)
#kf = kalman.KalmanFilter(0, 0, p_1, p_2, p_3)
kf.setModel(o_walk=O_WALK, o_range=O_RANGE)
#kf.setModel(O_WALK, O_RANGE, r)
elif filter == 2:
bf = bayesian.BaysianFilter(POINT_1_GRID, p_i, sigma_range)
bf.readMap(map_path)
elif filter == 3:
bkm = bkmixture.BKmixture(p_i)
bkm.setKalman([TRUE_POSITION_1_X, TRUE_POSITION_1_Y], p_1, p_2, p_3, O_WALK, O_RANGE, r)
bkm.setBayesian(sigma_range, map_path)
with open(write_path + write_file + '8' + '.csv', mode='w') as fw:
with open(read_path + read_file + 'A' + '.csv', encoding='utf-8', mode='r') as f:
for line in f:
data = line.split(',')
distanceMm = int(data[0])
getID = int(data[4])
#truenum = int(data[5])
#ファイルから取得したデータをcalibrate
if getID == DEVICE_A_ID:
dis_deviceA = calibration(getID, distanceMm)
elif getID == DEVICE_B_ID:
dis_deviceB = calibration(getID, distanceMm)
elif getID == DEVICE_C_ID:
dis_deviceC = calibration(getID, distanceMm)
#測距結果を用いて測位
if dis_deviceA != 0 and dis_deviceB != 0 and dis_deviceC != 0:
Z = np.mat([[dis_deviceA], [dis_deviceB], [dis_deviceC]])
time = 0
if filter == 1:
kf.estimate()
#kf.filter(Z)
kf.filter(np.square(Z))
x = kf.getStatus()
elif filter == 2:
grid = 1
was_grid = 0
for key in range(8):
was_grid = grid
bf.prediction()
x, grid = bf.update(Z)
if grid != was_grid:
time += 1
print(str(grid) + ',' + str(was_grid))
elif filter == 3:
x, grid = bkm.estimate(Z)
dis_deviceA = 0
dis_deviceB = 0
dis_deviceC = 0
#er = error(x, num + 1)
#er_sum += er
fw.write(str(int(x[0, 0])) + ',' + str(int(x[1, 0])) + ',' + str(time) + ',' + '\n')
mod = num % 8
if mod == 0:
draw.drawCircle(x[0, 0], x[1, 0], 100, "#EDAD0B", True)
elif mod == 1:
draw.drawCircle(x[0, 0], x[1, 0], 100, "#C7243A", True)
elif mod == 2:
draw.drawCircle(x[0, 0], x[1, 0], 100, "#D8E212", True)
elif mod == 3:
draw.drawCircle(x[0, 0], x[1, 0], 100, "#23AC0E", True)
elif mod == 4:
draw.drawCircle(x[0, 0], x[1, 0], 100, "#0086AB", True)
elif mod == 5:
draw.drawCircle(x[0, 0], x[1, 0], 100, "#3261AB", True)
elif mod == 6:
draw.drawCircle(x[0, 0], x[1, 0], 100, "#5D639E", True)
elif mod == 7:
draw.drawCircle(x[0, 0], x[1, 0], 100, "#A52175", True)
num += 1
#print('a')
#draw.pause(0.1)
draw.show()
'''
for num in range(POINT_NUM):
filename = FILE_NAME
truePosition = selectPosition(num + 1)
print("now:" + str(now_grid))
print("position:" + str(truePosition))
draw.drawCircle(truePosition[0, 0], truePosition[1, 0], 100, "#050A11", True)
if filter == 1:
#kf = kalman.KalmanFilter(truePosition[0, 0], truePosition[1, 0], p_1, p_2, p_3)
kf = kalman.KalmanFilter(0, 0, p_1, p_2, p_3)
kf.setModel(O_WALK, O_RANGE, r)
elif filter == 2:
bf = bayesian.BaysianFilter(now_grid, p_i, sigma_range)
# bf.readMap('/Users/andy/Wifi_rtt/mapInfo.json')
bf.readMap(map_path)
elif filter == 3:
bkm = bkmixture.BKmixture(p_i)
bkm.setKalman([truePosition[0, 0], truePosition[1, 0]], p_1, p_2, p_3, O_WALK, O_RANGE, r)
bkm.setBayesian(sigma_range, map_path)
with open(write_path + write_file + str(num+1) + '.csv', mode='w') as fw:
with open(read_path + read_file + str(num+1) + '.csv', encoding='utf-8', mode='r') as f:
f.readline()
for line in f:
data = line.split(',')
distanceMm = int(data[0])
getID = int(data[4])
#truenum = int(data[5])
#ファイルから取得したデータをcalibrate
if getID == DEVICE_A_ID:
dis_deviceA = calibration(getID, distanceMm)
elif getID == DEVICE_B_ID:
dis_deviceB = calibration(getID, distanceMm)
elif getID == DEVICE_C_ID:
dis_deviceC = calibration(getID, distanceMm)
#測距結果を用いて測位
if dis_deviceA != 0 and dis_deviceB != 0 and dis_deviceC != 0:
Z = np.mat([[dis_deviceA], [dis_deviceB], [dis_deviceC]])
if filter == 1:
kf.estimate()
#kf.filter(Z)
kf.filter(np.square(Z))
x = kf.getStatus()
elif filter == 2:
grid = 1
was_grid = 0
for key in range(5):
was_grid = grid
bf.prediction()
x, grid = bf.update(Z)
print(str(grid) + ',' + str(was_grid))
elif filter == 3:
x, grid = bkm.estimate(Z)
dis_deviceA = 0
dis_deviceB = 0
dis_deviceC = 0
er = error(x, num + 1)
er_sum += er
fw.write(str(x[0, 0]) + ',' + str(x[1, 0]) + ',' + str(er) + ',' + '\n')
draw.drawCircle(x[0, 0], x[1, 0], 100, "#EDAD0B", True)
#print('a')
#draw.pause(0.1)
'''
#draw.show()
return
cv2.waitKey(1)
if __name__ == '__main__':
pt_y = []
age = 0
dt = 1.0/30
A = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]])
H = np.array([1, 0, 0]).reshape(1, 3)
Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]])
R = np.array([0.5]).reshape(1, 1)
kf = dict()
for i in range(6):
kf[i] = kalman.KalmanFilter(A = A, H = H, Q = Q, R = R)
rospy.init_node('avm_segmentation')
avm_sub = message_filters.Subscriber('/pub_avm_image', Image)
front_sub = message_filters.Subscriber('/gmsl_camera/port_0/cam_0/image_raw', Image)
angle_pub = rospy.Publisher('/lane_angle', Float64, queue_size=1)
ts = message_filters.ApproximateTimeSynchronizer([avm_sub, front_sub], 10, 0.1, allow_headerless=True)
ts.registerCallback(callback)
rospy.spin()
cv2.destroyAllWindows()
N = 100
dt = .1
xvState = np.zeros((N,2)) # N rows by 2 cols
t = np.zeros(N) # 1D vector
x0v0 = np.array([100,0])
A = np.array([[1,dt],[0,1]])
u = np.transpose(np.array([[0,g*dt]])) # 2x1 col vec
xvState[0] = x0v0
modelFreeFall = kalman.ProcessModel(A,np.eye(2),u,xvState , 0.8)
oldState = modelFreeFall.state[0].reshape((2,1))
z = np.zeros((N,2))
H = np.eye(2)
measurementNoise = 0.9
sensor = kalman.MeasurementModel(2*H,measurementNoise,z)
kalman = kalman.KalmanFilter(np.zeros((2,2)))
stateEstimates = []
detP = []
for i in range(1,N):
oldState = modelFreeFall.state[i-1].reshape((2,1))
modelFreeFall.state[i] = np.transpose(modelFreeFall.calcNewState(oldState))
trueState = modelFreeFall.state[i].reshape((2,1))
measure = sensor.calcNewMeasurement(trueState)
kalman.predict(oldState,modelFreeFall,sensor)
kalman.calcKGain(sensor)
stateEstimates.append(kalman.correct(sensor,measure))
detP.append(np.linalg.det(kalman.P))
import cv2 as cv
import params as prm
import util as util
import kalman
kalman = kalman.KalmanFilter() #Se inicializa la clase del filtro de Kalman
#cap = cv.VideoCapture(cv.samples.findFile("videoPeq2.mp4"))
cap = cv.VideoCapture(cv.samples.findFile("gido_completo.mp4"))
#cap = cv.VideoCapture(cv.samples.findFile("pendulo_tobi.mp4"))
#cap = cv.VideoCapture(cv.samples.findFile("car.mp4")) #DESCOMENTAR LINEAS DE ABAJO
#cap = cv.VideoCapture(0)
#for i in range(250): #DESCOMENTAR PARA EL VIDEO CAR
# frame = cap.read() #(tiene un titulo al principio)
lower_thr, upper_thr = [], []
if prm.COLOR_ALGORITHM is True:
prev, prev_gray, x, y, w, h, lower_thr, upper_thr = util.captureROI(
cap) #Se captura por primera vez la seleccion del usuario
else:
prev, prev_gray, x, y, w, h = util.captureROI(cap)
prev = util.space_translate(
x, y, prev
) #Se aplica la trasformacion de las coordenadas de las features del espacio
#de la seleccion al espacio del frame.
frame_num = 0
error = False
dyn_h = h
dyn_w = w