/// <summary>
/// Update System State
/// </summary>
/// <param name="c"></param>
/// <returns></returns>
public CartesianCoordinates3D Update(CartesianCoordinates3D c)
{
try
{
var sphericalCoordinater3D = new SphericalCoordinates3D();
sphericalCoordinater3D = SystemConverter3D.ConvertToSphericalCoordinator(c);
var measurement = sphericalCoordinater3D.ToArray();
Ukf.Update(measurement);
var result = Ukf.GetState();
sphericalCoordinater3D = new SphericalCoordinates3D(result);
var sphericalCoordinate = new SphericalCoordinates3D();
return(SystemConverter3D.ConvertToCartesianCoordinator(sphericalCoordinater3D));
}
catch (Exception)
{
return(null);
}
}
/// <summary>
/// Add data to Dictionary
/// </summary>
/// <param name="receivedData"></param>
public void RegisterData(DataTransmissionModel receivedData)
{
DataProcessingModel dataProcessingModel;
var updateTimes = 1;
if (DataContainers.GetInstance().DATA_PROCESSING_CONTAINER.ContainsKey(receivedData.Flight))
{
dataProcessingModel = DataContainers.GetInstance().DATA_PROCESSING_CONTAINER[receivedData.Flight];
dataProcessingModel = DataProcessingOperations.DataTransmissionModelToDataProcessingModel(dataProcessingModel, receivedData);
DataContainers.GetInstance().DATA_RECEIVED_CONTAINER.Clear();
}
else
{
DataContainers.GetInstance().DATA_PROCESSING_CONTAINER.Add(receivedData.Flight, DataProcessingOperations.GetInstance().DataTransmissionModelToDataProcessingModelKalman(receivedData));
dataProcessingModel = DataContainers.GetInstance().DATA_PROCESSING_CONTAINER[receivedData.Flight];
updateTimes = 10;
}
if (!IsPositionNullOrZero(receivedData.Altitude) && !IsPositionNullOrZero(receivedData.Latitude) && !IsPositionNullOrZero(receivedData.Longitude))
{
var cartesianCoordinates = new CartesianCoordinates3D(
receivedData.Altitude.Value,
receivedData.Longitude.Value,
receivedData.Latitude.Value
);
for (var i = 0; i < updateTimes; i++)
{
cartesianCoordinates = dataProcessingModel.KalmanRunner.Update(cartesianCoordinates);
}
dataProcessingModel.Altitude = cartesianCoordinates.Altitude;
dataProcessingModel.Longitude = cartesianCoordinates.Longitude;
dataProcessingModel.Latitude = cartesianCoordinates.Latitude;
dataProcessingModel.UTC = DateTime.UtcNow;
}
}
