block

Tests.PreintegrationNoiseReplay

Held-input covariance used by preintegrated prediction

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`angularVelocity`	`Real`	`[3]`
input	`specificForce`	`Real`	`[3]`
input	`interval_s`	`Real`
input	`gyroscopeDensity`	`Real`
input	`accelerometerDensity`	`Real`
output	`covariance`	`Real`	`[15, 15](each start=0, each fixed=true)`

Modelica declarations and implementation

Show parameters, interfaces and source

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

block PreintegrationNoiseReplay
  "Held-input covariance used by preintegrated prediction"
  input Real angularVelocity[3];
  input Real specificForce[3];
  input Real interval_s;
  input Real gyroscopeDensity;
  input Real accelerometerDensity;
  output Real covariance[15, 15](each start=0, each fixed=true);
algorithm
  when sample(0, 0.01) then
    covariance := Estimation.StrapdownINS.ESKF.discreteProcessCovariance(
      Estimation.StrapdownINS.ESKF.continuousTransition(
        angularVelocity, specificForce),
      Estimation.StrapdownINS.ESKF.noiseInputMatrix(),
      Estimation.StrapdownINS.ESKF.processNoiseMatrix(
        Estimation.StrapdownINS.ProcessNoise(
          gyroscope_rad2_s=gyroscopeDensity * identity(3),
          accelerometer_m2_s3=accelerometerDensity * identity(3),
          gyroscopeBias_rad2_s3=zeros(3, 3),
          accelerometerBias_m2_s5=zeros(3, 3))),
      interval_s);
  end when;
end PreintegrationNoiseReplay;