function

Estimation.StrapdownINS.ESKF.opticalFlowRangeJacobian

Nadir ray-to-plane range and Jacobian in the ESKF right-error tangent

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`predicted`	`State`
input	`planeNormalWorldEnu`	`Real`	`[3] = {0.0, 0.0, 1.0}`
input	`planeOffset_m`	`Real`	`= 0.0`
output	`predictedRange_m`	`Real`
output	`H`	`Real`	`[TangentLength]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function opticalFlowRangeJacobian
  "Nadir ray-to-plane range and Jacobian in the ESKF right-error tangent"
  input State predicted;
  input Real planeNormalWorldEnu[3] = {0.0, 0.0, 1.0};
  input Real planeOffset_m = 0.0;
  output Real predictedRange_m;
  output Real H[TangentLength];
protected
  Real rotationWorldBody[3, 3];
  Real boresightBody[3];
  Real denominator;
algorithm
  boresightBody := {0.0, 0.0, -1.0};
  rotationWorldBody := LieGroups.SO3.Quat.to_DCM(
    predicted.quaternionWorldBody);
  denominator := planeNormalWorldEnu
    * (rotationWorldBody * boresightBody);
  predictedRange_m := (planeOffset_m
    - planeNormalWorldEnu * predicted.positionWorldEnu_m) / denominator;
  H := zeros(TangentLength);
  H[1:3] := -(planeNormalWorldEnu * rotationWorldBody) / denominator;
  H[7:9] := predictedRange_m / denominator
    * planeNormalWorldEnu * rotationWorldBody
    * LieGroups.SO3.Quat.wedge(boresightBody);
end opticalFlowRangeJacobian;