function

Estimation.StrapdownINS.ESKF.initialize

Initialize the geometric error-state filter

Initial state and uncertainty

Supply world ENU position and velocity, a body-FLU-to-world quaternion and body-frame IMU biases. The quaternion is normalized. The initial covariance uses the {position, velocity, attitude, gyro bias, accelerometer bias} tangent ordering; variance inputs are squared physical units, not standard deviations.

If an aiding position seeds the state, supply its world-frame covariance. The initializer rotates it to the local tangent and retains conservative configured variance floors. Zero optional seed covariance keeps the configured prior. Initialization does not itself execute prediction or sensor fusion.

Use Estimator for sampled lifecycle handling or carry the returned State explicitly between prediction and correction calls.

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`positionWorldEnu_m`	`Real`	`[3]`
input	`quaternionWorldBody`	`Real`	`[4]`
input	`variances`	`Estimation.StrapdownINS.InitialVariances`
input	`initialVelocityWorldEnu_m_s`	`Real`	`[3] = zeros(3)`
input	`initialGyroscopeBiasBodyFlu_rad_s`	`Real`	`[3] = zeros(3)`
input	`initialAccelerometerBiasBodyFlu_m_s2`	`Real`	`[3] = zeros(3)`
input	`initialPositionCovarianceWorld_m2`	`Real`	`[3, 3] = zeros(3, 3)`	Covariance of an aiding position used as the seed; zero keeps configured priors
input	`useSquareRootCovariance`	`Boolean`	`= false`
input	`barometerBias_m`	`Real`	`= 0.0`
input	`barometerBiasVariance_m2`	`Real`	`= 0.0`
input	`useJointBarometerBias`	`Boolean`	`= false`
output	`state`	`State`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function initialize "Initialize the geometric error-state filter"
  input Real positionWorldEnu_m[3];
  input Real quaternionWorldBody[4];
  input Estimation.StrapdownINS.InitialVariances variances;
  input Real initialVelocityWorldEnu_m_s[3] = zeros(3);
  input Real initialGyroscopeBiasBodyFlu_rad_s[3] = zeros(3);
  input Real initialAccelerometerBiasBodyFlu_m_s2[3] = zeros(3);
  input Real initialPositionCovarianceWorld_m2[3, 3] = zeros(3, 3)
    "Covariance of an aiding position used as the seed; zero keeps configured priors";
  input Boolean useSquareRootCovariance = false;
  input Real barometerBias_m = 0.0;
  input Real barometerBiasVariance_m2 = 0.0;
  input Boolean useJointBarometerBias = false;
  output State state;
protected
  Covariance initialCovariance;
algorithm
  initialCovariance := diagonal(cat(1,
    seedPositionVariances(initialPositionCovarianceWorld_m2,
      quaternionWorldBody, variances.position_m2),
    variances.velocity_m2_s2,
    variances.attitude_rad2,
    variances.gyroscopeBias_rad2_s2,
    variances.accelerometerBias_m2_s4));
  state := State(
    positionWorldEnu_m=positionWorldEnu_m,
    velocityWorldEnu_m_s=initialVelocityWorldEnu_m_s,
    quaternionWorldBody=LieGroups.SO3.Quat.normalize(quaternionWorldBody),
    gyroscopeBiasBodyFlu_rad_s=initialGyroscopeBiasBodyFlu_rad_s,
    accelerometerBiasBodyFlu_m_s2=initialAccelerometerBiasBodyFlu_m_s2,
    covariance=initialCovariance,
    useSquareRootCovariance=useSquareRootCovariance,
    barometerBiasCrossCovariance=zeros(TangentLength),
    barometerBias_m=barometerBias_m,
    barometerBiasVariance_m2=barometerBiasVariance_m2,
    useJointBarometerBias=useJointBarometerBias,
    covarianceRoot=if useSquareRootCovariance then diagonal(
      {sqrt(initialCovariance[axis, axis]) for axis in 1:TangentLength})
      else zeros(TangentLength, TangentLength));
  annotation(Documentation(info = "<html>
    <h4>Initial state and uncertainty</h4>
<p>Supply world ENU position and velocity, a body-FLU-to-world quaternion and
body-frame IMU biases. The quaternion is normalized. The initial covariance
uses the {position, velocity, attitude, gyro bias, accelerometer bias} tangent
ordering; variance inputs are squared physical units, not standard deviations.</p>
<p>If an aiding position seeds the state, supply its world-frame covariance.
The initializer rotates it to the local tangent and retains conservative
configured variance floors. Zero optional seed covariance keeps the configured
prior. Initialization does not itself execute prediction or sensor fusion.</p>
<p>Use <a href=\"modelica://Estimation.StrapdownINS.ESKF.Estimator\">Estimator</a>
for sampled lifecycle handling or carry the returned
<a href=\"modelica://Estimation.StrapdownINS.ESKF.State\">State</a> explicitly
between prediction and correction calls.</p>
    </html>"));
end initialize;