function

Estimation.StrapdownINS.rangeToPlane

Distance from a co-located nadir camera to a plane along its -body-z ray

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	`planeNormalWorldEnu`	`Real`	`[3] = {0.0, 0.0, 1.0}`
input	`planeOffset_m`	`Real`	`= 0.0`	d in normal' * position = d
output	`distance_m`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function rangeToPlane
  "Distance from a co-located nadir camera to a plane along its -body-z ray"
  input Real positionWorldEnu_m[3];
  input Real quaternionWorldBody[4];
  input Real planeNormalWorldEnu[3] = {0.0, 0.0, 1.0};
  input Real planeOffset_m = 0.0 "d in normal' * position = d";
  output Real distance_m;
protected
  Real rotationWorldBody[3, 3];
  Real rayWorldEnu[3];
  Real denominator;
algorithm
  rotationWorldBody := LieGroups.SO3.Quat.to_DCM(quaternionWorldBody);
  rayWorldEnu := rotationWorldBody * {0.0, 0.0, -1.0};
  denominator := planeNormalWorldEnu * rayWorldEnu;
  distance_m := (planeOffset_m
    - planeNormalWorldEnu * positionWorldEnu_m) / denominator;
end rangeToPlane;