def cost_m(x, trgt_cpts, p0, v0, psi0, dt, params):
"""Cost function for the monitoring trajectory
"""
pt = Bezier(trgt_cpts, tf=dt)
y = reshape_m(x, p0, v0, psi0, dt, params.deg, trgt_cpts[:, -1],
params.innerR)
p = Bezier(y, tf=dt).elev(params.degElev)
pdot = p.diff()
# if np.any((pdot*(pt-p)).cpts < 0):
# return 99999
costPts = ((pt.y - p.y) * pdot.x - (pt.x - p.x) * pdot.y).normSquare().cpts
# if np.sign(sum(pdot*(pt-p)))
return sum(costPts.squeeze())
