// Returns the current track's distribution coasted to target dateTime w/o changing contents of current track
public GaussianVector CoastTrack(StateTransitionModel stateTransitionModel, DateTime targetDateTime)
{
// Make a copy to hold result in
GaussianVector coastedData = gaussianVector.Clone();
// Save old mean
Vector oldMean = gaussianVector.mean.Clone();
// Predict state mean forward
coastedData.mean = stateTransitionModel.Evaluate(oldMean, dateTime, targetDateTime);
// Predict uncertainty using Jacobian
Matrix F = stateTransitionModel.GetJacobian(oldMean, dateTime, targetDateTime);
Matrix FT = F.Clone();
FT.Transpose();
coastedData.covariance = F * gaussianVector.covariance * FT;
// Return to caller, leaving current track unchanged
return coastedData;
}
public ExtendedKalmanFilter(StateTransitionModel stateTransitionModel, ProcessNoiseModel processNoiseModel)
{
this.stateTransitionModel = stateTransitionModel;
this.processNoiseModel = processNoiseModel;
}
// Use this for initialization
void Start()
{
// Initialize databases
unprocessedMeasurements = new Dictionary<uint, Dictionary<uint, List<GaussianMeasurement>>>(); // platformID, sensorID, meas
//unassociatedMeasurements = new Dictionary<uint, Dictionary<uint, List<GaussianMeasurement>>>(); // platformID, sensorID, meas
unassociatedMeasurements = new List<GaussianMeasurement>();
trackDatabase = new List<GaussianTrack>();
protoTrackDatabase = new List<GaussianTrack>();
// Initialize track ID bookkeeping
usedTrackIDs = new HashSet<ulong>();
availableTrackID = 0;
// Initialize models
stateTransitionModel = new ConstantVelocityModel();
processNoiseModel = new RandomAccelerationModel(2);
// Initialize components
associator = new ChiSquareAssociator(this, 16.2662f);
initializer = new SingleTrackInitializer(this, 5);
ekf = new ExtendedKalmanFilter(stateTransitionModel, processNoiseModel);
}