function

Estimation.StrapdownINS.ESKF.continuousTransition

Continuous local right-perturbation error dynamics for held IMU input

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`angularVelocityBodyFlu_rad_s`	`Real`	`[3]`
input	`specificForceBodyFlu_m_s2`	`Real`	`[3]`
output	`A`	`Real`	`[TangentLength, TangentLength]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function continuousTransition
  "Continuous local right-perturbation error dynamics for held IMU input"
  input Real angularVelocityBodyFlu_rad_s[3];
  input Real specificForceBodyFlu_m_s2[3];
  output Real A[TangentLength, TangentLength];
protected
  Real omegaCross[3, 3];
  Real forceCross[3, 3];
algorithm
  omegaCross := LieGroups.SO3.Quat.wedge(angularVelocityBodyFlu_rad_s);
  forceCross := LieGroups.SO3.Quat.wedge(specificForceBodyFlu_m_s2);
  A := cat(1,
    cat(2, -omegaCross, identity(3), zeros(3, 9)),
    cat(2, zeros(3, 3), -omegaCross, -forceCross,
      zeros(3, 3), -identity(3)),
    cat(2, zeros(3, 6), -omegaCross, -identity(3), zeros(3, 3)),
    zeros(6, TangentLength));
end continuousTransition;