// Performs a Kalman update on track using measurement and overwrites track with new estimate
public void UpdateTrack(GaussianTrack track, GaussianMeasurement measurement)
{
// Predict track forward to measurement time and add process noise
Matrix Q = processNoiseModel.Evaluate(track.gaussianVector.mean, track.dateTime, measurement.dateTime);
GaussianVector predictedGaussianVector = track.CoastTrack(stateTransitionModel, measurement.dateTime);
predictedGaussianVector.covariance += Q;
// Compute residual mean / covariance
Vector hx;
Matrix H;
Coordinate.Convert(new Vector(6), Coordinate.Type.UNITY6, predictedGaussianVector.mean, measurement.creatorUnityReference, measurement.coordinateType, out hx, out H);
// Compute innovation and Kalman gain
Vector y = measurement.gaussianVector.mean - hx;
Matrix HT = H.Clone();
HT.Transpose();
Matrix S = H * predictedGaussianVector.covariance * HT + measurement.gaussianVector.covariance;
Matrix K = S.SolveTranspose(predictedGaussianVector.covariance * HT);
K.Transpose();
//Matrix K = (predictedGaussianVector.covariance * HT)*S.Inverse();
// Update state estimate
int N = predictedGaussianVector.mean.Length;
predictedGaussianVector.covariance = (Matrix.Identity(N,N) - K*H) * predictedGaussianVector.covariance; // Problem
predictedGaussianVector.mean = predictedGaussianVector.mean + (K * y.ToColumnMatrix()).GetColumnVector(0);
// Write to the track
track.gaussianVector = predictedGaussianVector;
track.dateTime = measurement.dateTime;
}
// Add to list
public void AssociateMeasurement(GaussianMeasurement m)
{
lock (associatedMeasurements)
{
associatedMeasurements.Add(m);
}
}
public void AddUnassociatedMeasurement(GaussianMeasurement m)
{
lock (unassociatedMeasurements)
{
unassociatedMeasurements.Add(m);
}
}
// Creating an uninitialized track with an associated measurement
public GaussianProtoTrack(GaussianMeasurement m)
{
AssociateMeasurement(m);
}
// Update is called once per frame
void Update()
{
// Check if it is time to perform an update
timeSinceLastUpdateSec += Time.deltaTime;
if (timeSinceLastUpdateSec >= updatePeriodSec)
{
// Save where I am
Vector ownshipPosUnity = new Vector(6);
ownshipPosUnity[0] = gameObject.transform.position.x;
ownshipPosUnity[1] = gameObject.transform.position.y;
ownshipPosUnity[2] = gameObject.transform.position.z;
ownshipPosUnity[3] = 0;
ownshipPosUnity[4] = 0;
ownshipPosUnity[5] = 0;
// New scan
List<GaussianMeasurement> ms = new List<GaussianMeasurement>();
// Take a noisy measurement of each target
for (int i = 0; i < targets.Length; i++)
{
// Get target position
Vector targetPosUnity = GetUnityPosition(targets[i]);
// Generate gaussian noise
Vector gaussianNoise = new Vector(3);
gaussianNoise[0] = nd.NextDouble();
gaussianNoise[1] = nd.NextDouble();
gaussianNoise[2] = nd.NextDouble();
gaussianNoise = (noiseCovCholT * gaussianNoise.ToColumnMatrix()).GetColumnVector(0);
// Generate measurement
GaussianMeasurement m = new GaussianMeasurement(1, 1, ownshipPosUnity,
new GaussianVector(targetPosUnity + gaussianNoise, noiseCovariance),
Coordinate.Type.UNITY3,
System.DateTime.Now);
ms.Add(m);
}
// Add to fusion engine to be processed
fusionEngine.AddScanMeasurements(ms);
// Clear time since last update
timeSinceLastUpdateSec = 0.0f;
}
}
// Compare against result of chi2inv(p,3) where p represents desired probability for test
private double Compute3DimChiSquareDistance(GaussianMeasurement measurement, GaussianTrack track)
{
// Coast track to measurement time using state transition model
GaussianVector coastedTrack = track.CoastTrack(fusionEngine.stateTransitionModel, measurement.dateTime);
// Only take 3 dimensional components of coasted track
Vector coastedMean = VectorUtilities.Resize(coastedTrack.mean,3);
Matrix coastedCovariance = VectorUtilities.Resize(coastedTrack.covariance,3,3);
// Convert measurement to UNITY (track) 3 dimensional coordinates
Vector convMeasurementMean;
Matrix convMeasurementJacobian;
Coordinate.Convert(measurement.creatorUnityReference, measurement.coordinateType, measurement.gaussianVector.mean,
new Vector(3), Coordinate.Type.UNITY3, out convMeasurementMean, out convMeasurementJacobian);
Matrix convMeasurementJacobianT = convMeasurementJacobian.Clone();
convMeasurementJacobianT.Transpose();
Matrix convMeasurementCovariance = convMeasurementJacobian * measurement.gaussianVector.covariance * convMeasurementJacobianT;
// Compute 3 dimensional chi-square metric
Matrix meanDisplacement = (coastedMean - convMeasurementMean).ToColumnMatrix();
Matrix meanDisplacementT = meanDisplacement.Clone();
meanDisplacementT.Transpose();
return (meanDisplacementT*((coastedCovariance + convMeasurementCovariance).Solve(meanDisplacement)))[0,0];
}
private void AddToTieredDictionary(Dictionary<uint, Dictionary<uint, List<GaussianMeasurement>>> d, GaussianMeasurement m)
{
// Create a new dictionary for this platform ID if one doesn't already exist
if (!d.ContainsKey(m.platformID))
{
d[m.platformID] = new Dictionary<uint, List<GaussianMeasurement>>();
}
// Create a new dictionary for this sensor ID if one doesn't already exist
if (!d[m.platformID].ContainsKey(m.sensorID))
{
d[m.platformID][m.sensorID] = new List<GaussianMeasurement>();
}
// Add to the appropriate list
d[m.platformID][m.sensorID].Add(m);
}
