def FinalAssignment(planMatrix, kvStates, kvFuel, threatStates, threatIds, deltaT):
# system adjustable parameters
nKVs = mods.SAPs.KV_NUM_KVS
# check each KV for a "need-to-divert now" condition
finalAssign = -1*np.ones(nKVs)
for iKV in range(nKVs):
# find the assignment
thrtIds = np.where(planMatrix[iKV])[0]
if thrtIds.size > 0:
idThrt = thrtIds[0]
kvState = mods.PropagateECI(kvStates[iKV], 0.0, deltaT)
threatState = mods.PropagateECI(threatStates[idThrt], 0.0, deltaT)
tPOCA, _, _ = mods.FindPOCA(kvState, threatState)
tNow = threatStates[idThrt, 6]
tGo = tPOCA - (tNow + deltaT)
if tGo.size > 1:
print("something broke: tPOCA - {}, tNow - {}, deltaT - {}".format(tPOCA, tNow, deltaT))
ptImpact = mods.PropagateECI(threatState, 0.0, tGo)
if np.linalg.norm(kvStates[iKV, 0:3]) == 0 or np.linalg.norm(ptImpact[0:3]) == 0:
kv = mods.PropagateECI(kvStates[iKV], 0.0, tPOCA - tNow)
else:
kv, _ = mods.GaussProblem(kvStates[iKV, 0:3], ptImpact[0:3], tPOCA - tNow)
dVec = kv[3:6] - kvState[3:6]
dV = np.linalg.norm(dVec)
# see if we need to divert right now to make it. if so perform the divert
mustDivert = ((kvFuel[iKV] - dV - mods.SAPs.KV_FUEL_RESERVE) <= 0)
if mustDivert or tGo < 15.0:
kvStates[iKV, 3:6] = kvStates[iKV, 3:6] + dVec
kvFuel[iKV] = kvFuel[iKV] - dV
finalAssign[iKV] = threatIds[idThrt]
print("available threats: {}, \nassigned final threats: {}".format(threatIds, finalAssign.astype(int)))
# return result
return finalAssign.astype(int), kvStates
def generateStates(rPdmFinal, vPdmFinal, rCVFinal, vCVFinal, tFinal):
tmpState = np.append(rPdmFinal, vPdmFinal)
tmpState = np.append(tmpState, [mods.SAPs.MDL_T_DURATION])
#print("generate the initial threat PDM state")
# generate the initial threat PDM state
sPdmInitial = mods.PropagateECI(tmpState.reshape(7, 1), 0.0,
-mods.SAPs.MDL_T_DURATION)
#print("generate the center point of each cloud. the PDM is in the center of the complex"
#+ "while other clouds surround it.")
# generate the center point of each cloud. the PDM is in the center of the complex
# while other clouds surround it.
nClouds = np.random.randint(mods.SAPs.THT_MIN_NUM_CLOUDS,
mods.SAPs.THT_MAX_NUM_CLOUDS + 1)
rCloud = np.zeros((nClouds, 3))
thtRadius = mods.UniformRandRange(mods.SAPs.THT_MIN_RADIUS,
mods.SAPs.THT_MAX_RADIUS)[0]
for iCloud in range(nClouds):
# compute the position of the cloud w.r.t. the PDM at the final time
delR = np.random.rand(1, 3)
delR = delR / np.linalg.norm(delR)
delR = delR * np.absolute(np.random.randn() * thtRadius)
rCloud[iCloud] = rPdmFinal + delR
#print("generate the position of each threat at time = tFinal")
# generate the position of each threat at time = tFinal
rDeviation = mods.SAPs.THT_POS_DEV * np.random.rand()
nThreats = np.random.randint(mods.SAPs.THT_MIN_NUM_OBJS,
mods.SAPs.THT_MAX_NUM_OBJS + 1)
threatStates = np.zeros((nThreats, 7))
for iThreat in range(nThreats):
# determine which cloud this threat belongs to
iCloud = np.random.randint(0, nClouds)
# determine it's final state
rFinal = rCloud[iCloud] + np.random.randn(1, 3) * rDeviation
sFinal, _ = mods.GaussProblem(rFinal, sPdmInitial[0:3],
mods.SAPs.MDL_T_DURATION)
vFinal = -sFinal[3:6]
threatStates[iThreat, 0:3] = rFinal
threatStates[iThreat, 3:6] = vFinal
threatStates[iThreat, 6] = tFinal
# make cvState
tmpCVState = np.concatenate(
(rCVFinal.reshape(1, 3), vCVFinal.reshape(1, 3)), axis=1)
tmpCVState = np.append(tmpCVState, [tFinal])
cvState = mods.PropagateECI(tmpCVState.reshape(7, 1), 0.0,
-mods.SAPs.MDL_T_DURATION)
# print statistics to the screen
print("# Threat Objects = {}".format(nThreats))
print("# Threat Clouds = {}".format(nClouds))
print("Threat Divergence = {} m/s".format(mods.SAPs.THT_VEL_DEV))
print("EV Radius = {} m".format(thtRadius))
# release results
return cvState, threatStates, thtRadius
def KinematicReach(cvState, kvStates, kvFuel, threatStates):
# Numbers
nKVs = kvStates.shape[0]
nThreats = threatStates.shape[0]
if nKVs != nThreats:
print("number of KVs: {}".format(nKVs))
print("number of threats: {}".format(nThreats))
# Find the POCA between the CV and the center of the threat complex
aveState = np.mean(threatStates, axis=0)
tPOCA, rPOCA, _ = mods.FindPOCA(cvState, aveState)
# Propagate all threats to tPOCA
tGo = tPOCA - threatStates[0, 6]
threatStatesPOCA = np.zeros(threatStates.shape)
for iThrt in range(nThreats):
threatStatesPOCA[iThrt] = mods.PropagateECI(threatStates[iThrt].reshape(7,1), 0.0, tGo)
# Build the CV coordinate system to work in (and corresponding transformations)
rCV_eci = cvState[0:3]
vCV_eci = cvState[3:6]
# c1 = vCV_eci'
c1 = (rPOCA - rCV_eci) / np.linalg.norm( (rPOCA - rCV_eci) )
c2 = np.cross(np.array([0, 0, 1]), c1) / np.linalg.norm( np.cross(np.array([0, 0, 1]), c1) )
c3 = np.cross(c1, c2) / np.linalg.norm( np.cross(c1, c2) )
tCV2ECI = np.hstack( (c1.reshape(3,1), c2.reshape(3,1), c3.reshape(3,1)) )
#tECI2CV = np.transpose( tCV2ECI ) rotR*vec == vec*rotL so I have made some modifications below
# Put the KVs and threats in the CV reference frame
rKV_cv = np.zeros((nKVs, 3))
vKV_cv = np.zeros((nKVs, 3))
for iKV in range(nKVs):
rKV_cv[iKV] = np.dot( kvStates[iKV, 0:3] - rCV_eci, tCV2ECI) # same as (tECI2CV*kvStates')'
vKV_cv[iKV] = np.dot( kvStates[iKV, 3:6] - vCV_eci, tCV2ECI) # same as (tECI2CV*kvStates')'
rTht_cv = np.zeros((nThreats, 3))
for iThrt in range(nThreats):
rTht_cv[iThrt] = np.dot( threatStatesPOCA[iThrt, 0:3] - rPOCA, tCV2ECI )
# Determine the Kinematic feasibility of each KV/Threat pair
krMatrix = np.zeros((nKVs, nThreats))
dvMatrix = np.zeros((nKVs, nThreats))
for iKV in range(nKVs):
for iThrt in range(nThreats):
# New Method
dV, tBurn = RequiredDivert(
kvStates[iKV, 6],
tPOCA,
rKV_cv[iKV],
vKV_cv[iKV],
rTht_cv[iKV],
mods.SAPs.KV_MAX_ACC)
if tBurn < tGo and (kvFuel[iKV] - dV - mods.SAPs.KV_FUEL_RESERVE) > 0:
krMatrix[iKV, iThrt] = 1
dvMatrix[iKV, iThrt] = dV
return krMatrix, dvMatrix
def CorrelateTOM(rfStates, rfCov, rfIds, irStates, irCov, irIds, nKVs, rvID, kvStates):
# set constants
nRF = rfStates.shape[0]
nIR = irStates.shape[0]
# find the latest time in the states
rfLatest = max(rfStates[:,6])
irLatest = max(irStates[:,6])
tLatest = max(rfLatest, irLatest)
# propagate RF states to T = tLatest
RF_objects = np.zeros((nRF, 4))
RF_cov = np.zeros((nRF, 3, 3))
for ii in range(nRF):
propState, propCov = mods.PropagateECICov(rfStates[ii, 0:7], rfCov[ii], tLatest - rfStates[ii, 6], 2.0)
RF_objects[ii, 0] = rfIds[ii]
RF_objects[ii, 1:4] = propState[0:3]
RF_cov[ii] = propCov[0:3, 0:3]
# move the RV to top of the list in RF tracks
rvInd = np.where(rfIds==rvID)[0]
RF_objects = swapMat(RF_objects, 0, rvInd[0])
RV_cov = swapMat(RF_cov, 0, rvInd[0])
# propagate IR states to T = tLatest
IR_objects = np.zeros((nKVs, nIR, 4))
IR_cov = np.zeros((nKVs, nIR, 3, 3))
for jj in range(nKVs):
for ii in range(nIR):
propState, propCov = mods.PropagateECICov(irStates[ii, 0:7], irCov[ii],
tLatest - irStates[ii, 6], 2.0)
IR_objects[jj, ii, 0] = irIds[ii]
IR_objects[jj, ii, 1:4] = propState[0:3]
IR_cov[jj, ii] = propCov[0:3, 0:3]
# propagate KV states to T = tLatest
KV_pos = np.zeros((nKVs, 3))
for ii in range(nKVs):
propState = mods.PropagateECI(kvStates[ii, 0:7], 0.0, tLatest - kvStates[ii, 6])
KV_pos[ii] = propState[0:3]
# unique IDs
uniqueIds = np.intersect1d(irIds, rfIds)
# TOM correlation
return TomCorrelation(nKVs, uniqueIds.size, RF_objects, IR_objects, RV_cov, IR_cov, KV_pos)
def MakeTOM(threatStates, rvID, tTOM, pLethal, fracObjsSeen, unpack=False):
# pick which threats are seen by the radar
nThreats = int(threatStates.shape[0])
nThreatsSeen = int(np.round(fracObjsSeen * nThreats))
randIds = np.random.permutation(np.arange(nThreats))
thrtIds = randIds[0:nThreatsSeen]
# make sure the RV is in the TOM
if not thrtIds.__contains__(rvID):
indx = np.random.randint(thrtIds.shape[0])
thrtIds[indx] = int(rvID)
# propagate threat states to tTOM (truth data)
truthThreatStates = np.zeros(threatStates.shape)
for iThrt in range(nThreats):
dT = tTOM - threatStates[iThrt, 6]
truthThreatStates[iThrt] = mods.PropagateECI(threatStates[iThrt], 0.0,
dT)
# now build the TOM
tom = np.zeros((nThreatsSeen, 9))
tomCov = np.zeros((nThreatsSeen, 6, 6))
biasR = np.random.randn(3) * (mods.SAPs.RDR_POS_BIAS / np.sqrt(3.0))
biasV = np.random.randn(3) * (mods.SAPs.RDR_VEL_BIAS / np.sqrt(3.0))
indx = 0
for iThrt in thrtIds:
sigR = np.random.rand(3) * (mods.SAPs.RDR_POS_BIAS / np.sqrt(3.0))
sigV = np.random.rand(3) * (mods.SAPs.RDR_VEL_BIAS / np.sqrt(3.0))
tom[indx, 0:3] = sigR + biasR + truthThreatStates[iThrt, 0:3]
tom[indx, 3:6] = sigV + biasV + truthThreatStates[iThrt, 3:6]
tom[indx, 6] = tTOM
tom[indx, 7] = pLethal[iThrt]
tom[indx, 8] = int(iThrt)
covariance = np.append((sigR + biasR)**2, (sigV + biasV)**2)
tomCov[indx] = np.diag(covariance)
indx += 1
finalTOM = mods.TOMObject(tom, tomCov, thrtIds)
# pass out tom information
if unpack:
return tom, tomCov, thrtIds
else:
return finalTOM
def runSim():
# set up the scenario
kvStates = np.zeros((mods.SAPs.KV_NUM_KVS, 7))
kvFuel = np.ones(mods.SAPs.KV_NUM_KVS) * mods.SAPs.KV_MAX_DIVERT
[
cvState, cvFuel, threatStates, rvID, tankID, tPOCA, rPOCA, vClose,
aClose, thtRadius
] = mods.InitializeScenario()
nThreats = threatStates.shape[0]
assignedKVs = -1 * np.zeros(mods.SAPs.KV_NUM_KVS)
assignedThreats = -1 * np.ones(mods.SAPs.KV_NUM_KVS)
# begin the simulation
kvsDispensed = False
kvsDeployed = False
tomRecvd = False
irThreatsTracked = False
t = mods.SAPs.MDL_T_START
tFinal = t + mods.SAPs.MDL_T_DURATION + 1.0
tStep = mods.SAPs.MDL_TIMESTEP
lastAssign = False
exceptionCounter = 0
startTime = time.clock()
while t < tFinal:
# increment the time
t += tStep
elapsedTime = time.clock() - startTime
print("Processing time step t = {}".format(t))
print("current elapsed time = {}s".format(elapsedTime))
# propagate the CV and KVs to the nearest time
cvState = mods.PropagateECI(cvState, 0.0, tStep)
if kvsDispensed:
for iKV in range(kvStates.shape[0]):
kvStates[iKV] = mods.PropagateECI(kvStates[iKV], 0.0, tStep)
# propagate threats to the nearest time
for iThrt in range(nThreats):
threatStates[iThrt] = mods.PropagateECI(threatStates[iThrt], 0.0,
tStep)
# create the TOM at the appropriate time (one time only)
if tFinal - t >= mods.SAPs.RDR_TOM_TGO:
pLethalGround = mods.DiscriminationGround(nThreats, rvID,
mods.SAPs.RDR_KFACTOR)
if not tomRecvd and tFinal - t <= mods.SAPs.RDR_TOM_TGO:
tomRecvd = True
# Nx9, [x y z dx dy dz t P(RV) ID]
tom, tomCov, tomIds = mods.MakeTOM(
threatStates,
rvID,
t,
pLethalGround,
mods.SAPs.RDR_FRAC_TRACKS_DETECTED,
unpack=True)
# create tracks local to the CV/KV complex and perform TOM matching
if tFinal - t <= mods.SAPs.CV_TGO_SENSOR_ON:
pLethalOnboard = mods.DiscriminationOnboard(
nThreats, rvID, mods.SAPs.CV_KFACTOR)
onboardThreats, onboardThreatsCov, onboardThreatIds = mods.MakeOnboardThreats(
threatStates, rvID, pLethalOnboard,
mods.SAPs.CV_FRAC_TRACKS_DETECTED)
# update the onboard threats with results of TOM matching TODO
tomCorrData, winner = mods.CorrelateTOM(
tom, tomCov, tomIds, onboardThreats, onboardThreatsCov,
onboardThreatIds, mods.SAPs.KV_NUM_KVS, rvID, kvStates)
print("tomCorrData shape: {}".format(
np.asarray(tomCorrData).shape))
print("winner from tomCorrelation: {}".format(winner))
if not irThreatsTracked:
irThreatTracker = mods.zipArray(
(onboardThreatIds, np.ones_like(onboardThreatIds)))
irThreatsTracked = True
else:
pass
# update lethality measurement with correlated ground and TOM threats
if winner.size > 0:
rfIndex = int(winner[0])
irIndex = int(winner[1])
realIndex = np.where(irThreatTracker[:, 0] == irIndex)[0]
try:
irLocation = np.amin(realIndex)
except ValueError:
exceptionCounter += 1
print(
"\nEXCEPTION OCCURRED {}: \nirIndex: {}, rfIndex: {}".
format(exceptionCounter, irIndex, rfIndex))
print("available ifIndices: {}\n".format(
irThreatTracker[:, 0]))
irLocation = 0
weights = np.array([irThreatTracker[irLocation][1], 1])
irThreatTracker[irLocation, 1] += 1
print("original lethality estimate: {}".format(
pLethalOnboard[irIndex]))
pLethalTmp = np.array(
[pLethalOnboard[irIndex], pLethalGround[rfIndex]])
pLethalOnboard[irIndex] = np.average(pLethalTmp,
weights=weights)
print(" updated lethality estimate: {}".format(
pLethalOnboard[irIndex]))
# dispense the KVs (when its time)
# TODO investigate multiple dispenses (e.g. 1st for furthest threat
# clusters)
# TODO give time for start shot
if tFinal - t <= mods.SAPs.KV_BATTERY_LIFE and not kvsDispensed:
kvStates = mods.DispenseKVs(cvState)
kvsDispensed = True
tDispense = t
# deploy KVs (initial for zone defense)
if kvsDispensed and not kvsDeployed and t > tDispense + 10.0:
## use the TOM to arrange KVs for intercept
try:
ind = int(np.where(tomIds == rvID))
rvIdTmp = int(tomIds[ind])
except TypeError:
rvIdTmp = 0
#pRVs = tom[:, 7]
# need to grab the tomStates that are correlated with onboardThreat states
# currently just taking the array for the 1st KV, need to figure out if this
# is supposed to be coordinated with the winner index returned from correlateTOM
#try:
# tomIds = tomCorrData[0][:,0]
#except UnboundLocalError:
# tomIds = tom[:,8].astype(int)
tomIds = tom[:, 8].astype(int)
# use tomIds to create tomStates variable
tomSelectionCondition = [
np.asscalar(np.where(tom[:, 8] == selectedId)[0])
for selectedId in tomIds
]
redTOM = tom[tomSelectionCondition]
tomStates = np.zeros((redTOM.shape[0], 7))
for iTomThrt in range(redTOM.shape[0]):
tomStates[iTomThrt] = mods.PropagateECI(
redTOM[iTomThrt, 0:7], 0.0, t - redTOM[iTomThrt, 6])
# DeployKVs
cvState, cvFuel, kvStates, kvFuel, clusters = mods.DeployKVsTOM(
cvState, cvFuel, kvStates, kvFuel, tomStates, tomIds,
pLethalGround, tPOCA, rvIdTmp, 'aH5')
# spread KVs around the threat radius for flexibility
# [cvState, cvFuel, kvStates, kvFuel] = mods.DeployKVsNoTOM(cvState, cvFuel, ...
# kvStates, kvFuel, threatStates, rPOCA, tPOCA, rvID, thtRadius, aH4);
kvsDeployed = True
#monitor the kinematic reach of the KVs
if kvsDeployed:
# TODO: check this with Larry
# I think this is where tomCorrData comes in, it maps tomStates to the states seen by
# the CV. So I think the threatStates here really need to be the subset of paired threats
# that both the TOM and CV have information about. If this is incorrect let me know.
#try:
# correlatedThreats = threatStates[ tomCorrData[0][:,1].astype(int) ]
#except UnboundLocalError:
# correlatedThreats = threatStates
correlatedThreats = threatStates.copy()
# apply result to kinematic reach info
krMatrix, dVMatrix = mods.KinematicReach(cvState, kvStates, kvFuel,
correlatedThreats)
# perform weapon-target pairing
if kvsDeployed and not lastAssign:
pkMat = krMatrix * mods.SAPs.WTA_AVE_KV_PK
pkCond = (
pkMat <= 0
) # boolean matrix - True if pkMat[i,j] <= 0 (it shouldn't)
pkMat[
pkCond] = 0.0 # replaces pkMat[i, j] <= 0 with 0 (unneccessary?)
lethality = np.tile(pLethalGround, (mods.SAPs.KV_NUM_KVS, 1))
costMat = lethality * pkMat
costMat = (1 - costMat) * krMatrix
costCond = (costMat == 0
) # boolean matrix - True if costMat[i,j] == 0.0
costMat[costCond] = 1.0 # replaces costMat[i,j] == 0.0 with 1.0
# refine costMatrix based on assignedKVs to avoid assigning KVs that have already been assigned
costMat[(assignedKVs >= 0)] = 1
dVMatrix[(assignedKVs >= 0)] = 0
pLethalRed = pLethalGround[
(assignedThreats != 0)] * (1.0 - mods.doctrine.avePk)
# use Munkres alg to make assignment
planMat = mods.munkres(costMat)
#VisualizeWTA(planMat, ~krMatrix, pLethalGround, [], [], [], [], aH6)
# final assignment determination (kv-by-kv basis)
inThreatIds = np.arange(threatStates.shape[0])
finalAssignment, kvStates = mods.FinalAssignment(
planMat, kvStates, kvFuel, threatStates, inThreatIds, 5.0)
# incprporate finalAssign results
assignedKVs = np.logical_or(assignedKVs, finalAssignment)
# finalize assignment solutions
reassignmentCondition = ((assignedThreats == -1) &
(assignedKVs != 0))
assignedThreats[reassignmentCondition] = finalAssignment[
reassignmentCondition]
# perform terminal homing:
kvStates = mods.TerminalHoming(assignedThreats, kvStates,
threatStates)
return kvStates
def DeployKVsTOM(cvState, cvFuel, kvStates, kvFuel, threatStates, threatIds, pLethal, tPOCA, rvID, aH1):
# cluster the threats into manageable regions
clusterList = mods.Cluster(cvState, kvFuel, threatStates, threatIds, tPOCA, aH1)
# Now compute the number of KVs to send to each cluster based on the number of
# threats and their lethality ranking
nClusters = len(clusterList)
pLethalNorm = pLethal / np.sum(pLethal)
weights = np.zeros(nClusters)
indx = 0
for cluster in clusterList:
weights[indx] = np.sum( pLethal[cluster.ids] )
indx += 1
nAssigned = np.round(weights * mods.SAPs.KV_NUM_KVS)
# If there are any left over, assign them to the cluster with the largest number of threats.
# If there are too many assigned, get rid of one with the most assignments.
nLeftOver = mods.SAPs.KV_NUM_KVS - sum(nAssigned)
if nLeftOver != 0:
if nLeftOver < 0 and nAssigned.size > 0:
#adjIndex = np.unravel_index(nAssigned.argmax(), dims=nAssigned.shape)
adjIndex = nAssigned.argmax()
elif nLeftOver > 0 and nAssigned.size > 0:
#adjIndex = np.unravel_index(weights.argmax(), dims=nAssigned.shape)
adjIndex = weights.argmax()
# adjust nAssigned accordingly
try:
nAssigned[adjIndex] = nAssigned[adjIndex] + nLeftOver
except UnboundLocalError:
print("something might have broke? \n nAssigned: {} \n nLeftOver: {}".format(nAssigned, nLeftOver))
# Now divide each cluster into the same number of clusters as there are KVs assigned to it.
# Compute their centers as well (with k-means clustering)
clusterId = 0
nKV_total = mods.SAPs.KV_NUM_KVS
nKV = 0
kvPIP = np.zeros_like(kvStates)
kvPOCA = np.zeros((kvStates.shape[0], 3))
kvClust = np.zeros_like(kvStates, dtype=int)
kvKillRadius = np.zeros(kvStates.shape[0])
for cluster in clusterList:
nKVsAssigned = int( nAssigned[clusterId] )
if nKVsAssigned > 0:
pts = cluster.pts
nPts = pts.shape[0]
if nKVsAssigned == 1 or nPts == 1:
icx = np.zeros(1,int)
mu = np.average(pts, axis=0)
else:
mu, icx = mods.kMeans(pts, int( min(pts.shape[0], nKVsAssigned) ))
# build the PIP state for each KV
clstIds = np.unique(icx)
print("clusterIds: {}".format(clstIds))
nClsts = clstIds.shape[0]
kvStart = nKV
for iKV in clstIds:
print("nKV: {}, iKV: {}, nKVs: {}, nClsts: {}".format(
nKV, iKV, kvStates.shape[0], clstIds.shape[0]))
nKVmod = nKV % nKV_total
kvPIP[nKVmod, 0:3] = mu[iKV]
kvClust[nKVmod] = iKV
nKV += 1
# find out how many KVs still need to be assigned
nKVsLeftOver = nKVsAssigned - nClsts
for iKV in range(nKVsLeftOver):
nKVmod = nKV % nKV_total
kvPIP[nKVmod, 0:3] = cluster.ctr
kvClust[nKVmod] = clusterId
nKV += 1
# Perform the KV diverst
# tStates = cluster.states
# centroid = np.average(tStates, axis=1)
# divert the KVs to their guard positions
for iKV in range(kvStart, kvStart + nKVsAssigned):
iKV = iKV % nKV_total
tGo = cluster.tPOCA - kvStates[iKV, 6]
if np.linalg.norm(kvStates[iKV, 0:3]) == 0 or np.linalg.norm(kvPIP[iKV, 0:3]) == 0:
print("Trying to use GaussProblem on an origin vector.")
print("pos1: {}, pos2: {}".format(kvStates[iKV, 0:3], kvPIP[iKV]))
kv = mods.PropagateECI(kvStates[iKV], 0.0, tGo)
poca = mods.PropagateECI(kvPIP[iKV], 0.0, tGo)
else:
kv, poca = mods.GaussProblem(kvStates[iKV, 0:3], kvPIP[iKV, 0:3], tGo)
# calculate dV
dV = kv[3:6] - kvStates[iKV, 3:6]
kvStates[iKV, 3:6] = kvStates[iKV, 3:6] + dV
kvFuel[iKV] = kvFuel[iKV] - np.linalg.norm(dV)
kvPOCA[iKV] = poca[0:3]
# create kvKillRadius vector:
kvKillRadius = (kvFuel - mods.SAPs.KV_FUEL_RESERVE) * mods.SAPs.CV_FINAL_ASSGN_TGO
return cvState, cvFuel, kvStates, kvFuel, clusterList
def findClosingValues(inputDictionary={}):
print("INPUT DICTIONARY", inputDictionary)
tStart = inputDictionary.get('tStart')
if tStart == None:
tStart = mods.SAPs.MDL_T_START
tFinal = inputDictionary.get('tFinal')
if tFinal == None:
tFinal = tStart + mods.SAPs.MDL_T_DURATION
rCVFinal = np.array([-7, 0, 0]) * 1e6 # final CV position [ECI, m]
cvSpeed = mods.UniformRandRange(mods.SAPs.CV_SPEED_MIN,
mods.SAPs.CV_SPEED_MAX)
vCVFinal = np.array([0, 1, 0]) * cvSpeed # final CV velocity [ECI, m]
rPdmFinal = np.array([
-7, 0, 0
]) * 1e6 # final Payload Deployment Module (PDM) position, [ECI, m]
inBounds = False
while not inBounds:
v = np.array([0, np.random.randn(), np.random.rand()])
v = v / np.linalg.norm(v)
theta = mods.AngleBetweenVecs(vCVFinal, v)
if theta > mods.SAPs.CV_CLOSE_ANGLE_MIN and theta <= mods.SAPs.CV_CLOSE_ANGLE_MAX:
inBounds = True
thtSpeed = mods.UniformRandRange(mods.SAPs.THT_SPEED_MIN,
mods.SAPs.THT_SPEED_MAX)
vPdmFinal = v * thtSpeed
vClose = np.linalg.norm(vPdmFinal - vCVFinal)
aClose = mods.AngleBetweenVecs(vPdmFinal, vCVFinal)
cvState = inputDictionary.get('cvState')
threatStates = inputDictionary.get('threatStates')
thtRadius = inputDictionary.get('thtRadius')
# perform initial CV divert
if cvState is None or threatStates is None:
print("CAUTION: CV State or ThreatStates is None. Generating States")
cvState, threatStates, thtRadius = generateStates(
rPdmFinal, vPdmFinal, rCVFinal, vCVFinal, tFinal)
nThreats = threatStates.shape[0]
if thtRadius == None:
thtRadius = mods.UniformRandRange(mods.SAPs.THT_MIN_RADIUS,
mods.SAPs.THT_MAX_RADIUS)[0]
if inputDictionary.get('rvID') == None:
rvID = np.random.randint(nThreats)
if inputDictionary.get('tankID') == None:
tankID = np.random.randint(nThreats)
# compute the average state
aveState = np.sum(threatStates, axis=0) / nThreats
aveState = mods.PropagateECI(aveState.reshape(7, 1), 0.0,
-mods.SAPs.MDL_T_DURATION)
for iThreat in range(nThreats):
threatStates[iThreat] = mods.PropagateECI(
threatStates[iThreat].reshape(7, 1), 0.0,
-mods.SAPs.MDL_T_DURATION)
tPOCA, rPOCA, _ = mods.FindPOCA(cvState, aveState)
#print("findPOCA executed, passing on to GaussProblem...")
cv, _ = mods.GaussProblem(cvState[0:3], rPOCA, tPOCA - cvState[6])
dV = cv[3:6] - cvState[3:6]
# tBurn1 = np.linalg.norm(dV) / mods.SAPs.CV_MAX_ACC
cvState[3:6] = cvState[3:6] + dV
cvFuel = mods.SAPs.CV_MAX_DIVERT - np.linalg.norm(dV)
# print statistics to the screen
print("Closing Velocity = {} m/s".format(vClose))
print("Closing Angle = {} deg".format(aClose * 180 / np.pi))
print("rvID = {}".format(rvID))
# return outputs
return {
"cvState": cvState,
"cvFuel": cvFuel,
"threatStates": threatStates,
"rvID": rvID,
"tankID": tankID,
"tPOCA": tPOCA,
"rPOCA": rPOCA,
"vClose": vClose,
"aClose": aClose,
"thtRadius": thtRadius
}
def Cluster(cvState, kvFuel, threatStates, threatIds, tPOCA, aH1):
# propagate the threats to tPOCA
nThreats = threatStates.shape[0]
threatStatesPOCA = np.zeros(threatStates.shape)
iPOCA = 0
for thrtState in threatStates:
threatStatesPOCA[iPOCA] = mods.PropagateECI(thrtState, 0.0,
tPOCA - thrtState[-1])
iPOCA += 1
# find the cloud center and separate the positions at POCA for clustering
dVec = threatStatesPOCA[:, 0:3]
center = np.mean(dVec, axis=0)
centerSave = center.copy()
# perform clustering
tGo = tPOCA - cvState[6]
redThreatStates = threatStates.copy()
delIds = np.empty(0, dtype=int)
clusterList = []
while dVec.size > 0:
cluster = {}
# find the object furthest from centroid of object map
dCnt, farId, _ = FarPoint(dVec, center)
# compute the required delta-V for a "virgin" KV to fly there
tPOCAt, rPOCAt, _ = mods.FindPOCA(cvState, redThreatStates[farId])
if mods.vecNorm(cvState[0:3]) == 0 or mods.vecNorm(rPOCAt) == 0:
print("Trying to use GaussProblem on an origin vector.")
print("pos1: {}, pos2: {}".format(cvState[0:3], rPOCAt))
cv = mods.PropagateECI(cvState, 0.0, tPOCA - cvState[6])
else:
cv, _ = mods.GaussProblem(cvState[0:3], rPOCAt,
tPOCAt - cvState[6])
# compute the Delta-V required to get there for a KV
dV = mods.vecNorm(cv[3:6] - cvState[3:6])
# if the required delta-V is too large, get rid of this threat from the list and start
# over. The assumption is that KVs have just been dispensed or are not yet dispensed.
# KVs have approximately the same state as the CV, and no fuel has been burned.
dVAfterDivert = kvFuel[0] - dV - mods.SAPs.KV_FUEL_RESERVE
tBurn = dV / mods.SAPs.KV_MAX_ACC
# remove the threats from the threat list that are too far away
if dVAfterDivert <= 0 or tBurn >= tGo:
# keep track of which global ids were removed
delIds = np.append(delIds, threatIds[farId])
# remove non-viable threat from consideration
redThreatStates = np.delete(redThreatStates, farId, 0)
threatStatesPOCA = np.delete(threatStatesPOCA, farId, 0)
threatIds = np.delete(threatIds, farId, 0)
dVec = np.delete(dVec, farId, 0)
# skip remaining code and re-evaluate loop on the next point
continue
# compute the killRadius on the qualified KV
killRadius = dVAfterDivert * mods.SAPs.CV_FINAL_ASSGN_TGO
# compute the center of all threat objects within 'killRadius' of this object
# and iterate for a better fit
dCnt, _ = ReCenter(dVec, dCnt, killRadius)
diffCtr = np.zeros(3)
while mods.vecNorm(dCnt - diffCtr) > 0.1 and dVec.size > 1:
# Re-center
diffCtr = dCnt.copy()
dCnt, _ = ReCenter(dVec, dCnt, killRadius)
# Save as the cluster center location
cluster.update({"ctr": dCnt})
# Remove all the objects within 'killRadius' of this object from the object map
killIds = np.empty(0, dtype=int)
noKillIds = np.empty(0, dtype=int)
# calculate distance between each point and the object location
difference = np.subtract(dVec, dCnt)
distance = mods.vecNorm(difference, axis=1)
indx = 0
for dist in distance:
if dist >= killRadius:
noKillIds = np.append(noKillIds, indx)
else:
killIds = np.append(killIds, indx)
indx += 1
# create clusters from the objects corresponding to killIds
if killIds.size > 0:
cluster.update({"pts": dVec[killIds]})
cluster.update({"states": threatStatesPOCA[killIds]})
cluster.update({"ids": threatIds[killIds]})
cluster.update({"tPOCA": tPOCA, "kr": killRadius})
# add current cluster as a SimpleNamespace object to clusterList
clusterList.append(mods.SimpleNamespace(cluster))
# update dVec, threatIds, redThreatStates, threatStatesPOCA
# corresponding objects in noKillIds
dVec = np.delete(dVec, killIds, 0)
redThreatStates = np.delete(redThreatStates, killIds, 0)
threatStatesPOCA = np.delete(threatStatesPOCA, killIds, 0)
threatIds = np.delete(threatIds, killIds, 0)
else:
break
# return clusters to be used outside of the code
return clusterList