function

Estimation.StrapdownINS.ESKF.correctGpsVelocity

Correct world velocity from GPS Doppler velocity

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`predicted`	`State`
input	`measurement`	`Avionics.GpsSample`
input	`innovationGate`	`Real`	`= 0.0`	Per-degree-of-freedom NIS gate; non-positive disables
input	`measurementAge_s`	`Real`	`(unit = "s") = 0.0`
input	`angularVelocityMeasuredBodyFlu_rad_s`	`Real`	`[3] = zeros(3)`
input	`specificForceMeasuredBodyFlu_m_s2`	`Real`	`[3] = zeros(3)`
input	`gravityWorldEnu_m_s2`	`Real`	`[3] = {0.0, 0.0, -9.81}`
input	`maximumAidingDelay_s`	`Real`	`(unit = "s") = 0.25`
input	`useSemiDirectBias`	`Boolean`	`= false`
output	`corrected`	`State`
output	`accepted`	`Boolean`
output	`rejectionReason`	`Integer`		Estimation.StrapdownINS.Correction* outcome code
output	`normalizedInnovationSquared`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function correctGpsVelocity "Correct world velocity from GPS Doppler velocity"
  input State predicted;
  input Avionics.GpsSample measurement;
  input Real innovationGate = 0.0
    "Per-degree-of-freedom NIS gate; non-positive disables";
  input Real measurementAge_s(unit = "s") = 0.0;
  input Real angularVelocityMeasuredBodyFlu_rad_s[3] = zeros(3);
  input Real specificForceMeasuredBodyFlu_m_s2[3] = zeros(3);
  input Real gravityWorldEnu_m_s2[3] = {0.0, 0.0, -9.81};
  input Real maximumAidingDelay_s(unit = "s") = 0.25;
  input Boolean useSemiDirectBias = false;
  output State corrected;
  output Boolean accepted;
  output Integer rejectionReason
    "Estimation.StrapdownINS.Correction* outcome code";
  output Real normalizedInnovationSquared;
protected
  Real rotationWorldBody[3, 3];
  Real delayedVelocity[3];
  Real delayedQuaternion[4];
  Real delayedStateVector[16];
  Real currentToDelayed[TangentLength, TangentLength];
  Real residual[3];
  Real H[3, TangentLength];
  Real measurementCovariance[3, 3];
algorithm
  delayedStateVector := retrodict(predicted,
    angularVelocityMeasuredBodyFlu_rad_s,
    specificForceMeasuredBodyFlu_m_s2, gravityWorldEnu_m_s2,
    max(measurementAge_s, 0.0));
  delayedVelocity := delayedStateVector[4:6];
  delayedQuaternion := delayedStateVector[7:10];
  currentToDelayed := discreteTransition(continuousTransition(
    angularVelocityMeasuredBodyFlu_rad_s
      - predicted.gyroscopeBiasBodyFlu_rad_s,
    specificForceMeasuredBodyFlu_m_s2
      - predicted.accelerometerBiasBodyFlu_m_s2),
    -max(measurementAge_s, 0.0));
  rotationWorldBody := LieGroups.SO3.Quat.to_DCM(
    delayedQuaternion);
  residual := transpose(rotationWorldBody)
    * (measurement.velocityWorldEnu_m_s - delayedVelocity);
  H := cat(2, zeros(3, 3), identity(3),
    zeros(3, TangentLength - 6)) * currentToDelayed;
  measurementCovariance := transpose(rotationWorldBody)
    * measurement.velocityCovarianceWorld_m2_s2 * rotationWorldBody;
  corrected := copyState(predicted);
  accepted := false;
  normalizedInnovationSquared := 0.0;
  if measurementAge_s < -1.0e-6
      or measurementAge_s > maximumAidingDelay_s then
    rejectionReason := CorrectionRejectedTimestamp;
  else
    (corrected, accepted, rejectionReason, normalizedInnovationSquared) :=
      correctLinear(predicted, residual, H, measurementCovariance,
        innovationGate, zeros(3), zeros(TangentLength, size(residual, 1)),
        false, useSemiDirectBias);
  end if;
end correctGpsVelocity;