/// <summary>
/// Normalise the state vector: since quaternion is redundant we sometimes
/// need to enforce that it stays with magnitude 1.
/// </summary>
/// <param name="xv"></param>
public override void func_xvnorm_and_dxvnorm_by_dxv(Vector xv)
{
// Most parts of the state vector don't change so copy as starting point
xvnormRES.Update(xv);
// Most parts of Jacobian are identity
dxvnorm_by_dxvRES.SetIdentity();
// Extract quaternion
func_xp(xv);
func_q(xpRES);
Quaternion Tempqa = qRES;
Quaternion Tempqb = (Tempqa);
MatrixFixed Temp44a = MatrixFixed.dqnorm_by_dq(Tempqa);
xvnormRES.Update(Tempqb.GetRXYZ(), 3);
dxvnorm_by_dxvRES.Update(Temp44a, 3, 3);
}
public override void func_xvnorm_and_dxvnorm_by_dxv(Vector xv)
{
// Normalise the state vector: since quaternion is redundant we sometimes
// need to enforce that it stays with size 1
// Most parts of the state vector don't change so copy as starting point
xvnormRES.Update(xv);
// Most parts of Jacobian are identity
dxvnorm_by_dxvRES.SetIdentity();
// Extract quaternion
func_xp(xv);
func_q(xpRES);
Quaternion Tempqa = qRES;
Quaternion Tempqb = (Tempqa);
MatrixFixed Temp44a = MatrixFixed.dqnorm_by_dq(Tempqa);
xvnormRES.Update(Tempqb.GetRXYZ(), 3);
dxvnorm_by_dxvRES.Update(Temp44a, 3, 3);
}