def selection_learning_params(dp, p, k0, target):
"""
选择学习的参数
:param dp: params[ds,dkm,dkf] 变化量
:param p: params[s,km,kf]
:param k0: 初始曲率点
:param target: 目标
:return:
"""
mincost = MINCOST
mina = MINA
maxa = MAXA
da = DA
acc_space = np.arange(mina, maxa, da)
for a in acc_space:
# 加速度搜索
tp = p + a * dp
xc, yc, yawc = mm.generate_last_state(tp[0], tp[1], tp[2], k0)
dc = calc_diff(target, [xc], [yc], [yawc])
cost = np.linalg.norm(dc)
if cost <= mincost and a != 0.0:
mina = a
mincost = cost
return mina
def calc_J(target, p, h, k0):
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0] + h[0], p[1, 0], p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0] - h[0], p[1, 0], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d1 = np.array((dp - dn) / (2.0 * h[0])).reshape(3, 1)
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0], p[1, 0] + h[1], p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0], p[1, 0] - h[1], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d2 = np.array((dp - dn) / (2.0 * h[1])).reshape(3, 1)
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0], p[1, 0], p[2, 0] + h[2], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0], p[1, 0], p[2, 0] - h[2], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d3 = np.array((dp - dn) / (2.0 * h[2])).reshape(3, 1)
J = np.hstack((d1, d2, d3))
return J
def calc_J(target, p, h, k0):
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0] + h[0, 0], p[1, 0], p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0] - h[0, 0], p[1, 0], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d1 = np.matrix((dp - dn) / (2.0 * h[0, 0])).T
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0], p[1, 0] + h[1, 0], p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0], p[1, 0] - h[1, 0], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d2 = np.matrix((dp - dn) / (2.0 * h[1, 0])).T
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0], p[1, 0], p[2, 0] + h[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0], p[1, 0], p[2, 0] - h[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d3 = np.matrix((dp - dn) / (2.0 * h[2, 0])).T
J = np.hstack((d1, d2, d3))
return J
def calc_J(target, p, h, k0, v):
xp, yp, yawp = motion_model.generate_last_state(p[0, 0] + h[0], p[1, 0],
p[2, 0], k0, v)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(p[0, 0] - h[0], p[1, 0],
p[2, 0], k0, v)
dn = calc_diff(target, [xn], [yn], [yawn])
d1 = np.array((dp - dn) / (2.0 * h[0])).reshape(3, 1)
xp, yp, yawp = motion_model.generate_last_state(p[0, 0], p[1, 0] + h[1],
p[2, 0], k0, v)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(p[0, 0], p[1, 0] - h[1],
p[2, 0], k0, v)
dn = calc_diff(target, [xn], [yn], [yawn])
d2 = np.array((dp - dn) / (2.0 * h[1])).reshape(3, 1)
xp, yp, yawp = motion_model.generate_last_state(p[0, 0], p[1, 0],
p[2, 0] + h[2], k0, v)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(p[0, 0], p[1, 0],
p[2, 0] - h[2], k0, v)
dn = calc_diff(target, [xn], [yn], [yawn])
d3 = np.array((dp - dn) / (2.0 * h[2])).reshape(3, 1)
J = np.hstack((d1, d2, d3))
return J
def calc_J(target, p, h, k0):
## change of x (R^3) is the function of p(R^3)
xp, yp, yawp = motion_model.generate_last_state(p[0, 0] + h[0, 0], p[1, 0],
p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp]) # Eq. (13) in paper
xn, yn, yawn = motion_model.generate_last_state(p[0, 0] - h[0, 0], p[1, 0],
p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d1 = np.matrix((dp - dn) / (2.0 * h[1, 0])).T # Eq. (16) in paper
xp, yp, yawp = motion_model.generate_last_state(p[0, 0], p[1, 0] + h[1, 0],
p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(p[0, 0], p[1, 0] - h[1, 0],
p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d2 = np.matrix((dp - dn) / (2.0 * h[2, 0])).T
xp, yp, yawp = motion_model.generate_last_state(p[0, 0], p[1, 0],
p[2, 0] + h[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(p[0, 0], p[1, 0],
p[2, 0] - h[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d3 = np.matrix((dp - dn) / (2.0 * h[2, 0])).T
J = np.hstack((d1, d2, d3))
return J
def selection_learning_param(dp, p, k0, target):
mincost = float("inf")
mina = 1.0
maxa = 2.0
da = 0.5
for a in np.arange(mina, maxa, da):
tp = p + a * dp
xc, yc, yawc = motion_model.generate_last_state(
tp[0], tp[1], tp[2], k0)
dc = calc_diff(target, [xc], [yc], [yawc])
cost = np.linalg.norm(dc)
if cost <= mincost and a != 0.0:
mina = a
mincost = cost
# print(mincost, mina)
# input()
return mina
def selection_learning_param(dp, p, k0, target):
mincost = float("inf")
mina = 1.0
maxa = 2.0
da = 0.5
for a in np.arange(mina, maxa, da):
tp = p + a * dp
xc, yc, yawc = motion_model.generate_last_state(
tp[0], tp[1], tp[2], k0)
dc = calc_diff(target, [xc], [yc], [yawc])
cost = np.linalg.norm(dc)
if cost <= mincost and a != 0.0:
mina = a
mincost = cost
# print(mincost, mina)
# input()
return mina
def selection_learning_param(dp, p, k0, target):
mincost = float("inf")
mina = 0.6
maxa = 1.4 # for np.arange, can not reach max
da = 0.2
for a in np.arange(mina, maxa, da):
tp = p[:, :] + a * dp
xc, yc, yawc = motion_model.generate_last_state(
tp[0], tp[1], tp[2], k0)
dc = np.matrix(calc_diff(target, [xc], [yc], [yawc])).T
cost = np.linalg.norm(dc)
if cost <= mincost and a != 0.0:
mina = a
mincost = cost
# print(mincost, mina)
# input()
return mina
def clac_j(target, p, h, k0):
"""
计算Loss(target,current_state)的雅克比矩阵
:param target: target
:param p: [s,km,kf]
:param h: increment
:param k0: 固定死，只有km,kf是活的
:return:
"""
# 计算Jacobi matrix
# -[d(C(x,p))/d(p)]
# dp包括 ds, dkm, df
# p[0,0] ---> s +/- 0.5
# xp,yp,yawp 是由p[0,0]+h[0]所产生的所有可能的状态
xp, yp, yawp = mm.generate_last_state(p[0, 0] + h[0], p[1, 0], p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = mm.generate_last_state(p[0, 0] - h[0], p[1, 0], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d1 = np.array((dp - dn) / (2.0 * h[0])).reshape(3, 1)
# p[1,0] ---> km +/- 0.02
xp, yp, yawp = mm.generate_last_state(p[0, 0], p[1, 0] + h[1], p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = mm.generate_last_state(p[0, 0], p[1, 0] - h[1], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d2 = np.array((dp - dn) / (2.0 * h[1])).reshape(3, 1)
# p[2,0] ---> kf +/- 0.02
xp, yp, yawp = mm.generate_last_state(p[0, 0], p[1, 0], p[2, 0] + h[2], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = mm.generate_last_state(p[0, 0], p[1, 0], p[2, 0] - h[2], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d3 = np.array((dp - dn) / (2.0 * h[2])).reshape(3, 1)
# d(Loss(x,p))/d(p)
# p including s、km、kf
J = np.hstack((d1, d2, d3)) # 3x3
return J
