def BezierInterp(T, p0, p1):
# Bezier curve interpolation
# T-conversion factor, dp/dt = dp/ds*1/T
# p0,p1 are start/end point x,xd,xdd,xddd,xdddd
d = 3 # number of derivatives of p0,p1
n = 7 #degree of curve
Cout = np.zeros((n + 1, 3))
# note: interpolation is done in [0,1], then converted to [0,T]
for i in range(3):
# cal x,y,z dimensions seperately
CL = np.zeros((d + 1, (n + 1) / 2))
CR = np.zeros((d + 1, (n + 1) / 2))
x0 = np.array(
[p0[0, i], p0[1, i] * T, p0[2, i] * T**2, p0[3, i] * T**3])
x1 = np.array(
[p1[0, i], p1[1, i] * T, p1[2, i] * T**2, p1[3, i] * T**3])
CL[:, 0] = np.multiply(ft(n - np.arange(d + 1)) / ft(n), x0)
for j in range(d):
CL[0:d - j, j + 1] = CL[0:d - j, j] + CL[1:d - j + 1, j]
CR[:, -1] = np.multiply(ft(n - np.arange(d + 1)) / ft(n), x1)
for j in range(d):
CR[0:d - j,
-2 - j] = CR[0:d - j, -2 - j + 1] - CR[1:d - j + 1, -2 - j + 1]
# control points 10*3 matrix
Cout[:, i] = np.transpose(np.hstack((CL[0, :], CR[0, :])))
return Cout
def ref_extract(T, t, Cin):
#extract nominal state from Bezier control points
n = 7 #degree of cureve
d = 3 #derivative degree of Cin
Cout = np.zeros((d + 1, 3))
tn = min(t / T, 1) #nomarlized time, [0,1]
# calculte basis
Polybasis = np.zeros((d + 1, n + 1))
for k in range(d + 1):
for kk in range(n - k + 1):
Polybasis[k,
kk] = ft(n) / ft(n - k) * nCk(n - k, kk) * (tn**kk) * (
(1 - tn)**(n - k - kk))
for i in range(3):
Ctemp = np.zeros((d + 1, n + 1))
# cal 3 dimensional nominal states
Ctemp[0, :] = np.transpose(Cin[:, i])
for j in range(d):
if j == 0:
Ctemp[1, 0:-1] = Ctemp[j, 1:] - Ctemp[j, 0:-1]
else:
Ctemp[j + 1, 0:-1 - j] = Ctemp[j, 1:-j] - Ctemp[j, 0:-1 - j]
# cal 0-4th order derivatives
for k in range(d + 1):
Cout[k, i] = np.dot(Polybasis[k, :], Ctemp[k, :]) / T**k
return Cout
def zinb(value=sna, mu=lambduh, alpha=delta, psi=pzero):
# Initialise likeihood
like = 0.0
# Add zero component; zero probability + P(NB==0); value flags for non-zeros to cancel out
like += np.sum((np.log(psi + (1.-psi)*(alpha/(mu+alpha))**alpha))*Iz)
# Add count component; non-zero probability + P(NB>0); value flags for zeros to cancel out
like += np.sum((np.log(1.-psi) + np.log(gammaf(alpha+value))-np.log((ft(value)*gammaf(alpha))) + alpha*np.log(alpha/(mu+alpha)) + value*np.log(mu/(mu+alpha)))*Ic)
return like
def state_from_bez(T, t, Cin):
# Extract nominal state from Bezier control points
d = 4 # derivative degree of Cin
n = 9 # degree of curve
tn = min(t/T,1) # normalized time, [0,1]
Ctemp = np.zeros((d+1,n+1))
phat = np.zeros((d+1,3))
for i in range(3):
# cal 3 dimensional nominal states
Ctemp[0,:] = np.transpose(Cin[:,i])
for j in range(d):
if j == 0:
Ctemp[ 1, 0:-1] = Ctemp[j, 1:] - Ctemp[j, 0:-1]
else:
Ctemp[j+1, 0:-1-j] = Ctemp[j, 1:-j] - Ctemp[j, 0:-1-j]
# cal 0-4th order derivatives
for k in range(d+1):
temp = 0
for kk in range(n-k+1):
Btemp = nCk(n-k,kk)*(tn**kk)*((1-tn)**(n-k-kk))
temp = temp + ft(n)/ft(n-k)*Ctemp[k,kk]*Btemp
phat[k,i] = temp/T**k
return phat
