function

Estimation.StrapdownINS.updateBarometerBias

Update a scalar pressure-altitude bias estimate against navigation altitude

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`initializedPrevious`	`Boolean`
input	`biasPrevious_m`	`Real`
input	`variancePrevious_m2`	`Real`
input	`positionWorldEnu_m`	`Real`	`[3]`
input	`quaternionWorldBody`	`Real`	`[4]`
input	`navigationCovarianceLocal`	`Real`	`[15, 15]`
input	`measurement`	`Avionics.BarometerSample`
input	`dt`	`Real`	`(unit = "s")`
input	`initialBias_m`	`Real`	`= 0.0`
input	`initialVariance_m2`	`Real`	`= 4.0`
input	`processNoise_m2_s`	`Real`	`= 1.0e-4`
output	`initializedNext`	`Boolean`
output	`biasNext_m`	`Real`
output	`varianceNext_m2`	`Real`
output	`accepted`	`Boolean`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function updateBarometerBias
  "Update a scalar pressure-altitude bias estimate against navigation altitude"
  input Boolean initializedPrevious;
  input Real biasPrevious_m;
  input Real variancePrevious_m2;
  input Real positionWorldEnu_m[3];
  input Real quaternionWorldBody[4];
  input Real navigationCovarianceLocal[15, 15];
  input Avionics.BarometerSample measurement;
  input Real dt(unit = "s");
  input Real initialBias_m = 0.0;
  input Real initialVariance_m2 = 4.0;
  input Real processNoise_m2_s = 1.0e-4;
  output Boolean initializedNext;
  output Real biasNext_m;
  output Real varianceNext_m2;
  output Boolean accepted;
protected
  Real rotationWorldBody[3, 3];
  Real altitudeH[15];
  Real predictedVariance_m2;
  Real observationVariance_m2;
  Real innovationVariance_m2;
  Real gain;
  Real innovation_m;
  Boolean usable;
algorithm
  rotationWorldBody := LieGroups.SO3.Quat.to_DCM(quaternionWorldBody);
  altitudeH := zeros(15);
  altitudeH[1:3] := rotationWorldBody[3, :];
  predictedVariance_m2 := max(
    if initializedPrevious then variancePrevious_m2 else initialVariance_m2,
    1.0e-12) + max(processNoise_m2_s, 0.0) * max(dt, 0.0);
  observationVariance_m2 := measurement.variance_m2
    + altitudeH * navigationCovarianceLocal * altitudeH;
  innovationVariance_m2 := predictedVariance_m2 + observationVariance_m2;
  innovation_m := measurement.altitudeWorldEnu_m - positionWorldEnu_m[3]
    - (if initializedPrevious then biasPrevious_m else initialBias_m);
  usable := measurement.valid and measurement.fresh
    and abs(measurement.timestamp_s) < 1.0e100
    and abs(measurement.altitudeWorldEnu_m) < 1.0e100
    and measurement.variance_m2 > 0.0
    and observationVariance_m2 > 0.0
    and innovationVariance_m2 > 1.0e-12;
  if usable then
    gain := predictedVariance_m2 / innovationVariance_m2;
    biasNext_m := (if initializedPrevious then biasPrevious_m
      else initialBias_m) + gain * innovation_m;
    varianceNext_m2 := max((1.0 - gain) * predictedVariance_m2, 1.0e-12);
    initializedNext := true;
    accepted := true;
  else
    biasNext_m := if initializedPrevious then biasPrevious_m else initialBias_m;
    varianceNext_m2 := predictedVariance_m2;
    initializedNext := initializedPrevious;
    accepted := false;
  end if;
end updateBarometerBias;