function

Estimation.StrapdownINS.ESKF.navigationEstimate

Publish a canonical estimate with consistent attitude representations

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`state`	`State`
input	`imu`	`Avionics.ImuSample`
input	`gravityWorldEnu_m_s2`	`Real`	`[3]`
input	`validInput`	`Boolean`
output	`valid`	`Boolean`
output	`timestamp_s`	`Real`
output	`positionWorldEnu_m`	`Real`	`[3]`
output	`velocityWorldEnu_m_s`	`Real`	`[3]`
output	`accelerationWorldEnu_m_s2`	`Real`	`[3]`
output	`quaternionWorldBody`	`Real`	`[4]`
output	`rotationWorldBody`	`Real`	`[3, 3]`
output	`eulerRpy_rad`	`Real`	`[3]`
output	`angularVelocityBodyFlu_rad_s`	`Real`	`[3]`
output	`angularVelocityWorldEnu_rad_s`	`Real`	`[3]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function navigationEstimate
  "Publish a canonical estimate with consistent attitude representations"
  input State state;
  input Avionics.ImuSample imu;
  input Real gravityWorldEnu_m_s2[3];
  input Boolean validInput;
  output Boolean valid;
  output Real timestamp_s;
  output Real positionWorldEnu_m[3];
  output Real velocityWorldEnu_m_s[3];
  output Real accelerationWorldEnu_m_s2[3];
  output Real quaternionWorldBody[4];
  output Real rotationWorldBody[3, 3];
  output Real eulerRpy_rad[3];
  output Real angularVelocityBodyFlu_rad_s[3];
  output Real angularVelocityWorldEnu_rad_s[3];
protected
  Real eulerB321_rad[3];
  Real correctedAngularVelocityBody[3];
  Real correctedSpecificForceBody[3];
algorithm
  rotationWorldBody := LieGroups.SO3.Quat.to_DCM(
    state.quaternionWorldBody);
  eulerB321_rad := LieGroups.SO3.EulerB321.from_Quat(
    state.quaternionWorldBody);
  correctedAngularVelocityBody := imu.angularVelocityBodyFlu_rad_s
    - state.gyroscopeBiasBodyFlu_rad_s;
  correctedSpecificForceBody := imu.specificForceBodyFlu_m_s2
    - state.accelerometerBiasBodyFlu_m_s2;
  valid := validInput;
  timestamp_s := imu.timestamp_s;
  positionWorldEnu_m := state.positionWorldEnu_m;
  velocityWorldEnu_m_s := state.velocityWorldEnu_m_s;
  accelerationWorldEnu_m_s2 := rotationWorldBody
    * correctedSpecificForceBody + gravityWorldEnu_m_s2;
  quaternionWorldBody := state.quaternionWorldBody;
  eulerRpy_rad := {
    eulerB321_rad[3], eulerB321_rad[2], eulerB321_rad[1]};
  angularVelocityBodyFlu_rad_s := correctedAngularVelocityBody;
  angularVelocityWorldEnu_rad_s :=
    rotationWorldBody * correctedAngularVelocityBody;
end navigationEstimate;