private static Quaternion GetOrientation(Matrix state)
{
Vector stateVector = state.GetColumnVector(0);
return(new Quaternion(
(float)stateVector[QuaternionX],
(float)stateVector[QuaternionY],
(float)stateVector[QuaternionZ],
(float)stateVector[QuaternionW]));
}
private static GPSINSOutput ObservationMatrixToFilterResult(Matrix z)
{
Vector observationVector = z.GetColumnVector(0);
return(new GPSINSOutput
{
Position =
new Vector3((float)observationVector[ObsPositionX],
(float)observationVector[ObsPositionY],
(float)observationVector[ObsPositionZ]),
});
}
//
private static GPSFilter2DSample EstimateMatrixToFilterResult(Matrix x)
{
Vector stateVector = x.GetColumnVector(0);
return(new GPSFilter2DSample
{
Position =
new Vector3((float)stateVector[PositionX], (float)stateVector[PositionY],
(float)stateVector[PositionZ]),
// Velocity = new Vector3((float)stateVector[VelocityX], (float)stateVector[VelocityY], (float)stateVector[VelocityZ]),
// Acceleration = new Vector3((float)stateVector[AccelerationX], (float)stateVector[AccelerationY], (float)stateVector[AccelerationZ]),
// Time = TimeSpan.FromSeconds(stateVector[Time]),
});
}
private static GPSINSOutput EstimateMatrixToFilterResult(Matrix x)
{
Vector stateVector = x.GetColumnVector(0);
return(new GPSINSOutput
{
Position = new Vector3(
(float)stateVector[PositionX],
(float)stateVector[PositionY],
(float)stateVector[PositionZ]),
Velocity = new Vector3(
(float)stateVector[VelocityX],
(float)stateVector[VelocityY],
(float)stateVector[VelocityZ]),
Orientation = GetOrientation(x),
});
}
private static Quaternion GetOrientation(Matrix state)
{
Vector stateVector = state.GetColumnVector(0);
return new Quaternion(
(float) stateVector[QuaternionX],
(float) stateVector[QuaternionY],
(float) stateVector[QuaternionZ],
(float) stateVector[QuaternionW]);
}
private static GPSINSOutput ObservationMatrixToFilterResult(Matrix z)
{
Vector observationVector = z.GetColumnVector(0);
return new GPSINSOutput
{
Position =
new Vector3((float) observationVector[ObsPositionX],
(float) observationVector[ObsPositionY],
(float) observationVector[ObsPositionZ]),
};
}
private static GPSINSOutput EstimateMatrixToFilterResult(Matrix x)
{
Vector stateVector = x.GetColumnVector(0);
return new GPSINSOutput
{
Position = new Vector3(
(float) stateVector[PositionX],
(float) stateVector[PositionY],
(float) stateVector[PositionZ]),
Velocity = new Vector3(
(float) stateVector[VelocityX],
(float) stateVector[VelocityY],
(float) stateVector[VelocityZ]),
Orientation = GetOrientation(x),
};
}
//
private static GPSFilter2DSample EstimateMatrixToFilterResult(Matrix x)
{
Vector stateVector = x.GetColumnVector(0);
return new GPSFilter2DSample
{
Position =
new Vector3((float) stateVector[PositionX], (float) stateVector[PositionY],
(float) stateVector[PositionZ]),
// Velocity = new Vector3((float)stateVector[VelocityX], (float)stateVector[VelocityY], (float)stateVector[VelocityZ]),
// Acceleration = new Vector3((float)stateVector[AccelerationX], (float)stateVector[AccelerationY], (float)stateVector[AccelerationZ]),
// Time = TimeSpan.FromSeconds(stateVector[Time]),
};
}
