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