def BKCSmooth(parameterList, fitNum, order, smoothNum):
length = len(parameterList)
smoothList = []
t = []
R = []
V = []
tau = []
K = []
for i in range(int((length - 2 * fitNum) / smoothNum)):
for j in range(2 * fitNum + 1):
t.append(parameterList[i * smoothNum + j][0])
R.append(parameterList[i * smoothNum + j][1])
V.append(parameterList[i * smoothNum + j][2])
tau.append(parameterList[i * smoothNum + j][3])
K.append(parameterList[i * smoothNum + j][4])
matR = fitting.polyLeastFit(t, R, order)
matV = fitting.polyLeastFit(t, V, order)
mattau = fitting.polyLeastFit(t, tau, order)
matK = fitting.polyLeastFit(t, K, order)
del t[:]
del R[:]
del V[:]
del tau[:]
del K[:]
smoothT = [parameterList[i * smoothNum + fitNum][0]]
fitR = fitting.polyLeastFitCal(smoothT, matR)
fitV = fitting.polyLeastFitCal(smoothT, matV)
fitTau = fitting.polyLeastFitCal(smoothT, mattau)
fitK = fitting.polyLeastFitCal(smoothT, matK)
smoothList.append([smoothT[0], fitR[0], fitV[0], fitTau[0], fitK[0]])
# print smoothT[0],fitR[0],fitV[0],fitTau[0],fitK[0]
print "parameterList have be smoothed!"
return smoothList
def reduceSec(rangingList, fitNum, timeShift, channelR, channelC, order):
startSec = int(rangingList[0][0]) + 1
endSec = int(rangingList[-1][0])
reduceList = []
t = []
r = []
c = []
# fitNum=5
index = 0
length = len(rangingList)
for sec in range(startSec, endSec + 1):
while rangingList[index][0] < sec:
index += 1
for i in range(index - fitNum, index + fitNum):
if i >= length - 1:
break
t.append(rangingList[i][0] + timeShift)
r.append(rangingList[i][channelR])
c.append(rangingList[i][channelC])
matR = fitting.polyLeastFit(t, r, order)
matC = fitting.polyLeastFit(t, c, order)
reduceList.append([
sec,
fitting.polyLeastFitCal([float(sec)], matR)[0],
fitting.polyLeastFitCal([float(sec)], matC)[0]
])
del t[:]
del r[:]
del c[:]
return reduceList
def fitSmooth(rangingList, segment, order):
t = []
R = []
tau = []
T = []
clk = []
detSat = []
newList = []
length = len(rangingList)
for i in range(int(length / segment)):
for j in range(segment):
t.append(rangingList[i * segment + j][0])
R.append(rangingList[i * segment + j][1])
# clk.append(rangingList[i * segment + j][2])
clk.append(rangingList[i * segment + j][3])
detSat.append(rangingList[i * segment + j][4])
matR = fitting.polyLeastFit(t, R, order)
# matTau = fitting.polyLeastFit(t, tau, order)
matclk = fitting.polyLeastFit(t, clk, order)
matD = fitting.polyLeastFit(t, detSat, order)
# for k in range(segment):
rangingList[i * segment +
int(segment / 2)][1] = fitting.polyLeastFitCal(
[rangingList[i * segment + int(segment / 2)][0]],
matR)[0]
# rangingList[i * segment + int(segment / 2)][2] = fitting.polyLeastFitCal([rangingList[i * segment + int(segment / 2)][0]], matTau)[0]
rangingList[i * segment +
int(segment / 2)][3] = fitting.polyLeastFitCal(
[rangingList[i * segment + int(segment / 2)][0]],
matclk)[0]
rangingList[i * segment +
int(segment / 2)][4] = fitting.polyLeastFitCal(
[rangingList[i * segment + int(segment / 2)][0]],
matD)[0]
# rangingList[i * segment + int(segment / 2)][4] =rangingList[i*segment+int(segment/2)][1]
del t[:]
del R[:]
del clk[:]
del detSat[:]
newList.append(rangingList[i * segment + int(segment / 2)])
reamin = length - int(length / segment) * segment
# if reamin>0:
# for ii in range(int(length/segment)*segment,length):
# t.append(rangingList[ii][0])
# R.append(rangingList[ii][1])
# clk.append(rangingList[i * segment + j][3])
# matR=fitting.polyLeastFit(t,R,order)
# matclk = fitting.polyLeastFit(t, clk, order)
# rangingList[jj][1]=fitting.polyLeastFitCal([rangingList[jj][0]],matR)[0]
# rangingList[i * segment + int(segment / 2)][3] = \
# fitting.polyLeastFitCal([rangingList[i * segment + int(segment / 2)][0]], matclk)[0]
# newList.append(rangingList[int(length/segment)*segment+int(reamin/2)])
return newList
def filterbyfit(rangingList, segment, order, windows, channel):
t = []
R = []
filterList = []
count = 0
fitCount = int(segment / 2)
length = len(rangingList)
for i in range(length):
if i < segment:
for j in range(segment):
index = i + j
t.append(rangingList[index][0])
R.append(rangingList[index][channel])
mat = fitting.polyLeastFit(t, R, order)
del t[:]
del R[:]
det = rangingList[i][channel] - fitting.polyLeastFitCal(
[rangingList[i][0]], mat)[0]
if abs(det) < windows:
filterList.append(rangingList[i])
else:
count += 1
elif i < length - segment:
if fitCount == int(segment / 2):
mat = fitsegment(rangingList, i, segment, channel, order)
fitCount -= 1
if fitCount == 0:
fitCount = int(segment / 2)
det = rangingList[i][channel] - fitting.polyLeastFitCal(
[rangingList[i][0]], mat)[0]
if abs(det) < windows:
filterList.append(rangingList[i])
else:
count += 1
else:
for j in range(segment):
index = i + j - segment
t.append(rangingList[index][0])
R.append(rangingList[index][channel])
mat = fitting.polyLeastFit(t, R, order)
del t[:]
del R[:]
det = rangingList[i][channel] - fitting.polyLeastFitCal(
[rangingList[i][0]], mat)[0]
if abs(det) < windows:
filterList.append(rangingList[i])
# residualList.append([rangingList[i][0], det])
else:
count += 1
print 'fit filtered! %s points moved !' % (count)
return filterList
def fitfilter(matchList, segment, order, groSatShift, windows):
t = []
R = []
filterList = []
count = 0
c = 299792458.0
length = len(matchList)
for i in range(int(length / segment)):
for j in range(segment):
index = i * segment + j
t.append((matchList[index][1] + matchList[index][2]) /
(2 * 1000000000000.0) + groSatShift)
R.append((matchList[index][1] + matchList[index][3] -
matchList[index][0] - matchList[index][2]) * c /
(2 * 1000000000000.0))
mat = fitting.polyLeastFit(t, R, order)
for k in range(segment):
det = R[k] - fitting.polyLeastFitCal([t[k]], mat)[0]
if abs(det) < windows:
filterList.append(matchList[i * segment + k])
else:
count += 1
del t[:]
del R[:]
#newList.append(rangingList[i*segment+int(segment/2)])
reamin = length - int(length / segment) * segment
print 'fit filtered! %s points moved out!' % (count + reamin)
return filterList
def fitsegment(list, index, segment, channel, order):
t = []
R = []
for i in range(index - segment, index + segment):
t.append(list[i][0])
R.append(list[i][channel])
mat = fitting.polyLeastFit(t, R, order)
del t[:]
del R[:]
return mat
def fitEvaluate(fitList, timeCoin1, timeCoin2, threshold, order):
feedBack = False
timeList = []
delayList = []
for item in fitList:
timeList.append(item[1])
delayList.append(item[2])
mat = fitting.polyLeastFit(timeList, delayList, order)
timeFit = fitting.polyLeastFitCal([timeCoin2], mat)
det = timeCoin1 - timeCoin2 - timeFit[0]
if abs(det) < threshold:
feedBack = True
# print 'this coincidence is ok,',det
# else:
# print 'this coincidence out of threshold'
return feedBack
def leastsqToSec(timeList, tau, order):
X = []
Y = []
R = []
Sec = int(timeList[0][0] / tau)
count = 0
# def residuals(p):
# k, b = p
# return Y - (k * X + b)
for item in timeList:
if int(item[0] / tau) == Sec:
X.append(item[0] / tau)
Y.append(item[1])
count += 1
else:
if count >= 2:
# r = optimize.leastsq(residuals, [1.0, 0.0])
# k, p = r[0]
# s = [Sec, (k * (Sec) + p)/1000000000000]
print count
mat = fitting.polyLeastFit(X, Y, order)
y = fitting.polyLeastFitCal([Sec + 0.5], mat)
averY = sum(Y) / len(Y)
if y[0] > 3 * averY:
s = [Sec + 0.5, averY / 1000000000000]
print averY / 1000000000000, '0'
else:
s = [Sec + 0.5, y[0] / 1000000000000]
print y[0] / 1000000000000, '1'
#print s,k
R.append(s)
else:
s = [Sec + 0.5, item[1] / 1000000000000]
print item[1] / 1000000000000, '2'
R.append(s)
del X[:]
del Y[:]
X.append(item[0] / tau)
Y.append(item[1])
Sec = int(item[0] / tau)
count = 1
print 'data reduced by leastsq to second/%s.' % tau
filter.dotFilter(R, 1, 0.000000005, 3)
return R
def rangefilter(rangingList, segment, order, windows, channel):
t = []
R = []
filterList = []
residualList = []
count = 0
length = len(rangingList)
for i in range(length):
if i < segment:
for j in range(segment):
index = i + j
t.append(rangingList[index][0])
R.append(rangingList[index][channel])
mat = fitting.polyLeastFit(t, R, order)
del t[:]
del R[:]
det = rangingList[i][channel] - fitting.polyLeastFitCal(
[rangingList[i][0]], mat)[0]
if abs(det) < windows:
filterList.append(rangingList[i])
residualList.append([rangingList[i][0], det])
# print rangingList[i][0], det
else:
# rangingList[i ][1] = fitting.polyLeastFitCal([rangingList[i][0]], mat)[0]
# filterList.append(rangingList[i])
# residualList.append([rangingList[i][0], 0])
count += 1
elif i < length - segment:
for j in range(segment):
index = i + j - segment / 2
t.append(rangingList[index][0])
R.append(rangingList[index][channel])
mat = fitting.polyLeastFit(t, R, order)
del t[:]
del R[:]
det = rangingList[i][channel] - fitting.polyLeastFitCal(
[rangingList[i][0]], mat)[0]
if abs(det) < windows:
filterList.append(rangingList[i])
residualList.append([rangingList[i][0], det])
# print rangingList[i][0], det
else:
# rangingList[i ][channel] = fitting.polyLeastFitCal([rangingList[i][0]], mat)[0]
# filterList.append(rangingList[i])
# residualList.append([rangingList[i][0], 0])
count += 1
else:
for j in range(segment):
index = i + j - segment
t.append(rangingList[index][0])
R.append(rangingList[index][channel])
mat = fitting.polyLeastFit(t, R, order)
del t[:]
del R[:]
det = rangingList[i][channel] - fitting.polyLeastFitCal(
[rangingList[i][0]], mat)[0]
if abs(det) < windows:
filterList.append(rangingList[i])
residualList.append([rangingList[i][0], det])
# print rangingList[i][0], det
else:
# rangingList[i][channel] = fitting.polyLeastFitCal([rangingList[i][0]], mat)[0]
# filterList.append(rangingList[i])
# residualList.append([rangingList[i][0], 0])
count += 1
print 'fit filtered! %s points moved !' % (count)
return filterList, residualList
def asynLaserRanging(matchList, groSatShift):
length = len(matchList)
rangingList = []
tempR = []
iterNum = 1
c = 299792458.0
smoothNum = 20
for i in range(length):
# matchList，每行数据的顺序是：地面发射时间TE1、星上接收时间TM2、星上发射时间TM1、地面接收时间TE2
# t=(matchList[i][1]+matchList[i][2])/(2*1000000000000.0)+groSatShift
# R=(matchList[i][1]+matchList[i][3]-matchList[i][0]-matchList[i][2])*c/(2*1000000000000.0)
# tau=(matchList[i][3]-matchList[i][0]-(matchList[i][1]-matchList[i][2]))
# T=matchList[i][3]-matchList[i][0]
# clk=(matchList[i][1]-matchList[i][0]-(matchList[i][3]-matchList[i][2]))/2+groSatShift*1000000000000
#公式来源：提高深空异步激光测距
# t = (matchList[i][5] + matchList[i][6]) / (2 * 1000000000000.0)
# R=(matchList[i][5]+matchList[i][7]-matchList[i][4]-matchList[i][6])*c/(2*1000000000000.0)
# tau=(matchList[i][7]-matchList[i][4]-(matchList[i][5]-matchList[i][6]))
# T=matchList[i][7]-matchList[i][4]
# clk = (matchList[i][5] - matchList[i][4] - (matchList[i][7] - matchList[i][6])) / 2 + groSatShift * 1000000000000
# 公式来源：Degnan
#t = (matchList[i][3] + matchList[i][0]) / (2 * 1000000000000.0)
t = (matchList[i][7] + matchList[i][4]) / (2 * 1000000000000.0)
R = (matchList[i][5] + matchList[i][7] - matchList[i][4] -
matchList[i][6]) * c / (2 * 1000000000000.0)
# R = (matchList[i][1] + matchList[i][3] - matchList[i][0] - matchList[i][2]) * c / (2 * 1000000000000.0)
#tau=(matchList[i][3]-matchList[i][0]-(matchList[i][1]-matchList[i][2]))
tau = (matchList[i][7] - matchList[i][4] -
(matchList[i][5] - matchList[i][6]))
# T=matchList[i][3]-matchList[i][0]
clk = (matchList[i][1] + matchList[i][2] -
(matchList[i][0] +
matchList[i][3])) / 2 + groSatShift * 1000000000000
# detSat=matchList[i][5]-matchList[i][6]
detSat = matchList[i][1] - matchList[i][2] #被动式
rangingList.append([t, R, tau, clk, detSat])
# rangingList.append([t,R,tau,T,R,clk])
#print (matchList[i][4]+matchList[i][7])-(matchList[i][5]+matchList[i][6])
#TODO 数点合成一个点，平滑滤波，但是需注意数据不能缺失大段，否则会引入误差，待改善
rangingList = fitSmooth(rangingList, 100, 2)
# rangingList=reducebyfactor(rangingList,10)
rangingList, residualList = rangefilter(rangingList, 30, 4, 0.2)
length = len(rangingList)
tx = []
ry = []
for j in range(iterNum):
for i in range(length):
#TODO 平滑速度
if i <= smoothNum:
detR = (rangingList[i][1] - rangingList[i + 1][1]) / (
rangingList[i + 1][0] - rangingList[i][0])
elif i < length - smoothNum:
for index in range(i - smoothNum, i + smoothNum):
tx.append(rangingList[index][0])
ry.append(rangingList[index][1])
mat = fitting.polyLeastFit(tx, ry, 5)
tR = []
detRList = []
# for k in range(i-smoothNum/2,i+smoothNum):
# tR.append(rangingList[k][0])
# detRList.append((fitting.polyLeastFitCal([rangingList[k-1][0]],mat)[0]-fitting.polyLeastFitCal([rangingList[k+1][0]],mat)[0])/(rangingList[k+1][0]-rangingList[k-1][0]))
# matV=fitting.polyLeastFit(tR,detRList,2)
detR = (
fitting.polyLeastFitCal([rangingList[i - 1][0]], mat)[0] -
fitting.polyLeastFitCal([rangingList[i + 1][0]], mat)[0]
) / (rangingList[i + 1][0] - rangingList[i - 1][0])
# detRV=fitting.polyLeastFitCal([rangingList[i][0]],matV)[0]
# print detR,detRV,detR-detRV
del tx[:]
del ry[:]
del tR[:]
del detRList[:]
else:
detR = (rangingList[i - 1][1] - rangingList[i][1]) / (
rangingList[i][0] - rangingList[i - 1][0])
#提高
# Tau=rangingList[i][2]/(2+detR/(c-detR))/1000000000000.0
# tempR.append(rangingList[i][4]-detR*(rangingList[i][3]/1000000000000.0-Tau)/2.0)
#Degnan
Tau = rangingList[i][2] / (2 + 2 * detR / c)
# Tba=rangingList[i][3]-Tau*detR/c
# Tba = rangingList[i][3] - rangingList[i][4] * detR / (2*c)
# print detR,Tau*detR/c,rangingList[i][4] * detR / (2*c)
if j == iterNum - 1:
rangingList[i][2] = Tau
# rangingList[i][3]=Tba
rangingList[i] = (
rangingList[i] +
[detR, Tau * detR / c, rangingList[i][4] * detR / (2 * c)])
# print '%s\t%s\t%s'%(j,rangingList[i][0],detR)
# for i in range(length):
# rangingList[i][1]=tempR[i]
# del tempR[:]
print 'iterate %s times, asyn laser ranging is finished!' % iterNum
rangingList, residualList = rangefilter(rangingList, 30, 4, 0.15)
# rangingList = fitSmooth(rangingList, 10, 3)
# rangingList = rangefilter(rangingList, 30, 5, 0.08)
return rangingList, residualList
