// 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); }