function

SLAM.Fusion.correctMappedLandmarksTight

Fuse raw 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 correctMappedLandmarksTight "Fuse raw 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];
  Boolean geometryValid;
  Integer rejectionReason;
algorithm
  (residual, navigationJacobian, geometryValid) := linearizeMappedLandmarks(
    predicted, landmarksWorld_m, observations, rotationBodyCamera, cameraPositionBody_m, intrinsics);
  corrected := Estimation.StrapdownINS.ESKF.copyState(predicted);
  accepted := false;
  normalizedInnovationSquared := 0.0;
  if geometryValid then
    (corrected, accepted, rejectionReason, normalizedInnovationSquared) :=
      Estimation.StrapdownINS.ESKF.correctLinear(predicted, residual,
        navigationJacobian, measurementCovariance);
  end if;
end correctMappedLandmarksTight;