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