block

Tests.SquareRootCorrectionReplay

Deployment probe for correlated square-root corrections and constrained gains

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`priorState`	`Real`	`[16]`
input	`priorCovariance`	`Real`	`[15, 15]`
input	`priorRoot`	`Real`	`[15, 15]`
input	`measurementResidual`	`Real`	`[3]`
input	`observationMatrix`	`Real`	`[3, 15]`
input	`measurementCovariance`	`Real`	`[3, 3]`
input	`measurementStateCrossCovariance`	`Real`	`[15, 3]`
input	`attitudeAxis`	`Real`	`[3]`
input	`innovationGate`	`Real`
input	`headingOnly`	`Boolean`
input	`useSemiDirectBias`	`Boolean`
output	`correctedState`	`Real`	`[16]`
output	`posteriorCovariance`	`Real`	`[15, 15]`
output	`posteriorRoot`	`Real`	`[15, 15]`
output	`accepted`	`Boolean`
output	`reason`	`Integer`
output	`nis`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

block SquareRootCorrectionReplay
  "Deployment probe for correlated square-root corrections and constrained gains"
  input Real priorState[16];
  input Real priorCovariance[15, 15];
  input Real priorRoot[15, 15];
  input Real measurementResidual[3];
  input Real observationMatrix[3, 15];
  input Real measurementCovariance[3, 3];
  input Real measurementStateCrossCovariance[15, 3];
  input Real attitudeAxis[3];
  input Real innovationGate;
  input Boolean headingOnly;
  input Boolean useSemiDirectBias;
  output Real correctedState[16];
  output Real posteriorCovariance[15, 15];
  output Real posteriorRoot[15, 15];
  output Boolean accepted;
  output Integer reason;
  output Real nis;
protected
  Estimation.StrapdownINS.ESKF.State posterior;
algorithm
  when sample(0.0, 0.01) then
    (posterior, accepted, reason, nis) := Estimation.StrapdownINS.ESKF.correctLinear(
      Estimation.StrapdownINS.ESKF.State(
        barometerBiasCrossCovariance=zeros(15),
        barometerBias_m=0.0, barometerBiasVariance_m2=0.0, useJointBarometerBias=false,
        positionWorldEnu_m=priorState[1:3], velocityWorldEnu_m_s=priorState[4:6],
        quaternionWorldBody=priorState[7:10], gyroscopeBiasBodyFlu_rad_s=priorState[11:13],
        accelerometerBiasBodyFlu_m_s2=priorState[14:16], covariance=priorCovariance,
        covarianceRoot=priorRoot, useSquareRootCovariance=true),
      measurementResidual, observationMatrix, measurementCovariance,
      innovationGate, attitudeAxis, measurementStateCrossCovariance,
      headingOnly, useSemiDirectBias);
    correctedState := cat(1, posterior.positionWorldEnu_m,
      posterior.velocityWorldEnu_m_s, posterior.quaternionWorldBody,
      posterior.gyroscopeBiasBodyFlu_rad_s, posterior.accelerometerBiasBodyFlu_m_s2);
    posteriorCovariance := posterior.covariance;
    posteriorRoot := posterior.covarianceRoot;
  end when;
end SquareRootCorrectionReplay;