block

Tests.VisualLandmarkReplay

RGB-D observation and Lie injection qualification

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`quaternionWorldBody`	`Real`	`[4] = {1.0, 0.0, 0.0, 0.0}`
input	`positionWorld_m`	`Real`	`[3] = zeros(3)`
input	`landmarkWorld_m`	`Real`	`[3] = {0.0, 0.0, 5.0}`
input	`rotationBodyCamera`	`Real`	`[3, 3] = 1.0 * identity(3)`
input	`cameraPositionBody_m`	`Real`	`[3] = zeros(3)`
input	`intrinsics`	`Real`	`[4] = {400.0, 400.0, 320.0, 240.0}`
input	`correction`	`Real`	`[15] = zeros(15)`
output	`expected`	`Real`	`[3]`
output	`navigationJacobian`	`Real`	`[3, 15]`
output	`landmarkJacobian`	`Real`	`[3, 3]`
output	`valid`	`Boolean`

Modelica declarations and implementation

Show parameters, interfaces and source

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

block VisualLandmarkReplay "RGB-D observation and Lie injection qualification"
  input Real quaternionWorldBody[4] = {1.0, 0.0, 0.0, 0.0};
  input Real positionWorld_m[3] = zeros(3);
  input Real landmarkWorld_m[3] = {0.0, 0.0, 5.0};
  input Real rotationBodyCamera[3, 3] = 1.0 * identity(3);
  input Real cameraPositionBody_m[3] = zeros(3);
  input Real intrinsics[4] = {400.0, 400.0, 320.0, 240.0};
  input Real correction[15] = zeros(15);
  output Real expected[3];
  output Real navigationJacobian[3, 15];
  output Real landmarkJacobian[3, 3];
  output Boolean valid;
protected
  Estimation.StrapdownINS.ESKF.NominalState nominal(
    positionWorldEnu_m=positionWorld_m, velocityWorldEnu_m_s=zeros(3),
    quaternionWorldBody=quaternionWorldBody,
    gyroscopeBiasBodyFlu_rad_s=zeros(3), accelerometerBiasBodyFlu_m_s2=zeros(3));
  Estimation.StrapdownINS.ESKF.NominalState corrected;
algorithm
  when sample(0.0, 0.01) then
    corrected := Estimation.StrapdownINS.ESKF.inject(nominal, correction);
    (expected, navigationJacobian, landmarkJacobian, valid) :=
      Estimation.StrapdownINS.ESKF.Visual.linearizeLandmark(corrected,
        landmarkWorld_m, rotationBodyCamera, cameraPositionBody_m, intrinsics);
  end when;
end VisualLandmarkReplay;