def testAddStates(self):
# Create a simple state class to add to the filter
stateLength1 = np.random.randint(0, 10)
cov1 = np.random.randint(0, 10)
state1 = self.simpleState(
stateLength1,
{
't':0,
'stateVector':np.zeros(stateLength1),
'covariance':np.eye(stateLength1)*cov1,
'stateVectorID':0
}
)
stateLength2 = np.random.randint(0, 10)
cov2 = np.random.randint(1, 10)
state2 = self.simpleState(
stateLength2,
{
't':0,
'stateVector':np.zeros(stateLength2),
'covariance':np.eye(stateLength2)*cov2,
'stateVectorID':0
}
)
myFilter = md.ModularFilter()
myFilter.addStates('state1', state1)
myFilter.addStates('state2', state2)
self.assertEqual(stateLength1 + stateLength2, myFilter.totalDimension)
stackedCov = block_diag(np.eye(stateLength1) * cov1, np.eye(stateLength2) * cov2)
self.assertTrue(np.all(stackedCov == myFilter.covarianceMatrix.value))
with self.assertRaises(ValueError):
myFilter.addStates('state1', state2)
eulerT0Est = eulerT0True
eulerT0Est = np.array([
np.random.normal(eulerT0True[0], rollErrorStd),
np.random.normal(eulerT0True[1], DecErrorStd),
np.random.normal(eulerT0True[2], RAErrorStd)
])
biasEst = np.random.normal(biasTrue, scale=biasErrorSTD)
biasEst = np.array([0, 0, 0])
q0 = euler2quaternion(eulerT0True)
QScalar = 1e-12
RScalar = 1e-6
# Initiate filters
myJPDAF = me.ModularFilter(measurementValidationThreshold=0,
covarianceStorage='covariance')
myEKF = me.ModularFilter(covarianceStorage='covariance')
myML = me.ModularFilter(measurementValidationThreshold=1e-3,
covarianceStorage='covariance')
myTUOnly = me.ModularFilter(covarianceStorage='covariance')
initialAttitudeCovariance = np.zeros([3, 3])
initialAttitudeCovariance[0, 0] = np.square(rollErrorStd)
initialAttitudeCovariance[1, 1] = np.square(DecErrorStd)
initialAttitudeCovariance[2, 2] = np.square(RAErrorStd)
# initialAttitudeCovariance = attitude(0).rotation_matrix.transpose().dot(initialAttitudeCovariance).dot(attitude(0).rotation_matrix)
# Initiate attitude stubstates
JPDAFAtt = me.substates.Attitude(euler2quaternion(eulerT0Est),
initialAttitudeCovariance,
np.zeros(3),
orbitAmplitude * np.cos(t / orbitPeriod),
orbitAmplitude * np.sin(t / orbitPeriod), 0 * t
]))
def velocity(t):
return (
(orbitAmplitude / orbitPeriod) *
np.array([-np.sin(t / orbitPeriod),
np.cos(t / orbitPeriod), 0 * t]))
corrSubstateDict = {}
photonMeasurements = []
myFilter = md.ModularFilter(covarianceStorage=covarianceStorageMethod)
# myPointSource = md.signals.StaticXRayPointSource(
# pulsarRaDec['RA'] + 0.001,
# pulsarRaDec['DEC'],
# photonEnergyFlux=1e-15,
# name='Dummy'
# )
pointSourceObjectDict = {}
for signalIndex in range(len(pointSources)):
myRow = pointSources.iloc[signalIndex]
myRa = md.utils.spacegeometry.hms2rad(hms=myRow['ra'])
myDec = md.utils.spacegeometry.dms2rad(dms=myRow['dec'])
try:
def generalizedTestRun(self, measUpdateMethod='EKF'):
A1 = np.random.randint(10,100)
A2 = np.random.randint(10,100)
def pos1(t):
return A1 * np.sin(t/10)
def vel1(t):
return A1 * np.cos(t/10)/10
def acc1(t):
return -A1 * np.sin(t/10)/100
def pos2(t):
return A2 * np.cos(t/10)
def vel2(t):
return -A2 * np.sin(t/10)/10
def acc2(t):
return -A2 * np.cos(t/10)/100
myFilter = md.ModularFilter()
myFilterChol = md.ModularFilter(covarianceStorage='cholesky')
position1 = np.random.normal(pos1(0),1)
velocity1 = np.random.normal(vel1(0),1)
x1 = np.array([position1, velocity1])
cov1 = np.eye(2) * np.abs(np.random.normal(2))
positionObj1 = self.oneDPositionVelocity(
'object1',
{'t': 0,
'stateVector': np.array([position1, velocity1]),
'covariance': cov1,
'stateVectorID': 0
}
)
myFilter.addStates('object1', positionObj1)
positionObj1Chol = self.oneDPositionVelocity(
'object1',
{'t': 0,
'stateVector': np.array([position1, velocity1]),
'covariance': np.linalg.cholesky(cov1),
'stateVectorID': 0
},
covarianceStorage='cholesky'
)
myFilterChol.addStates('object1', positionObj1Chol)
signalSource1 = self.oneDObjectMeasurement('object1')
myFilter.addSignalSource('object1', signalSource1)
myFilterChol.addSignalSource('object1', signalSource1)
position2 = np.random.normal(pos2(0), 1)
velocity2 = np.random.normal(vel2(0), 1)
cov2 = np.eye(2)*np.abs(np.random.uniform(1,2))
cov2[1,0] = np.random.uniform(0,1)
cov2[0,1] = cov2[1,0]
x2 = np.array([position2, velocity2])
positionObj2 = self.oneDPositionVelocity(
'object2',
{'t': 0,
'stateVector': np.array([position2, velocity2]),
'covariance': cov2,
'stateVectorID': 0
}
)
myFilter.addStates('object2', positionObj2)
signalSource2 = self.oneDObjectMeasurement('object2')
myFilter.addSignalSource('object2', signalSource2)
positionObj2Chol = self.oneDPositionVelocity(
'object2',
{'t': 0,
'stateVector': np.array([position2, velocity2]),
'covariance': np.linalg.cholesky(cov2),
'stateVectorID': 0
},
covarianceStorage='cholesky'
)
myFilterChol.addStates('object2', positionObj2Chol)
myFilterChol.addSignalSource('object2', signalSource2)
self.assertEqual(myFilterChol.covarianceMatrix.form, 'cholesky')
self.assertEqual(myFilter.covarianceMatrix.form, 'covariance')
cov = block_diag(positionObj1.covariance().value, positionObj2.covariance().value)
x = np.append(positionObj1.stateVector, positionObj2.stateVector)
subQ = np.array([[1/4, 1/2],[1/2, 1]])
FMat = np.array([[1,1],[0,1]])
FMat = block_diag(FMat, FMat)
for time in range(0,100,1):
# Do dynamics update with randomly generated sigmas for acceleration
sigmaA1 = np.random.uniform(0.1,1)
sigmaA2 = np.random.uniform(0.1,1)
a1meas = np.random.normal(acc1(time), sigmaA1)
a2meas = np.random.normal(acc2(time), sigmaA1)
dynamics={
'object1acceleration': {
'value': a1meas,
'var': np.square(sigmaA1)
},
'object2acceleration': {
'value': a2meas,
'var': np.square(sigmaA2)
}
}
myFilter.timeUpdateEKF(1, dynamics=dynamics)
myFilterChol.timeUpdateEKF(1, dynamics=dynamics)
Q = block_diag(subQ * np.square(sigmaA1), subQ * np.square(sigmaA2))
cov =FMat.dot(cov).dot(FMat.transpose()) + Q
x = FMat.dot(x) + np.array([0, a1meas, 0, a2meas])
self.assertTrue(
np.allclose(
myFilter.covarianceMatrix.value,
cov
)
)
self.assertTrue(
np.allclose(
myFilterChol.covarianceMatrix.convertCovariance('covariance').value,
cov
)
)
self.assertTrue(
np.allclose(
myFilter.getGlobalStateVector(),
x
)
)
self.assertTrue(
np.allclose(
myFilterChol.getGlobalStateVector(),
x
)
)
# Now, generate random measurement matrices
measIndicator = np.random.randint(0,2, size=4)
#measIndicator = np.array([1,0,1,1])
H = None
R = None
state1Meas = {}
if measIndicator[0]:
subH = np.array([1, 0, 0, 0])
subR = np.random.uniform(0.1,1)
y = np.random.normal(pos1(time), np.sqrt(subR))
if H is None:
H = subH
R = np.array(subR)
yVec = np.array(y)
else:
H = np.vstack([
H,
subH
])
R = block_diag(R, subR)
yVec = np.append(yVec, y)
state1Meas['position'] = {
'value': y,
'var': subR
}
if measIndicator[1]:
subH = np.array([0, 1, 0, 0])
subR = np.random.uniform(0.1,1)
y = np.random.normal(vel1(time), np.sqrt(subR))
if H is None:
H = subH
R = np.array(subR)
yVec = np.array(y)
else:
H = np.vstack([
H,
subH
])
R = block_diag(R, subR)
yVec = np.append(yVec, y)
state1Meas['velocity'] = {
'value': y,
'var': subR
}
state2Meas = {}
if measIndicator[2]:
subH = np.array([0, 0, 1, 0])
subR = np.random.uniform(0.1,1)
y = np.random.normal(pos2(time), np.sqrt(subR))
if H is None:
H = subH
R = np.array(subR)
yVec = np.array(y)
else:
H = np.vstack([
H,
subH
])
R = block_diag(R, subR)
yVec = np.append(yVec, y)
state2Meas['position'] = {
'value': y,
'var': subR
}
if measIndicator[3]:
subH = np.array([0, 0, 0, 1])
subR = np.random.uniform(0.1,1)
y = np.random.normal(vel2(time), np.sqrt(subR))
if H is None:
H = subH
R = np.array(subR)
yVec = np.array(y)
else:
H = np.vstack([
H,
subH
])
R = block_diag(R, subR)
yVec = np.append(yVec, y)
state2Meas['velocity'] = {
'value': y,
'var': subR
}
if measUpdateMethod == 'EKF':
if H is not None:
S = H.dot(cov).dot(H.transpose()) + R
try:
K = cov.dot(H.transpose()).dot(np.linalg.inv(S))
except:
K = cov.dot(H.transpose())/S
dY = yVec - H.dot(x)
x = x + K.dot(dY)
if len(H.shape) == 1:
IminusKH = (np.eye(4) - np.outer(K,H))
cov = IminusKH.dot(cov).dot(IminusKH.transpose()) + (np.outer(K, K) * R)
else:
IminusKH = (np.eye(4) - K.dot(H))
cov = IminusKH.dot(cov).dot(IminusKH.transpose()) + K.dot(R).dot(K.transpose())
if state1Meas:
myFilter.measurementUpdateEKF(state1Meas, 'object1')
myFilterChol.measurementUpdateEKF(state1Meas, 'object1')
if state2Meas:
myFilter.measurementUpdateEKF(state2Meas, 'object2')
myFilterChol.measurementUpdateEKF(state2Meas, 'object2')
def testMeasurementUpdateJPDAF(self):
myFilter = md.ModularFilter()
myFilterChol = md.ModularFilter(covarianceStorage='cholesky')
x1 = np.array([0, 0])
cov1 = np.eye(2)
positionObj1 = self.oneDPositionVelocity(
'object1',
{'t': 0,
'stateVector': x1,
'covariance': cov1,
'stateVectorID': 0
}
)
myFilter.addStates('object1', positionObj1)
positionObj1Chol = self.oneDPositionVelocity(
'object1',
{'t': 0,
'stateVector': x1,
'covariance': np.linalg.cholesky(cov1),
'stateVectorID': 0
},
covarianceStorage='cholesky'
)
myFilterChol.addStates('object1', positionObj1Chol)
signalSource1 = self.oneDObjectMeasurement('object1')
myFilter.addSignalSource('object1', signalSource1)
myFilterChol.addSignalSource('object1', signalSource1)
cov2 = np.eye(2)
x2 = np.array([10, 10])
positionObj2 = self.oneDPositionVelocity(
'object2',
{'t': 0,
'stateVector': x2,
'covariance': cov2,
'stateVectorID': 0
}
)
myFilter.addStates('object2', positionObj2)
signalSource2 = self.oneDObjectMeasurement('object2')
myFilter.addSignalSource('object2', signalSource2)
positionObj2Chol = self.oneDPositionVelocity(
'object2',
{'t': 0,
'stateVector': x2,
'covariance': np.linalg.cholesky(cov2),
'stateVectorID': 0
},
covarianceStorage='cholesky'
)
myFilterChol.addStates('object2', positionObj2Chol)
myFilterChol.addSignalSource('object2', signalSource2)
# Generate some fake measurements which have a 50/50 chance of coming from either object
myMeas = {
'position': {
'value': 5,
'var': 1
},
'velocity': {
'value': 5,
'var':1
}
}
myFilter.measurementUpdateJPDAF(myMeas)
myFilterChol.measurementUpdateJPDAF(myMeas)
self.assertTrue(np.allclose(
myFilter.getGlobalStateVector(),
myFilterChol.getGlobalStateVector()
)
)
self.assertTrue(np.allclose(
myFilter.covarianceMatrix.value,
myFilterChol.covarianceMatrix.convertCovariance('covariance').value
)
)
def testMeasurementUpdateEKF(self):
myFilter = md.ModularFilter()
myFilterChol = md.ModularFilter(covarianceStorage='cholesky')
position1 = np.random.normal(1)
velocity1 = np.random.normal(1)
x1 = np.array([position1, velocity1])
cov1 = np.eye(2) * np.abs(np.random.normal(2))
positionObj1 = self.oneDPositionVelocity(
'object1',
{'t': 0,
'stateVector': np.array([position1, velocity1]),
'covariance': cov1,
'stateVectorID': 0
}
)
myFilter.addStates('object1', positionObj1)
positionObj1Chol = self.oneDPositionVelocity(
'object1',
{'t': 0,
'stateVector': np.array([position1, velocity1]),
'covariance': np.linalg.cholesky(cov1),
'stateVectorID': 0
},
covarianceStorage='cholesky'
)
myFilterChol.addStates('object1', positionObj1Chol)
signalSource1 = self.oneDObjectMeasurement('object1')
myFilter.addSignalSource('object1', signalSource1)
myFilterChol.addSignalSource('object1', signalSource1)
position2 = np.random.normal(1)
velocity2 = np.random.normal(1)
cov2 = np.eye(2)*np.abs(np.random.normal(2))
x2 = np.array([position2, velocity2])
positionObj2 = self.oneDPositionVelocity(
'object2',
{'t': 0,
'stateVector': np.array([position2, velocity2]),
'covariance': cov2,
'stateVectorID': 0
}
)
myFilter.addStates('object2', positionObj2)
signalSource2 = self.oneDObjectMeasurement('object2')
myFilter.addSignalSource('object2', signalSource2)
positionObj2Chol = self.oneDPositionVelocity(
'object2',
{'t': 0,
'stateVector': np.array([position2, velocity2]),
'covariance': np.linalg.cholesky(cov2),
'stateVectorID': 0
},
covarianceStorage='cholesky'
)
myFilterChol.addStates('object2', positionObj2Chol)
myFilterChol.addSignalSource('object2', signalSource2)
self.assertEqual(myFilterChol.covarianceMatrix.form, 'cholesky')
self.assertEqual(myFilter.covarianceMatrix.form, 'covariance')
# Generate postition measurement for state 1
pos1Meas = np.random.normal(x1[0])
v1Meas = np.random.normal(x1[1])
state1Meas = {
'position': {
'value': pos1Meas,
'var': 1
},
}
myFilter.measurementUpdateEKF(state1Meas, 'object1')
myFilterChol.measurementUpdateEKF(state1Meas, 'object1')
# Now compute the measurement update independently.
H = np.array([1,0])
S = H.dot(cov1).dot(H.transpose()) + np.eye(1)
K = cov1.dot(H.transpose())/(S[0,0])
# K = cov1.dot(H.transpose()).dot(np.linalg.inv(S))
dY = pos1Meas - H.dot(x1)
x1Plus = x1 + K.dot(dY)
IminusKH = (np.eye(2) - np.outer(K,H))
# IminusKH = (np.eye(2) - K.dot(H))
P1Plus = IminusKH.dot(cov1).dot(IminusKH.transpose()) + np.outer(K,K)
# P1Plus = IminusKH.dot(cov1).dot(IminusKH.transpose()) + K.dot(K.transpose())
# Verify that the updated state vector and covariance matches ours
self.assertTrue(np.allclose(x1Plus, positionObj1.stateVector))
self.assertTrue(np.allclose(x1Plus, positionObj1Chol.stateVector))
self.assertTrue(np.allclose(P1Plus, positionObj1.covariance().value))
self.assertTrue(np.allclose(
np.linalg.cholesky(P1Plus),
positionObj1Chol.covariance().value
))
# Verify that state 2 is unchanged
self.assertTrue(np.allclose(x2,positionObj2.stateVector))
self.assertTrue(np.allclose(cov2, positionObj2.covariance().value))
self.assertTrue(np.allclose(x2,positionObj2Chol.stateVector))
self.assertTrue(np.allclose(np.linalg.cholesky(cov2), positionObj2Chol.covariance().value))
# Generate velocity measurement for state2
vVar = 1
v2Meas = np.random.normal(x2[1], np.sqrt(vVar))
state2Meas = {
'velocity': {
'value': v2Meas,
'var': vVar
}
}
myFilter.measurementUpdateEKF(state2Meas, 'object2')
myFilterChol.measurementUpdateEKF(state2Meas, 'object2')
# Now compute the measurement update independently.
H = np.array([0,1])
S = H.dot(cov2).dot(H.transpose()) + np.array([[vVar]])
K = cov2.dot(H.transpose())/(S[0,0])
dY = v2Meas - H.dot(x2)
x2Plus = x2 + K.dot(dY)
IminusKH = (np.eye(2) - np.outer(K,H))
P2Plus = IminusKH.dot(cov2).dot(IminusKH.transpose()) + np.outer(K,K)*vVar
# Verify that state 2 is updated and matches our state2 computations
self.assertTrue(np.allclose(x2Plus, positionObj2.stateVector))
self.assertTrue(np.allclose(P2Plus, positionObj2.covariance().value))
self.assertTrue(np.allclose(x2Plus, positionObj2Chol.stateVector))
self.assertTrue(np.allclose(np.linalg.cholesky(P2Plus), positionObj2Chol.covariance().value))
# Verify that state 1 is unchanged
self.assertTrue(np.allclose(x1Plus, positionObj1.stateVector))
self.assertTrue(np.allclose(P1Plus, positionObj1.covariance().value))
self.assertTrue(np.allclose(x1Plus, positionObj1Chol.stateVector))
self.assertTrue(np.allclose(np.linalg.cholesky(P1Plus), positionObj1Chol.covariance().value))
# Verify that the covariances are stored properly in the filter
self.assertTrue(
np.allclose(block_diag(P1Plus, P2Plus), myFilter.covarianceMatrix.value)
)
self.assertTrue(
np.allclose(
np.linalg.cholesky(block_diag(P1Plus, P2Plus)),
myFilterChol.covarianceMatrix.value)
)
myFilter.timeUpdateEKF(1)
myFilterChol.timeUpdateEKF(1)
x1Minus = positionObj1.stateVector
P1Minus = positionObj1.covariance().value
x2Minus = positionObj2.stateVector
P2Minus = positionObj2.covariance().value
# Generate position and velocity measurement for state1
vVar = 0.01
x1Meas = np.random.normal(x1Minus[0], 1)
v1Meas = np.random.normal(x1Minus[1], np.sqrt(vVar))
state1Meas = {
'velocity': {
'value': v1Meas,
'var': vVar
},
'position': {
'value': x1Meas,
'var': 1
}
}
myFilter.measurementUpdateEKF(state1Meas, 'object1')
myFilterChol.measurementUpdateEKF(state1Meas, 'object1')
# Now compute the measurement update independently.
H = np.array([[1,0],[0,1]])
R = np.array([[1,0],[0,vVar]])
S = H.dot(P1Minus).dot(H.transpose()) + R
K = P1Minus.dot(H.transpose()).dot(np.linalg.inv(S))
dY = np.array([x1Meas, v1Meas]) - H.dot(x1Minus)
x1Plus = x1Minus + K.dot(dY)
IminusKH = (np.eye(2) - K.dot(H))
P1Plus = IminusKH.dot(P1Minus).dot(IminusKH.transpose()) + K.dot(R).dot(K.transpose())
# Verify that state 1 is updated
self.assertTrue(np.allclose(x1Plus, positionObj1.stateVector))
self.assertTrue(np.allclose(P1Plus, positionObj1.covariance().value))
self.assertTrue(np.allclose(x1Plus, positionObj1Chol.stateVector))
self.assertTrue(np.allclose(P1Plus, positionObj1Chol.covariance().convertCovariance('covariance').value
))
# Verify that state 2 is unchanged
self.assertTrue(np.allclose(x2Minus, positionObj2.stateVector))
self.assertTrue(np.allclose(P2Minus, positionObj2.covariance().value))
self.assertTrue(np.allclose(x2Minus, positionObj2Chol.stateVector))
self.assertTrue(np.allclose(P2Minus, positionObj2Chol.covariance().convertCovariance('covariance').value))
self.assertTrue(np.allclose(np.abs(np.linalg.cholesky(P2Minus)), np.abs(positionObj2Chol.covariance().value)))
def testTimeUpdateEKFCholesky(self):
myFilter = md.ModularFilter(covarianceStorage='cholesky')
position1 = np.random.normal(1)
velocity1 = np.random.normal(1)
x1 = np.array([position1, velocity1])
cov1 = np.random.normal(np.zeros([2,2]))
cov1 = cov1.dot(cov1.transpose())
cov1Sqrt = np.linalg.cholesky(cov1)
positionObj1 = self.oneDPositionVelocity(
'object1',
{'t': 0,
'stateVector': np.array([position1, velocity1]),
'covariance': cov1Sqrt,
'stateVectorID': 0
},
covarianceStorage='cholesky'
)
myFilter.addStates('object1', positionObj1)
position2 = np.random.normal(1)
velocity2 = np.random.normal(1)
cov2 = np.random.normal(np.zeros([2,2]))
cov2 = cov2.dot(cov2.transpose())
cov2Sqrt = np.linalg.cholesky(cov2)
x2 = np.array([position2, velocity2])
positionObj2 = self.oneDPositionVelocity(
'object2',
{'t': 0,
'stateVector': np.array([position2, velocity2]),
'covariance': cov2Sqrt,
'stateVectorID': 0
},
covarianceStorage='cholesky'
)
myFilter.addStates('object2', positionObj2)
self.assertEqual(positionObj1.covariance().form, 'cholesky')
self.assertEqual(positionObj2.covariance().form, 'cholesky')
self.assertEqual(myFilter.covarianceMatrix.form, 'cholesky')
# First, test the time update without the dynamics
myFilter.timeUpdateEKF(1)
FMat = np.array([[1,1],[0,1]])
self.assertTrue(
np.allclose(
positionObj1.covariance().convertCovariance('covariance').value,
FMat.dot(cov1).dot(FMat.transpose())
)
)
self.assertTrue(
np.allclose(
positionObj1.getStateVector()['stateVector'],
FMat.dot(np.array([position1, velocity1]))
)
)
self.assertTrue(
np.allclose(
positionObj2.covariance().convertCovariance('covariance').value,
FMat.dot(cov2).dot(FMat.transpose())
)
)
self.assertTrue(
np.allclose(
positionObj2.getStateVector()['stateVector'],
FMat.dot(np.array([position2, velocity2]))
)
)
# Now, do an update with dynamics
dynamics={
'object1acceleration': {'value':0,'var':1},
'object2acceleration': {'value':1,'var':1}
}
cov1 = positionObj1.covariance().convertCovariance('covariance').value
cov2 = positionObj2.covariance().convertCovariance('covariance').value
x1 = positionObj1.stateVector
x2 = positionObj2.stateVector
myFilter.timeUpdateEKF(1, dynamics=dynamics)
Q = np.array([[1/4, 1/2],[1/2, 1]])
self.assertTrue(
np.allclose(
positionObj1.covariance().convertCovariance('covariance').value,
FMat.dot(cov1).dot(FMat.transpose()) + Q
)
)
self.assertTrue(
np.allclose(
positionObj1.getStateVector()['stateVector'],
FMat.dot(x1)
)
)
self.assertTrue(
np.allclose(
positionObj2.covariance().convertCovariance('covariance').value,
FMat.dot(cov2).dot(FMat.transpose()) + Q
)
)
self.assertTrue(
np.allclose(
positionObj2.getStateVector()['stateVector'],
FMat.dot(x2) + np.array([0,1])
)
)
self.assertTrue(positionObj1.getStateVector()['aPriori'])
self.assertTrue(positionObj2.getStateVector()['aPriori'])
self.assertEqual(positionObj1.getStateVector()['t'], 2)
self.assertEqual(positionObj2.getStateVector()['t'], 2)
def run4DOFSimulation(traj):
# pulsarDir = './pulsarData/'
# pulsarCatalogFileName = 'pulsarCatalog.txt'
pulsarObjectDict = loadPulsarData(detectorArea=traj.detectorArea)
myPulsarObject = pulsarObjectDict[traj.pulsarName]
pulsarRaDec = myPulsarObject.__RaDec__
pointSources = md.utils.accessPSC.xamin_coneSearch(
pulsarRaDec['RA'] * 180.0/np.pi,
pulsarRaDec['DEC'] * 180.0/np.pi,
FOV=traj.detectorFOV,
catalog='xmmslewcln',
fluxKey='flux_b8',
# minSignificance=20
)
def attitude(t, returnQ=True):
if hasattr(t, '__len__'):
attitudeArray = []
for i in range(len(t)):
attitudeArray.append(attitude(t[i],returnQ))
return attitudeArray
else:
#eulerAngles = [t * angularVelocity[0], t* angularVelocity[1], t* angularVelocity[2]]
eulerAngles = [
0,
-pulsarRaDec['DEC'],
pulsarRaDec['RA']
]
if returnQ:
return md.utils.euler2quaternion(eulerAngles)
else:
return(eulerAngles)
def omega(t):
return(traj.angularVelocity)
def position(t):
return(
np.array([
traj.orbitAmplitude * np.cos(t/traj.orbitPeriod),
traj.orbitAmplitude * np.sin(t/traj.orbitPeriod),
0 * t
])
)
def velocity(t):
return(
(traj.orbitAmplitude/traj.orbitPeriod) *
np.array([
-np.sin(t/traj.orbitPeriod),
np.cos(t/traj.orbitPeriod),
0 * t
]
)
)
arrivalT = 1
lastT = 0
successfulRun = False
vDrift = 0
pulsarUnitVector = myPulsarObject.unitVec()
vMeas = velocity(arrivalT) + np.random.normal(0, scale=np.sqrt(traj.vVar), size=3)
vDrift += vMeas.dot(pulsarUnitVector) * (arrivalT - lastT)
arrivalT = 0
lastT = 0
while not successfulRun:
# try:
pointSourceObjectDict = {}
photonMeasurements = []
myFilter = md.ModularFilter()
for signalIndex in range(len(pointSources)):
myRow = pointSources.iloc[signalIndex]
myRa = md.utils.spacegeometry.hms2rad(hms=myRow['ra'])
myDec = md.utils.spacegeometry.dms2rad(dms=myRow['dec'])
try:
myFlux = float(myRow['flux'])
print('Initializing static point source %s.' %myRow['name'])
except:
print('Point source %s had invalid flux. Skipping.' %myRow['name'])
if myFlux > 0.0 and myFlux < 1e-10:
if (
(np.abs(pulsarRaDec['RA'] - myRa) > 1e-9) and
(np.abs(pulsarRaDec['DEC'] - myDec) > 1e-9)
):
pointSourceObjectDict[myRow['name']] = (
md.signals.StaticXRayPointSource(
myRa,
myDec,
photonEnergyFlux=myFlux,
detectorArea=traj.detectorArea,
name=myRow['name']
)
)
photonMeasurements+=pointSourceObjectDict[myRow['name']].generatePhotonArrivals(
traj.runtime,
attitude=attitude
)
try:
if myFlux > 1e-14:
myFilter.addSignalSource(myRow['name'],pointSourceObjectDict[myRow['name']])
except:
print('The signal source %s has already been added. Skipping.' %myRow['name'])
# Generate photon arrivals for each pulsar in the list
photonMeasurements += myPulsarObject.generatePhotonArrivals(
traj.runtime,
position=position,
attitude=attitude
)
if traj.scaleProcessNoise is True:
processNoise = traj.processNoise * traj.detectorArea
else:
processNoise = traj.processNoise
# print('Pulsar object period:')
# print(myPulsarObject.pulsarPeriod)
correlationSubstate = md.substates.CorrelationVector(
myPulsarObject,
traj.filterTaps,
myPulsarObject.pulsarPeriod/(traj.filterTaps+1),
signalTDOA=0,
TDOAVar=myPulsarObject.pulsarPeriod,
measurementNoiseScaleFactor=traj.measurementNoiseScaleFactor,
processNoise=processNoise,
centerPeak=True,
peakLockThreshold=traj.peakLockThreshold,
)
myFilter.addSignalSource(myPulsarObject.name, myPulsarObject)
myFilter.addStates(myPulsarObject.name, correlationSubstate)
backgroundNoise = md.signals.UniformNoiseXRaySource(
detectorArea=traj.detectorArea,
detectorFOV=traj.detectorFOV
)
photonMeasurements += backgroundNoise.generatePhotonArrivals(traj.runtime)
photonMeasurements = sorted(photonMeasurements, key=lambda k: k['t']['value'])
initialAttitude = md.utils.euler2quaternion(
attitude(0, returnQ=False) + np.random.normal(0, scale=traj.initialAttitudeSigma, size=3)
)
# print('initializing attitude')
myAttitude = md.substates.Attitude(
attitudeQuaternion=initialAttitude,
attitudeErrorCovariance=np.eye(3)*np.square(traj.initialAttitudeSigma),
gyroBiasCovariance=np.eye(3)*1e-100)
myFilter.addStates('attitude', myAttitude)
myFilter.addSignalSource('background', backgroundNoise)
# myMeas = {
# 't': {'value': 0}
# }
# myFilter.measurementUpdateEKF(myMeas, myPulsar.name)
lastUpdateTime = 0
lastT = 0
timeUpdateOnlyTDOA = []
timeUpdateOnlyT = []
constantOffset = traj.constantPhaseOffset * myPulsarObject.pulsarPeriod
for photonMeas in photonMeasurements:
arrivalT = photonMeas['t']['value']
vMeas = velocity(arrivalT) + np.random.normal(0, scale=np.sqrt(traj.vVar), size=3)
vDrift += vMeas.dot(pulsarUnitVector) * (arrivalT - lastT)
omegaMeas = omega(arrivalT) + np.random.normal(0,scale=np.sqrt(traj.omegaVar),size=3)
dynamics = {
'velocity': {'value': vMeas, 'var': np.eye(3)*traj.vVar},
'omega': {'value': omega(arrivalT), 'var': np.eye(3) * traj.omegaVar},
}
if arrivalT > lastT:
myFilter.timeUpdateEKF(arrivalT-lastT, dynamics=dynamics)
# if myCorrelation.peakLock is True:
# myCorrelation.realTimePlot()
#myFilter.timeUpdateEKF(photon-lastT)
# print(photonMeas)
photonMeas['RA']['value'] = np.random.normal(photonMeas['RA']['value'], np.sqrt(traj.AOAVar))
photonMeas['DEC']['value'] = np.random.normal(photonMeas['DEC']['value'], np.sqrt(traj.AOAVar))
photonMeas['RA']['var'] = traj.AOAVar
photonMeas['DEC']['var'] = traj.AOAVar
photonMeas['t']['var'] = 1e-20
photonMeas['t']['value'] -= constantOffset
if traj.measurementUpdateMethod == 'EKF':
myFilter.measurementUpdateEKF(photonMeas, photonMeas['name'])
elif traj.measurementUpdateMethod == 'JPDAF':
myFilter.measurementUpdateJPDAF(photonMeas)
elif traj.measurementUpdateMethod == 'MLE':
myFilter.measurementUpdateMLE(photonMeas)
else:
raise ValueError('Unrecougnized update method %s' %traj.measurementUpdateMethod)
if (arrivalT-lastUpdateTime) > 100:
lastUpdateTime = int(arrivalT)
estimatedDelay = correlationSubstate.stateVectorHistory[-1]['signalTDOA']
delayError = md.utils.spacegeometry.phaseError(
estimatedDelay,
constantOffset,
myPulsarObject.pulsarPeriod
)
print(
(
'Area: %i \tTime: %f\tTrue TDOA %f\t' +
'Est TDOA %f\tPhase Error %f\tVDrift %f'
)
%(
traj.detectorArea,
arrivalT,
constantOffset,
estimatedDelay,
delayError/myPulsarObject.pulsarPeriod,
vDrift
)
)
#myFilter.realTimePlot()
# for key in corrSubstateDict:
# corrSubstateDict[key].realTimePlot()
lastT = arrivalT
successfulRun = True
# except Exception as inst:
# print(type(inst)) # the exception instance
# print(inst.args) # arguments stored in .args
# print(inst)
# print('Aborting run because of error... restarting.')
traj.f_add_result(
'correlationSubstate.$',
correlationSubstate.getStateVector(),
comment='Correlation vector substate'
)
traj.f_add_result(
'attitudeSubstate.$',
myAttitude.getStateVector(),
comment='Attitude substate'
)
traj.f_add_result(
'constantOffset.$',
constantOffset,
comment='Offset in seconds'
)
traj.f_add_result(
'finalDelayError.$',
md.utils.spacegeometry.phaseError(
correlationSubstate.stateVectorHistory[-1]['signalTDOA'],
constantOffset,
myPulsarObject.pulsarPeriod
),
comment='Final delay error estimate'
)
traj.f_add_result(
'finalDelayVar.$',
correlationSubstate.stateVectorHistory[-1]['TDOAVar'],
comment='Variance of final TDOA estimate error'
)
traj.f_add_result(
'finalEulerError.$',
md.utils.eulerAngleDiff(
myAttitude.stateVectorHistory[-1]['eulerAngles'],
attitude(myAttitude.stateVectorHistory[-1]['t'], returnQ=False)
),
comment='Final euler angle error'
)
traj.f_add_result(
'eulerSTD.$',
myAttitude.stateVectorHistory[-1]['eulerSTD'],
comment='standard deviation of euler angle estimate error'
)
traj.f_add_result(
'peakLock.$',
correlationSubstate.peakLock,
comment='indicates whether peak lock had been reached at end of run'
)
traj.f_add_result(
'vDrift.$',
vDrift,
comment='Velocity integration drift over the course of the run, in km'
)
TDOAVar=0,
measurementNoiseScaleFactor=1,
processNoise=1e-5,
centerPeak=True,
peakLockThreshold=0.5,
velocityNoiseScaleFactor=1,
navProcessNoise=navProcessNoise,
internalNavFilter='deep',
vInitial=vInitial,
aInitial=aInitial,
gradInitial=gradInitial,
peakEstimator='EK',
)
myCorrelation.storeLastStateVectors = 10
myFilter = md.ModularFilter()
myFilter.addSignalSource(myPulsar.name, myPulsar)
myFilter.addStates(myPulsar.name, myCorrelation)
myMeas = {'t': {'value': 0}}
myFilter.measurementUpdateEKF(myMeas, myPulsar.name)
lastUpdateTime = 0
lastT = 0
timeUpdateOnlyTDOA = []
lastTUOTDOA = mySpacecraft.dynamics.position(0).dot(
myUnitVec) / myPulsar.speedOfLight()
timeUpdateOnlyT = []
vVec = [vInitial['value'] * speedOfLight]