function

Estimation.StrapdownINS.initialAlignmentQuaternion

Level from stationary specific force and align yaw from raw magnetic field

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default	Description
input	`specificForceBodyFlu_m_s2`	`Real`	`[3]`
input	`magneticFieldBodyFlu_T`	`Real`	`[3]`
input	`magneticFieldWorldEnu_T`	`Real`	`[3]`
input	`fallbackQuaternionWorldBody`	`Real`	`[4] = {1.0, 0.0, 0.0, 0.0}`
input	`useMagnetometer`	`Boolean`	`= true`	When false the heading is left at zero and the alignment is accepted on the specific force alone, for a vehicle without a usable magnetometer
output	`quaternionWorldBody`	`Real`	`[4]`
output	`accepted`	`Boolean`

Modelica declarations and implementation

Show parameters, interfaces and source

Parameters, inputs, outputs, defaults and equations from the source file.

function initialAlignmentQuaternion
  "Level from stationary specific force and align yaw from raw magnetic field"
  input Real specificForceBodyFlu_m_s2[3];
  input Real magneticFieldBodyFlu_T[3];
  input Real magneticFieldWorldEnu_T[3];
  input Real fallbackQuaternionWorldBody[4] = {1.0, 0.0, 0.0, 0.0};
  input Boolean useMagnetometer = true
    "When false the heading is left at zero and the alignment is accepted on
     the specific force alone, for a vehicle without a usable magnetometer";
  output Real quaternionWorldBody[4];
  output Boolean accepted;
protected
  Real magnitude;
  Real roll;
  Real pitch;
  Real levelingQuaternion[4];
  Real fieldLeveled[3];
  Real heading;
  Real horizontalMagnitudeSquared;
  Real referenceHorizontalMagnitudeSquared;
algorithm
  magnitude := sqrt(specificForceBodyFlu_m_s2
    * specificForceBodyFlu_m_s2);
  accepted := magnitude > 1.0e-3
    and magnitude < Estimation.StrapdownINS.ESKF.FiniteMagnitudeLimit;
  // For R_world_body = Rz(yaw) Ry(pitch) Rx(roll), a stationary
  // accelerometer observes R' * {0,0,g} =
  // {-sin(pitch), cos(pitch)sin(roll), cos(pitch)cos(roll)} * g.
  pitch := asin(MathUtilities.clip(
    -specificForceBodyFlu_m_s2[1] / max(magnitude, 1.0e-3), -1.0, 1.0));
  roll := atan2(specificForceBodyFlu_m_s2[2],
    specificForceBodyFlu_m_s2[3]);
  levelingQuaternion := LieGroups.SO3.EulerB321.to_Quat(
    {0.0, pitch, roll});
  fieldLeveled := LieGroups.SO3.Quat.to_DCM(levelingQuaternion)
    * magneticFieldBodyFlu_T;
  horizontalMagnitudeSquared := fieldLeveled[1] * fieldLeveled[1]
    + fieldLeveled[2] * fieldLeveled[2];
  referenceHorizontalMagnitudeSquared :=
    magneticFieldWorldEnu_T[1] * magneticFieldWorldEnu_T[1]
      + magneticFieldWorldEnu_T[2] * magneticFieldWorldEnu_T[2];
  if useMagnetometer then
    accepted := accepted and horizontalMagnitudeSquared > 1.0e-20
      and referenceHorizontalMagnitudeSquared > 1.0e-20;
    heading := MathUtilities.wrapAngle(
      atan2(magneticFieldWorldEnu_T[2], magneticFieldWorldEnu_T[1])
        - atan2(fieldLeveled[2], fieldLeveled[1]));
  else
    heading := 0.0;
  end if;
  if accepted then
    quaternionWorldBody := LieGroups.SO3.EulerB321.to_Quat(
      {heading, pitch, roll});
  else
    quaternionWorldBody := LieGroups.SO3.Quat.normalize(
      fallbackQuaternionWorldBody);
  end if;
end initialAlignmentQuaternion;