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