function

SLAM.Fusion.correctMappedLandmarksLoose

Fuse a compressed pose from independent fixed-map RGB-D observations

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`predicted`	`Estimation.StrapdownINS.ESKF.State`
input	`landmarksWorld_m`	`Real`	`[:, 3]`
input	`observations`	`Real`	`[size(landmarksWorld_m, 1), 3]`
input	`measurementCovariance`	`Real`	`[3 * size(landmarksWorld_m, 1), 3 * size(landmarksWorld_m, 1)]`
input	`rotationBodyCamera`	`Real`	`[3, 3]`
input	`cameraPositionBody_m`	`Real`	`[3]`
input	`intrinsics`	`Real`	`[4]`
output	`corrected`	`Estimation.StrapdownINS.ESKF.State`
output	`accepted`	`Boolean`
output	`normalizedInnovationSquared`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function correctMappedLandmarksLoose "Fuse a compressed pose from independent fixed-map RGB-D observations"
  input Estimation.StrapdownINS.ESKF.State predicted;
  input Real landmarksWorld_m[:, 3];
  input Real observations[size(landmarksWorld_m, 1), 3];
  input Real measurementCovariance[3 * size(landmarksWorld_m, 1), 3 * size(landmarksWorld_m, 1)];
  input Real rotationBodyCamera[3, 3];
  input Real cameraPositionBody_m[3];
  input Real intrinsics[4];
  output Estimation.StrapdownINS.ESKF.State corrected;
  output Boolean accepted;
  output Real normalizedInnovationSquared;
protected
  Real residual[3 * size(landmarksWorld_m, 1)];
  Real navigationJacobian[3 * size(landmarksWorld_m, 1), 15];
  Real poseResidual[6];
  Real poseCovariance[6, 6];
  Real poseSelector[6, 15];
  Boolean geometryValid;
  Boolean compressionValid;
  Integer rejectionReason;
algorithm
  (residual, navigationJacobian, geometryValid) := linearizeMappedLandmarks(
    predicted, landmarksWorld_m, observations, rotationBodyCamera, cameraPositionBody_m, intrinsics);
  (poseResidual, poseCovariance, compressionValid) := compressPose(
    residual, navigationJacobian, measurementCovariance);
  poseSelector := zeros(6, 15);
  poseSelector[1:3, 1:3] := 1.0 * identity(3);
  poseSelector[4:6, 7:9] := 1.0 * identity(3);
  corrected := Estimation.StrapdownINS.ESKF.copyState(predicted);
  accepted := false;
  normalizedInnovationSquared := 0.0;
  if geometryValid and compressionValid then
    (corrected, accepted, rejectionReason, normalizedInnovationSquared) :=
      Estimation.StrapdownINS.ESKF.correctLinear(predicted, poseResidual,
        poseSelector, poseCovariance);
  end if;
end correctMappedLandmarksLoose;