/// <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>
/// Predict Values
/// </summary>
/// <returns></returns>
public CartesianCoordinates3D Predict(double times = 1)
{
for (var i = 0; i < times; i++)
{
Ukf.Predict();
}
var result = Ukf.GetState();
if (result == null)
{
return(null);
}
var sphericalCoordinator = new SphericalCoordinates3D(result);
return(SystemConverter3D.ConvertToCartesianCoordinator(sphericalCoordinator));
}