function

Estimation.StrapdownINS.ESKF.conditionAlignment

Condition the initial tangent covariance on the gravity vector used for alignment

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`prior`	`State`
input	`specificForceBodyFlu_m_s2`	`Real`	`[3]`
input	`specificForceCovariance`	`Real`	`[3, 3]`
input	`gravityWorldEnu_m_s2`	`Real`	`[3]`
input	`innovationGate`	`Real`
input	`useSemiDirectBias`	`Boolean`	`= false`
output	`conditioned`	`State`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function conditionAlignment
  "Condition the initial tangent covariance on the gravity vector used for alignment"
  input State prior;
  input Real specificForceBodyFlu_m_s2[3];
  input Real specificForceCovariance[3, 3];
  input Real gravityWorldEnu_m_s2[3];
  input Real innovationGate;
  input Boolean useSemiDirectBias = false;
  output State conditioned;
protected
  Real gravityBodyFlu[3];
  Real residual[3];
  Real H[3, TangentLength];
  Boolean accepted;
  Integer outcome;
  Real nis;
algorithm
  gravityBodyFlu := -transpose(LieGroups.SO3.Quat.to_DCM(
    prior.quaternionWorldBody)) * gravityWorldEnu_m_s2;
  residual := specificForceBodyFlu_m_s2 - gravityBodyFlu
    - prior.accelerometerBiasBodyFlu_m_s2;
  H := cat(2, zeros(3, 6), LieGroups.SO3.Quat.wedge(gravityBodyFlu),
    zeros(3, 3), identity(3));
  (conditioned, accepted, outcome, nis) := correctLinear(
    prior, residual, H, specificForceCovariance, innovationGate, zeros(3), zeros(TangentLength, size(residual, 1)),
        false, useSemiDirectBias);
end conditionAlignment;